sl-mechanical fd742f6890 feat: Configure Nav2 recovery behaviors (Issue #479 )

Implement conservative recovery behaviors for autonomous navigation on FC + Hoverboard ESC drivetrain.

Recovery Sequence (round-robin, 6 retries):
  1. Clear costmaps (local + global)
  2. Spin 90° @ 0.5 rad/s max (conservative for self-balancer)
  3. Wait 5 seconds (allow dynamic obstacles to move)
  4. Backup 0.3m @ 0.1 m/s (deadlock escape, very conservative)

Configuration:
  - backup: 0.3m reverse, 0.1 m/s speed, 5s timeout
  - spin: 90° rotation, 0.5 rad/s max angular velocity
  - wait: 5-second pause for obstacle clearing
  - progress_checker: 20cm minimum movement threshold in 10s window

Safety:
  - E-stop (Issue #459) takes priority over recovery behaviors
  - Emergency stop system runs independently on STM32 firmware
  - Conservative speeds for FC + Hoverboard ESC stability

Files Modified:
  - jetson/config/nav2_params.yaml: behavior_server parameters
  - jetson/ros2_ws/src/saltybot_bringup/behavior_trees/navigate_to_pose_with_recovery.xml: BT updates
  - jetson/config/RECOVERY_BEHAVIORS.md: Configuration documentation

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>

2026-03-05 14:40:45 -05:00

3.6 KiB

Raw Blame History

Nav2 Recovery Behaviors Configuration

Issue #479: Configure conservative recovery behaviors for SaltyBot autonomous navigation.

Overview

Recovery behaviors are triggered when Nav2 encounters navigation failures (path following failures, stuck detection, etc.). The recovery system attempts multiple strategies before giving up:

Clear costmaps — Reset perception obstacles
Spin recovery — In-place 90° rotation to detect surroundings
Wait recovery — 5-second pause for dynamic obstacles to move
Backup recovery — Reverse 0.3m to escape deadlocks

Configuration Details

Backup Recovery (Issue #479)

Distance: 0.3 meters reverse
Speed: 0.1 m/s (very conservative for FC + Hoverboard ESC)
Max velocity: 0.15 m/s (absolute limit)
Time limit: 5 seconds maximum

Spin Recovery

Angle: 1.57 radians (90°)
Max angular velocity: 0.5 rad/s (conservative for self-balancing robot)
Min angular velocity: 0.25 rad/s
Angular acceleration: 1.6 rad/s² (half of normal to ensure smooth motion)
Time limit: 10 seconds

Wait Recovery

Duration: 5 seconds
Purpose: Allow dynamic obstacles (people, other robots) to clear the path

Progress Checker (Issue #479)

Minimum movement threshold: 0.2 meters (20 cm)
Time window: 10 seconds
Behavior: If the robot doesn't move 20cm in 10 seconds, trigger recovery sequence

Safety: E-Stop Priority (Issue #459)

The emergency stop system (Issue #459, saltybot_emergency package) runs independently of Nav2 and takes absolute priority:

Trigger: Obstacles within 0.3m (configurable)
Action: Immediate zero velocity command, overrides all recovery behaviors
Integration: Publishes directly to /cmd_vel, pre-empting Nav2 output

Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the STM32 firmware.

Behavior Tree Sequence

Recovery runs in a round-robin fashion with up to 6 retry cycles:

NavigateRecovery (6 retries)
├── Normal navigation (compute path → follow path)
└── Recovery fallback:
    ├── Clear costmaps
    ├── Spin 90°
    ├── Wait 5s
    └── Back up 0.3m @ 0.1 m/s

Each cycle, the next recovery action is attempted (round-robin). If navigation succeeds, recovery stops. If all retries exhaust, goal fails.

Constraints for FC + Hoverboard ESC

This configuration is specifically tuned for:

Drivetrain: Flux Capacitor (FC) balancing controller + Hoverboard brushless ESC
Max linear velocity: 1.0 m/s
Max angular velocity: 1.5 rad/s
Recovery velocity constraints: 50% of normal for stability

The conservative recovery speeds ensure smooth deceleration profiles on the self-balancing drivetrain without tipping or oscillation.

Configuration Files

nav2_params.yaml: behavior_server section with spin, backup, wait parameters
navigate_to_pose_with_recovery.xml: Behavior tree defining recovery sequence
emergency.launch.py: E-stop system (independent, higher priority)

Testing

To test recovery behaviors:

# Run Nav2 with recovery enabled
ros2 launch saltybot_bringup nav2.launch.py

# Trigger recovery by creating obstacles in the path
# or setting unreachable goals

# Monitor recovery with:
ros2 topic echo /cmd_vel  # Check velocity commands during recovery
ros2 topic echo /diagnostics  # Monitor Nav2 status

References

Issue #479: Nav2 recovery behaviors
Issue #459: Emergency stop cascade (E-stop takes priority)
Nav2 recovery behavior documentation: https://docs.nav2.org/

3.6 KiB Raw Blame History