|
|
173b538f44
|
feat(controls): Priority-based cmd_vel multiplexer (Issue #228)
Implement priority-based velocity command routing with timeout-based fallback:
- Teleop (priority 1) > Nav2 (priority 2) > Docking (priority 3)
- 0.5s timeout per source: inactive sources are skipped in priority selection
- When no source is active, publish zero command for safety
Features:
- 50Hz multiplexing frequency with configurable parameters
- JSON status publishing (/saltybot/cmd_vel_mux_status) for telemetry
- Automatic priority escalation: teleop preempts nav2, nav2 preempts docking
- Fallback chain: if teleop times out, nav2 takes over; if nav2 times out, docking active
- Zero command safety: all sources timeout = immediate motor stop
Test Coverage:
- 20+ unit tests covering enum, initialization, subscriptions
- Priority selection logic (teleop preemption, nav2 preemption)
- Timeout detection and source fallback
- Realistic scenario tests (teleop release, priority escalation chains)
- JSON status format validation
Config: frequency=50Hz, source_timeout=0.5s
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
|
2026-03-02 12:01:00 -05:00 |
|
