feat(controls): Priority-based cmd_vel multiplexer (Issue #228) #230

sl-controls · 2026-03-02T12:01:19-05:00

sl-controls commented

2026-03-02 12:01:19 -05:00

Summary

Implement priority-based velocity command routing with timeout-based fallback.

Priority chain: Teleop (1) > Nav2 (2) > Docking (3)

0.5s timeout per source: inactive sources are skipped
Teleop preempts Nav2, Nav2 preempts Docking
Zero command safety when all sources timeout

Features

50Hz multiplexing with configurable parameters
JSON status publishing to /saltybot/cmd_vel_mux_status for telemetry
Automatic fallback chain: teleop → nav2 → docking
Zero command safety: immediate motor stop if all sources inactive
Comprehensive testing: 20+ unit tests covering priorities, timeouts, and scenarios

Test Coverage

✅ Enum and initialization tests
✅ All three subscription handlers (teleop, nav2, docking)
✅ Priority selection logic (teleop preemption, nav2 preemption)
✅ Timeout detection and source fallback
✅ Realistic scenario tests (teleop release, priority escalation chains, all-timeout safety)
✅ JSON status format validation

Configuration

frequency: 50 Hz (20ms cycle)
source_timeout: 0.5 seconds

Closes Issue #228.

## Summary Implement priority-based velocity command routing with timeout-based fallback. **Priority chain**: Teleop (1) > Nav2 (2) > Docking (3) - 0.5s timeout per source: inactive sources are skipped - Teleop preempts Nav2, Nav2 preempts Docking - Zero command safety when all sources timeout ## Features - **50Hz multiplexing** with configurable parameters - **JSON status publishing** to `/saltybot/cmd_vel_mux_status` for telemetry - **Automatic fallback chain**: teleop → nav2 → docking - **Zero command safety**: immediate motor stop if all sources inactive - **Comprehensive testing**: 20+ unit tests covering priorities, timeouts, and scenarios ## Test Coverage ✅ Enum and initialization tests ✅ All three subscription handlers (teleop, nav2, docking) ✅ Priority selection logic (teleop preemption, nav2 preemption) ✅ Timeout detection and source fallback ✅ Realistic scenario tests (teleop release, priority escalation chains, all-timeout safety) ✅ JSON status format validation ## Configuration - `frequency: 50` Hz (20ms cycle) - `source_timeout: 0.5` seconds Closes Issue #228.

sl-controls added 1 commit 2026-03-02 12:01:19 -05:00

feat(controls): Priority-based cmd_vel multiplexer (Issue #228 ) 173b538f44

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>

sl-jetson merged commit a566ab61cd into main

2026-03-02 12:08:49 -05:00

sl-jetson referenced this issue from a commit

2026-03-02 12:08:50 -05:00

Merge pull request 'feat(controls): Priority-based cmd_vel multiplexer (Issue #228)' (#230) from sl-controls/issue-228-cmd-vel-mux 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#230