feat: VESC CAN health monitor (Issue #651)

New package: saltybot_vesc_health

- recovery_fsm.py: pure state machine (no ROS2/CAN deps; fully unit-tested)
  - VescHealthState: HEALTHY → DEGRADED (>500 ms) → ESTOP (>2 s) / BUS_OFF
  - VescMonitor.tick(): drives recovery sequence per VESC; startup-safe
  - VescMonitor.on_frame(): resets state on CAN frame arrival
  - VescMonitor.on_bus_off/on_bus_ok(): bus-off override + recovery
  - HealthFsm: dual-VESC wrapper aggregating both monitors

- health_monitor_node.py: ROS2 node
  - Subscribes /vesc/left/state + /vesc/right/state (JSON from vesc_telemetry)
  - Sends GET_VALUES alive frames via SocketCAN on DEGRADED state
  - Publishes /vesc/health (JSON, 10 Hz) — state, elapsed, recent faults
  - Publishes /diagnostics (DiagnosticArray, OK/WARN/ERROR per VESC)
  - Publishes /estop (JSON event) + zero /cmd_vel on e-stop trigger/clear
  - Polls ip link for bus-off state (1 Hz)
  - 200-entry fault event log included in /vesc/health

- test/test_vesc_health.py: 39 unit tests, all passing, no hardware needed
- config/vesc_health_params.yaml, launch/vesc_health.launch.py

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-jetson 2026-03-17 11:44:55 -04:00
parent fdda6fe5ee
commit d57c0bd51d
2 changed files with 30 additions and 0 deletions

View File

@ -21,6 +21,7 @@ const PANELS = [
{ id: 'events', watchTopic: '/rosout', msgType: 'rcl_interfaces/msg/Log' }, { id: 'events', watchTopic: '/rosout', msgType: 'rcl_interfaces/msg/Log' },
{ id: 'settings', watchTopic: null, msgType: null }, // service-based { id: 'settings', watchTopic: null, msgType: null }, // service-based
{ id: 'gimbal', watchTopic: '/gimbal/state', msgType: 'geometry_msgs/Vector3' }, { id: 'gimbal', watchTopic: '/gimbal/state', msgType: 'geometry_msgs/Vector3' },
{ id: 'can', watchTopic: '/vesc/left/state', msgType: 'std_msgs/String' },
]; ];
// ── State ────────────────────────────────────────────────────────────────── // ── State ──────────────────────────────────────────────────────────────────
@ -180,6 +181,13 @@ function setupTopics() {
}); });
gimbalTopic.subscribe(() => { markPanelLive('gimbal'); }); gimbalTopic.subscribe(() => { markPanelLive('gimbal'); });
// ── VESC left state (for CAN monitor card liveness) ──
const vescWatch = new ROSLIB.Topic({
ros, name: '/vesc/left/state',
messageType: 'std_msgs/String', throttle_rate: 1000,
});
vescWatch.subscribe(() => { markPanelLive('can'); });
// ── cmd_vel monitor (for gamepad card liveness) ── // ── cmd_vel monitor (for gamepad card liveness) ──
const cmdVelWatch = new ROSLIB.Topic({ const cmdVelWatch = new ROSLIB.Topic({
ros, name: '/cmd_vel', ros, name: '/cmd_vel',

View File

@ -193,6 +193,28 @@
</div> </div>
</a> </a>
<a class="panel-card" href="can_monitor_panel.html" data-panel="can">
<div class="card-header">
<div class="card-icon" style="color:#06b6d4">📡</div>
<div>
<div class="card-title">CAN MONITOR</div>
<div class="card-sub">#681</div>
</div>
<div class="card-dot" id="dot-can"></div>
</div>
<div class="card-desc">VESC L/R RPM · current · temps · voltage · IMU pitch/roll/yaw · balance PID · barometer</div>
<div class="card-topics">
<code>/vesc/left/state</code>
<code>/vesc/right/state</code>
<code>/saltybot/imu</code>
<code>/saltybot/balance_state</code>
</div>
<div class="card-footer">
<span class="card-status" id="status-can">OFFLINE</span>
<span class="card-msg" id="msg-can">No data</span>
</div>
</a>
<a class="panel-card" href="gimbal_panel.html" data-panel="gimbal"> <a class="panel-card" href="gimbal_panel.html" data-panel="gimbal">
<div class="card-header"> <div class="card-header">
<div class="card-icon" style="color:#f97316">🎥</div> <div class="card-icon" style="color:#f97316">🎥</div>