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 <noreply@anthropic.com>
sl-controls
1c8430e68a