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

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,
+    ])

jetson/ros2_ws/src/saltybot_pure_pursuit/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_pure_pursuit</name>
+  <version>0.1.0</version>
+  <description>Pure pursuit path follower for Nav2 autonomous navigation</description>
+
+  <maintainer email="sl-controls@saltybot.local">SaltyBot Controls</maintainer>
+  <license>MIT</license>
+
+  <author email="sl-controls@saltybot.local">SaltyBot Controls Team</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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()

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

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',
+        ],
+    },
+)

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