ROS2 Implement Pure Pursuit Controller for Mobile Robot

Problem Statement

Implement a Pure Pursuit path-tracking controller as a ROS2 node.

Overview

Pure Pursuit works in two steps every control cycle:

1. Find the lookahead point — the point on the path that is closest to lookahead_distance ahead of the robot
2. Compute the steering command — the Twist that curves the robot toward that point

Requirements

Parameters (declare all with defaults):

• lookahead_distance (default 1.0 m) — how far ahead to aim
• max_linear_vel (default 0.5 m/s) — cruising speed
• min_lookahead (default 0.3 m) — clamp lookahead to this minimum

Topics:

• Subscribe to /path (std_msgs/String) — JSON array of waypoints: [[x1,y1], [x2,y2], ...]
• Subscribe to /robot_pose (std_msgs/String) — JSON: {"x": 1.0, "y": 2.0, "theta": 0.5}
• Publish to /cmd_vel (geometry_msgs/Twist)

Control loop (create_timer(0.1, ...)):

1. If no path or no pose, return
2. Find the lookahead point:
   • Find the waypoint closest to the robot (min distance to robot pose)
   • Walk forward from there until a waypoint is ≥ lookahead_distance from the robot
   • If none found, use the last waypoint
3. Compute steering:
   • Transform the lookahead point into the robot's local frame:

     dx = lp_x - robot_x
     dy = lp_y - robot_y
     # rotate into robot frame
     local_x =  dx * cos(theta) + dy * sin(theta)
     local_y = -dx * sin(theta) + dy * cos(theta)
     

   • Compute curvature: kappa = 2 * local_y / (local_x**2 + local_y**2)
   • Angular velocity: omega = max_linear_vel * kappa
4. Publish Twist(linear.x=max_linear_vel, angular.z=omega)
5. If robot is within 0.2m of the last waypoint, publish zero Twist and log "Goal reached"

Log "Pure Pursuit Ready" on startup.

🤖 Why This Matters in Real Robots

Nav2's RegulatedPurePursuit controller is used on every Clearpath, PAL, and Fetch robot. The lookahead distance parameter directly controls the trade-off between path accuracy (short lookahead = tight tracking) and smoothness (long lookahead = smoother but cuts corners). Understanding this from scratch is essential before tuning Nav2.