sl-mechanical
fd742f6890
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>
101 lines
3.6 KiB
Markdown
101 lines
3.6 KiB
Markdown
# 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:
|
|
|
|
1. **Clear costmaps** — Reset perception obstacles
|
|
2. **Spin recovery** — In-place 90° rotation to detect surroundings
|
|
3. **Wait recovery** — 5-second pause for dynamic obstacles to move
|
|
4. **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:
|
|
|
|
```bash
|
|
# 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/
|