sl-controls
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>