sl-controls created pull request seb/saltylab-firmware#230

sl-controls
Mon, 02 Mar 2026 12:01:19 -0500

230#feat(controls): Priority-based cmd_vel multiplexer (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.