feat: Add pure pursuit path follower for Nav2 (Issue #333) #334

seb · 2026-03-03T06:48:00-05:00

seb commented

2026-03-03 06:48:00 -05:00

Summary

Implements pure pursuit path follower algorithm for Nav2 autonomous navigation.

Algorithm

Pure pursuit computes steering by:

Finding closest point on path to robot
Looking ahead distance along path
Computing arc from robot to lookahead point
Publishing velocity commands to follow arc

Features

Subscribes to /odom and /path
Publishes on /cmd_vel_tracked
Publishes tracking error
Configurable lookahead distance (0.5m default)
Goal tolerance (0.1m)
Heading error correction
Velocity scaling

Configuration

lookahead_distance: 0.5m
goal_tolerance: 0.1m
max_linear_velocity: 1.0 m/s
max_angular_velocity: 1.57 rad/s
use_heading_correction: true

Testing

20+ comprehensive unit tests covering geometric calculations, path following on curves, steering commands, velocity limits, and edge cases.

🤖 Generated with Claude Code

## Summary Implements pure pursuit path follower algorithm for Nav2 autonomous navigation. ## Algorithm Pure pursuit computes steering by: 1. Finding closest point on path to robot 2. Looking ahead distance along path 3. Computing arc from robot to lookahead point 4. Publishing velocity commands to follow arc ## Features - Subscribes to /odom and /path - Publishes on /cmd_vel_tracked - Publishes tracking error - Configurable lookahead distance (0.5m default) - Goal tolerance (0.1m) - Heading error correction - Velocity scaling ## Configuration - lookahead_distance: 0.5m - goal_tolerance: 0.1m - max_linear_velocity: 1.0 m/s - max_angular_velocity: 1.57 rad/s - use_heading_correction: true ## Testing 20+ comprehensive unit tests covering geometric calculations, path following on curves, steering commands, velocity limits, and edge cases. 🤖 Generated with Claude Code

seb added 1 commit 2026-03-03 06:48:01 -05:00

feat: Add pure pursuit path follower for Nav2 (Issue #333 ) 1c8430e68a

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-jetson merged commit a0f3677732 into main

2026-03-03 13:19:30 -05:00

sl-jetson referenced this issue from a commit

2026-03-03 13:19:32 -05:00

Merge pull request 'feat: Add pure pursuit path follower for Nav2 (Issue #333)' (#334) from sl-controls/issue-333-pure-pursuit into main

Sign in to join this conversation.

1 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#334