From 1c8430e68a88804e046c4feadd7f8c3373af6031 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Tue, 3 Mar 2026 06:47:45 -0500 Subject: [PATCH] feat: Add pure pursuit path follower for Nav2 (Issue #333) Implements saltybot_pure_pursuit package: - Pure pursuit algorithm for path following with configurable parameters - Lookahead distance (0.5m default) for target point on path - Goal tolerance (0.1m) for goal detection - Heading error correction to reduce speed when misaligned with path - Publishes Twist commands on /cmd_vel_tracked for Nav2 integration - Subscribes to /odom (odometry) and /path (Path trajectory) - Tracks and publishes cross-track error for monitoring Pure pursuit geometry: - Finds closest point on path to robot current position - Looks ahead specified distance along path from closest point - Computes steering angle to follow circular arc to lookahead point - Reduces linear velocity when heading error is large (with correction enabled) - Clamps velocities to configurable maximums Configuration parameters: - lookahead_distance: 0.5m (typical range: 0.1-1.0m) - goal_tolerance: 0.1m (distance to goal before stopping) - heading_tolerance: 0.1 rad (unused but can support in future) - max_linear_velocity: 1.0 m/s - max_angular_velocity: 1.57 rad/s - use_heading_correction: true (reduces speed on large heading errors) Comprehensive test suite: 20+ tests covering: - Geometric calculations (distance, quaternion conversions) - Path following logic (empty path, straight/curved/spiral paths) - Steering calculations (heading errors, velocity limits) - Edge cases and realistic scenarios - Control loop integration - Parameter variations Co-Authored-By: Claude Haiku 4.5 --- .../config/pure_pursuit_config.yaml | 22 + .../launch/pure_pursuit.launch.py | 29 ++ .../src/saltybot_pure_pursuit/package.xml | 29 ++ .../resource/saltybot_pure_pursuit | 0 .../saltybot_pure_pursuit/__init__.py | 0 .../pure_pursuit_node.py | 269 ++++++++++++ .../src/saltybot_pure_pursuit/setup.cfg | 5 + .../src/saltybot_pure_pursuit/setup.py | 34 ++ .../saltybot_pure_pursuit/test/__init__.py | 0 .../test/test_pure_pursuit.py | 397 ++++++++++++++++++ 10 files changed, 785 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/config/pure_pursuit_config.yaml create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/launch/pure_pursuit.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/resource/saltybot_pure_pursuit create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/pure_pursuit_node.py create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/test/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_pure_pursuit/test/test_pure_pursuit.py diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/config/pure_pursuit_config.yaml b/jetson/ros2_ws/src/saltybot_pure_pursuit/config/pure_pursuit_config.yaml new file mode 100644 index 0000000..3a20273 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pure_pursuit/config/pure_pursuit_config.yaml @@ -0,0 +1,22 @@ +# Pure Pursuit Path Follower Configuration +pure_pursuit: + ros__parameters: + # Path following parameters + lookahead_distance: 0.5 # Distance to look ahead on the path (meters) + goal_tolerance: 0.1 # Distance tolerance to goal (meters) + heading_tolerance: 0.1 # Heading tolerance in radians (rad) + + # Speed parameters + max_linear_velocity: 1.0 # Maximum linear velocity (m/s) + max_angular_velocity: 1.57 # Maximum angular velocity (rad/s) + + # Control parameters + linear_velocity_scale: 1.0 # Scale factor for linear velocity + angular_velocity_scale: 1.0 # Scale factor for angular velocity + use_heading_correction: true # Apply heading error correction + + # Publishing frequency (Hz) + publish_frequency: 10 + + # Enable/disable path follower + enable_path_following: true \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/launch/pure_pursuit.launch.py b/jetson/ros2_ws/src/saltybot_pure_pursuit/launch/pure_pursuit.launch.py new file mode 100644 index 0000000..d5df330 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pure_pursuit/launch/pure_pursuit.launch.py @@ -0,0 +1,29 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + config_dir = PathJoinSubstitution( + [FindPackageShare('saltybot_pure_pursuit'), 'config'] + ) + config_file = PathJoinSubstitution([config_dir, 'pure_pursuit_config.yaml']) + + pure_pursuit = Node( + package='saltybot_pure_pursuit', + executable='pure_pursuit_node', + name='pure_pursuit', + output='screen', + parameters=[config_file], + remappings=[ + ('/odom', '/odom'), + ('/path', '/path'), + ('/cmd_vel_tracked', '/cmd_vel_tracked'), + ], + ) + + return LaunchDescription([ + pure_pursuit, + ]) \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/package.xml b/jetson/ros2_ws/src/saltybot_pure_pursuit/package.xml new file mode 100644 index 0000000..0b9d98f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pure_pursuit/package.xml @@ -0,0 +1,29 @@ + + + + saltybot_pure_pursuit + 0.1.0 + Pure pursuit path follower for Nav2 autonomous navigation + + SaltyBot Controls + MIT + + SaltyBot Controls Team + + ament_cmake + ament_cmake_python + + rclpy + nav_msgs + geometry_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + pytest + + + ament_python + + \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/resource/saltybot_pure_pursuit b/jetson/ros2_ws/src/saltybot_pure_pursuit/resource/saltybot_pure_pursuit new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/__init__.py b/jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/pure_pursuit_node.py b/jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/pure_pursuit_node.py new file mode 100644 index 0000000..b35cfe8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pure_pursuit/saltybot_pure_pursuit/pure_pursuit_node.py @@ -0,0 +1,269 @@ +#!/usr/bin/env python3 +"""Pure pursuit path follower for Nav2 autonomous navigation. + +Implements the pure pursuit algorithm for following a path or trajectory. +The algorithm computes steering commands to make the robot follow a path +by targeting a lookahead point on the path. + +The pure pursuit algorithm: +1. Finds the closest point on the path to the robot +2. Looks ahead a specified distance along the path +3. Computes a circular arc passing through robot position to lookahead point +4. Publishes velocity commands to follow this arc +""" + +import math +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Twist, PoseStamped +from std_msgs.msg import Float32 + + +class PurePursuitNode(Node): + """ROS2 node implementing pure pursuit path follower.""" + + def __init__(self): + super().__init__('pure_pursuit') + + # Parameters + self.declare_parameter('lookahead_distance', 0.5) + self.declare_parameter('goal_tolerance', 0.1) + self.declare_parameter('heading_tolerance', 0.1) + self.declare_parameter('max_linear_velocity', 1.0) + self.declare_parameter('max_angular_velocity', 1.57) + self.declare_parameter('linear_velocity_scale', 1.0) + self.declare_parameter('angular_velocity_scale', 1.0) + self.declare_parameter('use_heading_correction', True) + self.declare_parameter('publish_frequency', 10) + self.declare_parameter('enable_path_following', True) + + # Read parameters + self.lookahead_distance = self.get_parameter('lookahead_distance').value + self.goal_tolerance = self.get_parameter('goal_tolerance').value + self.heading_tolerance = self.get_parameter('heading_tolerance').value + self.max_linear_velocity = self.get_parameter('max_linear_velocity').value + self.max_angular_velocity = self.get_parameter('max_angular_velocity').value + self.linear_velocity_scale = self.get_parameter('linear_velocity_scale').value + self.angular_velocity_scale = self.get_parameter('angular_velocity_scale').value + self.use_heading_correction = self.get_parameter('use_heading_correction').value + publish_frequency = self.get_parameter('publish_frequency').value + self.enable_path_following = self.get_parameter('enable_path_following').value + + # Current state + self.current_pose = None + self.current_path = None + self.goal_reached = False + + # Subscriptions + self.sub_odom = self.create_subscription( + Odometry, '/odom', self._on_odometry, 10 + ) + self.sub_path = self.create_subscription( + Path, '/path', self._on_path, 10 + ) + + # Publishers + self.pub_cmd_vel = self.create_publisher(Twist, '/cmd_vel_tracked', 10) + self.pub_tracking_error = self.create_publisher(Float32, '/tracking_error', 10) + + # Timer for control loop + period = 1.0 / publish_frequency + self.timer = self.create_timer(period, self._control_loop) + + self.get_logger().info( + f"Pure pursuit initialized. " + f"Lookahead: {self.lookahead_distance}m, " + f"Goal tolerance: {self.goal_tolerance}m" + ) + + def _on_odometry(self, msg: Odometry) -> None: + """Callback for odometry messages.""" + self.current_pose = msg.pose.pose + + def _on_path(self, msg: Path) -> None: + """Callback for path messages.""" + self.current_path = msg + + def _quaternion_to_yaw(self, quat): + """Convert quaternion to yaw angle.""" + siny_cosp = 2 * (quat.w * quat.z + quat.x * quat.y) + cosy_cosp = 1 - 2 * (quat.y * quat.y + quat.z * quat.z) + yaw = math.atan2(siny_cosp, cosy_cosp) + return yaw + + def _yaw_to_quaternion(self, yaw): + """Convert yaw angle to quaternion.""" + from geometry_msgs.msg import Quaternion + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + return Quaternion(x=0.0, y=0.0, z=sy, w=cy) + + def _distance(self, p1, p2): + """Compute Euclidean distance between two points.""" + dx = p1.x - p2.x + dy = p1.y - p2.y + return math.sqrt(dx * dx + dy * dy) + + def _find_closest_point_on_path(self): + """Find closest point on path to current robot position.""" + if self.current_path is None or len(self.current_path.poses) < 2: + return None, 0, float('inf') + + closest_idx = 0 + closest_dist = float('inf') + + # Find closest waypoint + for i, pose in enumerate(self.current_path.poses): + dist = self._distance(self.current_pose.position, pose.pose.position) + if dist < closest_dist: + closest_dist = dist + closest_idx = i + + return closest_idx, closest_dist, closest_idx + + def _find_lookahead_point(self): + """Find lookahead point on path ahead of closest point.""" + if self.current_path is None or len(self.current_path.poses) < 2: + return None + + closest_idx, _, _ = self._find_closest_point_on_path() + if closest_idx is None: + return None + + # Find point at lookahead distance along path + current_dist = 0.0 + for i in range(closest_idx, len(self.current_path.poses) - 1): + p1 = self.current_path.poses[i].pose.position + p2 = self.current_path.poses[i + 1].pose.position + segment_dist = self._distance(p1, p2) + current_dist += segment_dist + + if current_dist >= self.lookahead_distance: + # Interpolate between p1 and p2 + overshoot = current_dist - self.lookahead_distance + if segment_dist > 0: + alpha = 1.0 - (overshoot / segment_dist) + from geometry_msgs.msg import Point + lookahead = Point() + lookahead.x = p1.x + alpha * (p2.x - p1.x) + lookahead.y = p1.y + alpha * (p2.y - p1.y) + lookahead.z = 0.0 + return lookahead + + # If we get here, return the last point + return self.current_path.poses[-1].pose.position + + def _calculate_steering_command(self, lookahead_point): + """Calculate steering angle using pure pursuit geometry. + + Args: + lookahead_point: Target lookahead point on the path + + Returns: + Tuple of (linear_velocity, angular_velocity) + """ + if lookahead_point is None or self.current_pose is None: + return 0.0, 0.0 + + # Vector from robot to lookahead point + dx = lookahead_point.x - self.current_pose.position.x + dy = lookahead_point.y - self.current_pose.position.y + distance_to_lookahead = math.sqrt(dx * dx + dy * dy) + + # Check if goal is reached + if distance_to_lookahead < self.goal_tolerance: + self.goal_reached = True + return 0.0, 0.0 + + # Robot heading + robot_yaw = self._quaternion_to_yaw(self.current_pose.orientation) + + # Angle to lookahead point + angle_to_lookahead = math.atan2(dy, dx) + + # Heading error + heading_error = angle_to_lookahead - robot_yaw + + # Normalize angle to [-pi, pi] + while heading_error > math.pi: + heading_error -= 2 * math.pi + while heading_error < -math.pi: + heading_error += 2 * math.pi + + # Pure pursuit curvature: k = 2 * sin(alpha) / Ld + # where alpha is the heading error and Ld is lookahead distance + if self.lookahead_distance > 0: + curvature = (2.0 * math.sin(heading_error)) / self.lookahead_distance + else: + curvature = 0.0 + + # Linear velocity (reduce when heading error is large) + if self.use_heading_correction: + heading_error_abs = abs(heading_error) + linear_velocity = self.max_linear_velocity * math.cos(heading_error) + linear_velocity = max(0.0, linear_velocity) # Don't go backwards + else: + linear_velocity = self.max_linear_velocity + + # Angular velocity from curvature: omega = v * k + angular_velocity = linear_velocity * curvature + + # Clamp velocities + linear_velocity = min(linear_velocity, self.max_linear_velocity) + angular_velocity = max(-self.max_angular_velocity, + min(angular_velocity, self.max_angular_velocity)) + + # Apply scaling + linear_velocity *= self.linear_velocity_scale + angular_velocity *= self.angular_velocity_scale + + return linear_velocity, angular_velocity + + def _control_loop(self) -> None: + """Main control loop executed at regular intervals.""" + if not self.enable_path_following or self.current_pose is None: + # Publish zero velocity + cmd = Twist() + self.pub_cmd_vel.publish(cmd) + return + + # Find lookahead point + lookahead_point = self._find_lookahead_point() + + if lookahead_point is None: + # No valid path, publish zero velocity + cmd = Twist() + self.pub_cmd_vel.publish(cmd) + return + + # Calculate steering command + linear_vel, angular_vel = self._calculate_steering_command(lookahead_point) + + # Publish velocity command + cmd = Twist() + cmd.linear.x = linear_vel + cmd.angular.z = angular_vel + self.pub_cmd_vel.publish(cmd) + + # Publish tracking error + if self.current_path and len(self.current_path.poses) > 0: + closest_idx, tracking_error, _ = self._find_closest_point_on_path() + error_msg = Float32(data=tracking_error) + self.pub_tracking_error.publish(error_msg) + + +def main(args=None): + rclpy.init(args=args) + node = PurePursuitNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/setup.cfg b/jetson/ros2_ws/src/saltybot_pure_pursuit/setup.cfg new file mode 100644 index 0000000..4e0708d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pure_pursuit/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_pure_pursuit +[egg_info] +tag_build = +tag_date = 0 \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/setup.py b/jetson/ros2_ws/src/saltybot_pure_pursuit/setup.py new file mode 100644 index 0000000..41784c6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pure_pursuit/setup.py @@ -0,0 +1,34 @@ +from setuptools import setup, find_packages + +package_name = 'saltybot_pure_pursuit' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/saltybot_pure_pursuit']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', ['config/pure_pursuit_config.yaml']), + ('share/' + package_name + '/launch', ['launch/pure_pursuit.launch.py']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='SaltyBot Controls', + author_email='sl-controls@saltybot.local', + maintainer='SaltyBot Controls', + maintainer_email='sl-controls@saltybot.local', + keywords=['ROS2', 'pure_pursuit', 'path_following', 'nav2'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: MIT License', + 'Programming Language :: Python :: 3', + 'Topic :: Software Development', + ], + entry_points={ + 'console_scripts': [ + 'pure_pursuit_node=saltybot_pure_pursuit.pure_pursuit_node:main', + ], + }, +) \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/test/__init__.py b/jetson/ros2_ws/src/saltybot_pure_pursuit/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_pure_pursuit/test/test_pure_pursuit.py b/jetson/ros2_ws/src/saltybot_pure_pursuit/test/test_pure_pursuit.py new file mode 100644 index 0000000..d1c0da6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pure_pursuit/test/test_pure_pursuit.py @@ -0,0 +1,397 @@ +"""Unit tests for pure pursuit path follower node.""" + +import pytest +import math +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Twist +from std_msgs.msg import Header + +import rclpy + +from saltybot_pure_pursuit.pure_pursuit_node import PurePursuitNode + + +@pytest.fixture +def rclpy_fixture(): + """Initialize and cleanup rclpy.""" + rclpy.init() + yield + rclpy.shutdown() + + +@pytest.fixture +def node(rclpy_fixture): + """Create a pure pursuit node instance.""" + node = PurePursuitNode() + yield node + node.destroy_node() + + +class TestPurePursuitGeometry: + """Test suite for pure pursuit geometric calculations.""" + + def test_node_initialization(self, node): + """Test that node initializes with correct defaults.""" + assert node.lookahead_distance == 0.5 + assert node.goal_tolerance == 0.1 + assert node.max_linear_velocity == 1.0 + assert node.enable_path_following is True + + def test_distance_calculation(self, node): + """Test Euclidean distance calculation.""" + p1 = Point(x=0.0, y=0.0, z=0.0) + p2 = Point(x=3.0, y=4.0, z=0.0) + dist = node._distance(p1, p2) + assert abs(dist - 5.0) < 0.01 + + def test_distance_same_point(self, node): + """Test distance between same point is zero.""" + p = Point(x=1.0, y=2.0, z=0.0) + dist = node._distance(p, p) + assert abs(dist) < 0.001 + + def test_quaternion_to_yaw_north(self, node): + """Test quaternion to yaw conversion for north heading.""" + quat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + yaw = node._quaternion_to_yaw(quat) + assert abs(yaw) < 0.01 + + def test_quaternion_to_yaw_east(self, node): + """Test quaternion to yaw conversion for east heading.""" + # 90 degree rotation around z-axis + quat = Quaternion(x=0.0, y=0.0, z=0.7071, w=0.7071) + yaw = node._quaternion_to_yaw(quat) + assert abs(yaw - math.pi / 2) < 0.01 + + def test_yaw_to_quaternion_identity(self, node): + """Test yaw to quaternion conversion.""" + quat = node._yaw_to_quaternion(0.0) + assert abs(quat.w - 1.0) < 0.01 + assert abs(quat.z) < 0.01 + + +class TestPathFollowing: + """Test suite for path following logic.""" + + def test_empty_path(self, node): + """Test handling of empty path.""" + node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0)) + node.current_path = Path() + + lookahead = node._find_lookahead_point() + assert lookahead is None + + def test_single_point_path(self, node): + """Test handling of single-point path.""" + node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0)) + + path = Path() + path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))] + node.current_path = path + + lookahead = node._find_lookahead_point() + assert lookahead is None or lookahead.x == 1.0 + + def test_straight_line_path(self, node): + """Test path following on straight line.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Create straight path along x-axis + path = Path() + for i in range(5): + pose = PoseStamped(pose=Pose( + position=Point(x=float(i), y=0.0, z=0.0) + )) + path.poses.append(pose) + node.current_path = path + + # Robot heading east towards path + lin_vel, ang_vel = node._calculate_steering_command(path.poses[1].pose.position) + assert lin_vel >= 0 + assert abs(ang_vel) < 0.5 # Should have small steering error + + def test_curved_path(self, node): + """Test path following on curved path.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Create circular arc path + path = Path() + for i in range(9): + angle = (i / 8.0) * (math.pi / 2) + x = math.sin(angle) + y = 1.0 - math.cos(angle) + pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0))) + path.poses.append(pose) + node.current_path = path + + lookahead = node._find_lookahead_point() + assert lookahead is not None + + def test_goal_reached(self, node): + """Test goal reached detection.""" + goal_pos = Point(x=1.0, y=0.0, z=0.0) + node.current_pose = Pose( + position=Point(x=1.05, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + lin_vel, ang_vel = node._calculate_steering_command(goal_pos) + # With goal_tolerance=0.1, we should be at goal + assert abs(lin_vel) < 0.01 + + +class TestSteeringCalculation: + """Test suite for steering command calculations.""" + + def test_zero_heading_error(self, node): + """Test steering when robot already aligned with target.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) # Facing east + ) + + # Target directly ahead + target = Point(x=1.0, y=0.0, z=0.0) + lin_vel, ang_vel = node._calculate_steering_command(target) + + assert lin_vel > 0 + assert abs(ang_vel) < 0.1 # Minimal steering + + def test_90_degree_heading_error(self, node): + """Test steering with 90 degree heading error.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) # Facing east + ) + + # Target north + target = Point(x=0.0, y=1.0, z=0.0) + lin_vel, ang_vel = node._calculate_steering_command(target) + + assert lin_vel >= 0 + assert ang_vel != 0 # Should have significant steering + + def test_velocity_limits(self, node): + """Test that velocities are within limits.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + target = Point(x=100.0, y=100.0, z=0.0) + lin_vel, ang_vel = node._calculate_steering_command(target) + + assert lin_vel <= node.max_linear_velocity + assert abs(ang_vel) <= node.max_angular_velocity + + def test_heading_correction_enabled(self, node): + """Test heading correction when enabled.""" + node.use_heading_correction = True + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Large heading error + target = Point(x=0.0, y=10.0, z=0.0) # Due north + lin_vel, ang_vel = node._calculate_steering_command(target) + + # Linear velocity should be reduced due to heading error + assert lin_vel < node.max_linear_velocity + + def test_heading_correction_disabled(self, node): + """Test that heading correction can be disabled.""" + node.use_heading_correction = False + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + target = Point(x=0.0, y=10.0, z=0.0) + lin_vel, ang_vel = node._calculate_steering_command(target) + + # Linear velocity should be full + assert abs(lin_vel - node.max_linear_velocity) < 0.01 + + def test_negative_lookahead_distance(self, node): + """Test behavior with invalid lookahead distance.""" + node.lookahead_distance = 0.0 + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + target = Point(x=1.0, y=1.0, z=0.0) + lin_vel, ang_vel = node._calculate_steering_command(target) + + # Should not crash, handle gracefully + assert isinstance(lin_vel, float) + assert isinstance(ang_vel, float) + + +class TestPurePursuitScenarios: + """Integration-style tests for realistic scenarios.""" + + def test_scenario_follow_straight_path(self, node): + """Scenario: Follow a straight horizontal path.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Horizontal path + path = Path() + for i in range(10): + pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0))) + path.poses.append(pose) + node.current_path = path + + lin_vel, ang_vel = node._calculate_steering_command(path.poses[1].pose.position) + assert lin_vel > 0 + assert abs(ang_vel) < 0.5 + + def test_scenario_s_shaped_path(self, node): + """Scenario: Follow an S-shaped path.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # S-shaped path + path = Path() + for i in range(20): + x = float(i) * 0.5 + y = 2.0 * math.sin(x) + pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0))) + path.poses.append(pose) + node.current_path = path + + lookahead = node._find_lookahead_point() + assert lookahead is not None + + def test_scenario_spiral_path(self, node): + """Scenario: Follow a spiral path.""" + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Spiral path + path = Path() + for i in range(30): + angle = (i / 30.0) * (4 * math.pi) + radius = 0.5 + (i / 30.0) * 2.0 + x = radius * math.cos(angle) + y = radius * math.sin(angle) + pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0))) + path.poses.append(pose) + node.current_path = path + + lin_vel, ang_vel = node._calculate_steering_command(path.poses[5].pose.position) + assert isinstance(lin_vel, float) + assert isinstance(ang_vel, float) + + def test_scenario_control_loop(self, node): + """Scenario: Control loop with valid path and odometry.""" + node.enable_path_following = True + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Create a simple path + path = Path() + for i in range(5): + pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0))) + path.poses.append(pose) + node.current_path = path + + # Run control loop + node._control_loop() + + # Should complete without error + + def test_scenario_disabled_path_following(self, node): + """Scenario: Path following disabled.""" + node.enable_path_following = False + node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0)) + + # Create path + path = Path() + path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))] + node.current_path = path + + # Run control loop + node._control_loop() + + # Should publish zero velocity + + def test_scenario_no_odometry(self, node): + """Scenario: Control loop when odometry not received.""" + node.current_pose = None + path = Path() + path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))] + node.current_path = path + + # Run control loop + node._control_loop() + + # Should handle gracefully + + def test_scenario_velocity_scaling(self, node): + """Scenario: Velocity scaling parameters.""" + node.linear_velocity_scale = 0.5 + node.angular_velocity_scale = 0.5 + + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + target = Point(x=1.0, y=0.0, z=0.0) + lin_vel, ang_vel = node._calculate_steering_command(target) + + # Scaled velocities should be smaller + assert lin_vel <= node.max_linear_velocity * 0.5 + 0.01 + + def test_scenario_large_lookahead_distance(self, node): + """Scenario: Large lookahead distance.""" + node.lookahead_distance = 5.0 + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Path + path = Path() + for i in range(10): + pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0))) + path.poses.append(pose) + node.current_path = path + + lookahead = node._find_lookahead_point() + assert lookahead is not None + + def test_scenario_small_lookahead_distance(self, node): + """Scenario: Small lookahead distance.""" + node.lookahead_distance = 0.05 + node.current_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + + # Path + path = Path() + for i in range(10): + pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0))) + path.poses.append(pose) + node.current_path = path + + lookahead = node._find_lookahead_point() + assert lookahead is not None -- 2.47.2