Update trial_controller_backup.py

colcon_ws/src/my_controller/my_controller/trial_controller_backup.py

@@ -14,22 +14,6 @@ class MoveRobotNode(Node):
 
     
     def __init__(self):
-        
-        super().__init__('move_robot_node')
-
-
-        """
-   
-        
-        self.declare_parameter('x', 1.0)
-        self.declare_parameter('y', 1.0)
-        self.declare_parameter('theta', 0.0)
-        self.declare_parameter('velocity', 1.0)
-        """
-
-
-
-
         super().__init__('move_robot_node')
         #self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 10)  #for differential robot
         #self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)                      #normal msg topic
@@ -39,32 +23,20 @@ class MoveRobotNode(Node):
         #self.subscription = self.create_subscription(Odometry, '/diff_cont/odom', self.odom_callback, 10)    #differential
         #self.subscription = self.create_subscription(Odometry, '/robot/robotnik_base_control/odom', self.odom_callback, 10)   #for kairos simulation
         self.subscription = self.create_subscription(Odometry, '/kairosAB/robotnik_base_control/odom', self.odom_callback, 10)   #for kairos AB real
+        self.current_pose = self.get_start_pose()
 
-        self.current_pose =  [0.02, 0.02, 0.02]
-        
-    """
-        
-    def get_start_pose(self,msg):
 
-        start_x =0.0
-        start_y =0.0
         
+    def get_start_pose(self):
   
-        start_theta = 0.0
-        
-        return [start_x, start_y, start_theta]
-        
-        start_pose_str = self.get_parameter('start_pose').get_parameter_value().string_value
-        start_x, start_y, start_theta = map(float, start_pose_str.split())
-        self.start_theta = start_theta
+        start_x = float(input("Enter start x: "))
+        start_y = float(input("Enter start y: "))
+        self.start_theta = float( input("Enter start theta from -π to π :"))
         return [start_x, start_y, self.start_theta]
 
-        
-    """   
     #function to update the current pose based on the odometry from gazebo 
     def odom_callback(self, msg):
 
-
         self.current_pose[0] = msg.pose.pose.position.x
         self.current_pose[1] = msg.pose.pose.position.y
         orientation_q = msg.pose.pose.orientation
@@ -76,10 +48,6 @@ class MoveRobotNode(Node):
     #function for moving the robot 
     def move_robot(self, goal_pose, velocity):
 
-
-
-
-
         error_x = goal_pose[0] - self.current_pose[0]
         error_y = goal_pose[1] - self.current_pose[1]
     
@@ -87,7 +55,7 @@ class MoveRobotNode(Node):
         # Stage 1: Move the robot forward towards the goal
         distance = (error_x**2 + error_y**2)**0.5
 
-        while distance >= 0.01:
+        while distance >= 0.05:
             error_x = goal_pose[0] - self.current_pose[0]
             error_y = goal_pose[1] - self.current_pose[1]
             distance = (error_x**2 + error_y**2)**0.5
@@ -97,8 +65,8 @@ class MoveRobotNode(Node):
             # direction towards the goal
             direction_to_goal = [goal_pose[0] - self.current_pose[0], goal_pose[1] - self.current_pose[1]]
 
-            # instea of start_theta is now current_pose[2]
-            current_direction = [math.cos(self.current_pose[2]), math.sin(self.current_pose[2])]
+            # current direction of the robot based on start_theta pose 
+            current_direction = [math.cos(self.start_theta), math.sin(self.start_theta)]
 
             # angle between the current direction and the direction to the goal
             angle_to_goal = math.atan2(direction_to_goal[1], direction_to_goal[0]) - math.atan2(current_direction[1], current_direction[0])
@@ -112,7 +80,7 @@ class MoveRobotNode(Node):
             msg = Twist()
             msg.linear.x = linear_velocity_x
             msg.linear.y= linear_velocity_y
-            if distance < 0.02:  # threshold if the robot is close enough to the target orientation
+            if distance < 0.05:  # threshold if the robot is close enough to the target orientation
      
                 print("first stage reached END") # stop rotating
 
@@ -164,24 +132,11 @@ def main(args=None):
     
     move_robot_node = MoveRobotNode()
 
-
-    
     goal_pose = [float(input("Enter goal x: ")), float(input("Enter goal y: ")), float(input("Enter goal theta from -π to π : "))]
     velocity = float(input("Enter velocity: "))   
-    """
-    x = move_robot_node.get_parameter('x').get_parameter_value().double_value
-    y = move_robot_node.get_parameter('y').get_parameter_value().double_value
-    theta = move_robot_node.get_parameter('theta').get_parameter_value().double_value
-    velocity = move_robot_node.get_parameter('velocity').get_parameter_value().double_value
-    goal_pose=[float(x),float(y),float(theta)]
-    """
-
-
-    
-     
-    
     move_robot_node.move_robot(goal_pose, velocity)
 
+
     rclpy.spin(move_robot_node)
 
     move_robot_node.destroy_node()
@@ -189,3 +144,4 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
+