waypoint_navigator

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import math
import time

class EBotNavigator(Node):
    """
    A ROS2 node to navigate the eBot through a series of predefined waypoints
    using a proportional controller (P-controller).
    """

    def __init__(self):
        super().__init__('ebot_navigator_node')

        # Define the sequence of goal poses [x, y, yaw]
        # P1: [-1.53, -1.95, 1.57] - Start of first aisle
        # P1.5: [-1.0, 0.0, 0.0] - NEW INTERMEDIATE POINT for better centering in Zone 1/2 aisle
        # P2: [0.13, 1.24, 0.00] - End of first aisle / Start of second aisle
        # P3: [0.38, -3.32, -1.57] - Final point
        self.goals = [
            [-1.53, -1.95, 1.57],
            [-1.00, 0.00, 0.00], # Intermediate point to clear the center of the field
            [0.13, 1.24, 0.00],
            [0.38, -3.32, -1.57]
        ]
        self.current_goal_index = 0
        self.all_goals_reached = False

        # --- UPDATED TOLERANCES (as per task requirements) ---
        self.distance_tolerance = 0.30  # meters (from +-0.3m requirement)
        self.angle_tolerance = math.radians(10) # radians (from +-10 degrees requirement)

        # --- ADJUSTED Proportional control gains ---
        self.linear_gain = 0.4 # Slightly reduced for more control
        self.angular_gain = 2.5 # Slightly increased for faster turning
        self.max_linear_speed = 0.3 # m/s
        self.max_angular_speed = 1.0 # rad/s

        # Robot's current state variables
        self.current_x = 0.0
        self.current_y = 0.0
        self.current_yaw = 0.0

        # Create a publisher and subscriber
        self.velocity_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
        self.odom_subscriber = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10
        )
        
        # Create a timer to run the control loop at 10 Hz
        self.control_loop_timer = self.create_timer(0.1, self.control_loop)

        self.get_logger().info("eBot Navigator node has been started with updated parameters and waypoints.")
        # Removed time.sleep(1) as it can block the ROS2 initialization

    def odom_callback(self, msg):
        """
        Callback function for the /odom subscriber.
        Updates the robot's current position and orientation.
        """
        self.current_x = msg.pose.pose.position.x
        self.current_y = msg.pose.pose.position.y
        
        # Convert quaternion orientation to Euler angle (yaw)
        orientation_q = msg.pose.pose.orientation
        self.current_yaw = self.euler_from_quaternion(orientation_q)

    def euler_from_quaternion(self, q):
        """
        Convert a quaternion into a yaw angle (rotation around the z-axis).
        """
        t3 = +2.0 * (q.w * q.z + q.x * q.y)
        t4 = +1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        yaw_z = math.atan2(t3, t4)
        return yaw_z

    def control_loop(self):
        """
        Main control loop for navigating to waypoints.
        Implements a proportional controller for position and orientation.
        """
        if self.all_goals_reached:
            self.stop_robot()
            return

        # Get the current goal
        target_x, target_y, target_yaw = self.goals[self.current_goal_index]

        # Calculate the distance and angle to the target position
        distance_to_target = math.sqrt((target_x - self.current_x)**2 + (target_y - self.current_y)**2)
        angle_to_target = math.atan2(target_y - self.current_y, target_x - self.current_x)

        # Calculate the error in angle (heading) for the position
        angle_error = angle_to_target - self.current_yaw
        angle_error = math.atan2(math.sin(angle_error), math.cos(angle_error))

        vel_msg = Twist()

        # --- Navigation Logic ---
        
        # 1. Approach the target position (Move and steer)
        if distance_to_target > self.distance_tolerance:
            
            # Linear speed is proportional to the distance
            vel_msg.linear.x = self.linear_gain * distance_to_target
            
            # Angular speed is proportional to the heading error
            vel_msg.angular.z = self.angular_gain * angle_error
            
        # 2. Reached the target position, now adjust final orientation (Pure rotation)
        else:
            # Calculate the final orientation error
            final_angle_error = target_yaw - self.current_yaw
            final_angle_error = math.atan2(math.sin(final_angle_error), math.cos(final_angle_error))

            # If the final orientation is not correct, turn.
            if abs(final_angle_error) > self.angle_tolerance:
                vel_msg.linear.x = 0.0
                vel_msg.angular.z = self.angular_gain * final_angle_error
                self.get_logger().info(f"Position for Goal {self.current_goal_index + 1} reached. Adjusting final orientation. Error: {final_angle_error:.3f} rad")
            # If the final orientation is correct, we have reached the goal pose.
            else:
                self.get_logger().info(f"--- Goal {self.current_goal_index + 1} Reached! ---")
                self.stop_robot() # Stop briefly at the goal
                
                # Move to the next goal
                self.current_goal_index += 1
                if self.current_goal_index >= len(self.goals):
                    self.all_goals_reached = True
                    self.get_logger().info("--- All goals have been reached! Mission complete. ---")
                else:
                    self.get_logger().info(f"Proceeding to Goal {self.current_goal_index + 1}...")
                    time.sleep(1.0) # Pause before starting the next goal

        # --- Velocity Limiting ---
        vel_msg.linear.x = max(-self.max_linear_speed, min(self.max_linear_speed, vel_msg.linear.x))
        vel_msg.angular.z = max(-self.max_angular_speed, min(self.max_angular_speed, vel_msg.angular.z))

        # Publish the velocity command
        self.velocity_publisher.publish(vel_msg)

    def stop_robot(self):
        """
        Publishes a zero-velocity Twist message to stop the robot.
        """
        stop_msg = Twist()
        stop_msg.linear.x = 0.0
        stop_msg.angular.z = 0.0
        self.velocity_publisher.publish(stop_msg)
        # self.get_logger().info("Robot stopped.") # Log removed for less console spam


def main(args=None):
    rclpy.init(args=args)
    ebot_navigator = EBotNavigator()
    try:
        rclpy.spin(ebot_navigator)
    except KeyboardInterrupt:
        ebot_navigator.get_logger().info("Shutting down node...")
    finally:
        # Ensure the robot stops when the node is shut down
        ebot_navigator.stop_robot()
        ebot_navigator.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()