seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: ROS2 sensor health monitor (Issue #566) #571

Closed
sl-jetson wants to merge 1 commits from sl-uwb/issue-545-tag-firmware into main
pull from: sl-uwb/issue-545-tag-firmware
merge into: seb:main
Conversation 0 Commits 1 Files Changed 2 +390
sl-jetson commented 2026-03-14 11:48:19 -04:00
Collaborator
Copy Link

Summary

Adds sensor_health_node to the existing saltybot_health_monitor package (Issue #566).

  • Monitors 8 sensor topics for staleness: camera color/depth/info, lidar, IMU, UWB, battery, motor daemon
  • Per-sensor OK/WARN/ERROR thresholds (configurable in sensor_health_params.yaml)
  • Publishes DiagnosticArray on /saltybot/diagnostics at 1 Hz
  • Publishes JSON summary on /saltybot/sensor_health
  • MQTT to saltybot/health (paho-mqtt, optional — graceful degradation if not installed)
  • Thread-safe SensorWatcher with message rate estimation
  • Critical sensor flag: camera, lidar, IMU, motor_daemon escalate overall status to ERROR
  • Auto-reconnect: MQTT reconnects in background
  • 35 unit tests, all passing offline (no hardware or ROS2 runtime required)

Test plan

  • colcon build --packages-select saltybot_health_monitor succeeds
  • ros2 launch saltybot_health_monitor sensor_health.launch.py starts node
  • /saltybot/diagnostics publishes DiagnosticArray at 1 Hz
  • /saltybot/sensor_health publishes JSON with per-sensor status
  • Stop camera driver — camera_color and camera_depth → ERROR within 3s
  • Restart camera driver — returns to OK within 1s of first frame
  • MQTT: mosquitto_sub -t saltybot/health receives updates at 1 Hz
  • mqtt_enabled:=false — node starts without MQTT, no errors
  • pytest test/test_sensor_health.py → 35/35 passed

Closes #566

## Summary Adds **sensor_health_node** to the existing **saltybot_health_monitor** package (Issue #566). - Monitors 8 sensor topics for staleness: camera color/depth/info, lidar, IMU, UWB, battery, motor daemon - Per-sensor OK/WARN/ERROR thresholds (configurable in sensor_health_params.yaml) - Publishes **DiagnosticArray** on **/saltybot/diagnostics** at 1 Hz - Publishes JSON summary on **/saltybot/sensor_health** - MQTT to **saltybot/health** (paho-mqtt, optional — graceful degradation if not installed) - Thread-safe SensorWatcher with message rate estimation - Critical sensor flag: camera, lidar, IMU, motor_daemon escalate overall status to ERROR - Auto-reconnect: MQTT reconnects in background - 35 unit tests, all passing offline (no hardware or ROS2 runtime required) ## Test plan - [ ] `colcon build --packages-select saltybot_health_monitor` succeeds - [ ] `ros2 launch saltybot_health_monitor sensor_health.launch.py` starts node - [ ] `/saltybot/diagnostics` publishes `DiagnosticArray` at 1 Hz - [ ] `/saltybot/sensor_health` publishes JSON with per-sensor status - [ ] Stop camera driver — `camera_color` and `camera_depth` → ERROR within 3s - [ ] Restart camera driver — returns to OK within 1s of first frame - [ ] MQTT: `mosquitto_sub -t saltybot/health` receives updates at 1 Hz - [ ] `mqtt_enabled:=false` — node starts without MQTT, no errors - [ ] `pytest test/test_sensor_health.py` → 35/35 passed Closes #566
sl-jetson added 1 commit 2026-03-14 11:48:20 -04:00 
Tag firmware for Makerfabs ESP32 UWB Pro worn by person being tracked.
Initiates DS-TWR with each robot anchor in 20 Hz round-robin.

- DS-TWR initiator: Poll→(RESP received)→Final with timestamps Ra/Da/Db
- Anchor side computes authoritative range; tag computes local estimate
- Round-robin across NUM_ANCHORS anchors (default 2) at 20 Hz
- Streams +RANGE:<id>,<mm>,<rssi> over USB serial (bench debug)
- LED blink on each successful range
- TAG_ID, NUM_ANCHORS, RANGE_INTERVAL_MS configurable in platformio.ini
- Pin map: SCK=18 MISO=19 MOSI=23 CS=21 RST=27 IRQ=34

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson closed this pull request 2026-03-14 11:48:49 -04:00

Pull request closed

Please reopen this pull request to perform a merge.
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#571
No description provided.
Delete Branch "sl-uwb/issue-545-tag-firmware"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?