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

feat: MQTT-to-ROS2 phone sensor bridge (Issue #601) #605

Merged
sl-jetson merged 2 commits from sl-android/issue-601-mqtt-ros2-bridge into main 2026-03-14 15:55:23 -04:00
Conversation 0 Commits 2 Files Changed 4 +556
sl-jetson commented 2026-03-14 15:00:13 -04:00
Collaborator
Copy Link

Summary

Jetson-side ROS2 node that bridges the three MQTT topics from phone/sensor_dashboard.py into typed ROS2 messages.

Files changed

File Change
saltybot_phone/mqtt_ros2_bridge_node.py New ROS2 node
launch/mqtt_bridge.launch.py New launch file
setup.py Add mqtt_ros2_bridge console script
package.xml Add python3-paho-mqtt exec_depend

Topic mapping

MQTT topic ROS2 topic Message type
saltybot/phone/imu /saltybot/phone/imu sensor_msgs/Imu
saltybot/phone/gps /saltybot/phone/gps sensor_msgs/NavSatFix
saltybot/phone/battery /saltybot/phone/battery sensor_msgs/BatteryState
(internal) /saltybot/phone/bridge/status std_msgs/String (JSON)

Architecture

  • paho-mqtt loop_start() runs MQTT network in its own thread; on_message callback only enqueues (topic, payload) — no rclpy calls from MQTT thread
  • ROS2 50 Hz timer drains queue and publishes — all ROS2 operations on executor thread
  • Auto-reconnect: paho reconnect_delay_set(5s → 20s)

Message details

IMU — accel + gyro with configurable diagonal covariance; orientation_covariance[0] = -1 (unknown, REP-145)

NavSatFix — full 3×3 position covariance from accuracy_m; STATUS_FIX for gps/fused/network providers; COVARIANCE_TYPE_APPROXIMATED

BatteryState — percentage [0–1]; temperature in Kelvin; Android health string → POWER_SUPPLY_HEALTH_*; voltage/current = NaN (unavailable on Android)

Timestamp alignment

Default: ROS2 wall clock (consistent with all other nodes). use_phone_timestamp:=true uses the phone's Unix epoch ts field when clock drift < warn_drift_s (default 1.0 s); warns and falls back to ROS2 clock if drift exceeds threshold.

Input validation

  • IMU: finite value check on all 6 DOF values
  • GPS: latitude ∈ [−90, 90], longitude ∈ [−180, 180]
  • Battery: percentage ∈ [0, 100]
  • Malformed JSON and validation failures → DEBUG log + error counter

Status topic (0.2 Hz)

{"mqtt_connected":true,"rx":{"saltybot/phone/imu":150,...},"pub":{...},"err":{...},"age_s":{"imu":0.2,"gps":1.0,"battery":0.9},"queue_depth":0}

Test plan

  • python3 -m py_compile saltybot_phone/mqtt_ros2_bridge_node.py — clean
  • On phone: python3 phone/sensor_dashboard.py --broker <jetson_ip>
  • On Jetson: ros2 launch saltybot_phone mqtt_bridge.launch.py mqtt_host:=localhost
  • ros2 topic hz /saltybot/phone/imu → ~5 Hz
  • ros2 topic echo /saltybot/phone/gps → valid lat/lon/covariance
  • ros2 topic echo /saltybot/phone/battery → percentage ∈ [0,1], temp in K
  • ros2 topic echo /saltybot/phone/bridge/status → JSON with mqtt_connected:true
  • Kill MQTT broker → mqtt_connected:false in status; restart → auto-reconnect
  • use_phone_timestamp:=true with > 1s clock skew → warning logged, ROS2 clock used

🤖 Generated with Claude Code

## Summary Jetson-side ROS2 node that bridges the three MQTT topics from `phone/sensor_dashboard.py` into typed ROS2 messages. ### Files changed | File | Change | |------|--------| | `saltybot_phone/mqtt_ros2_bridge_node.py` | New ROS2 node | | `launch/mqtt_bridge.launch.py` | New launch file | | `setup.py` | Add `mqtt_ros2_bridge` console script | | `package.xml` | Add `python3-paho-mqtt` exec_depend | ### Topic mapping | MQTT topic | ROS2 topic | Message type | |------------|-----------|--------------| | `saltybot/phone/imu` | `/saltybot/phone/imu` | `sensor_msgs/Imu` | | `saltybot/phone/gps` | `/saltybot/phone/gps` | `sensor_msgs/NavSatFix` | | `saltybot/phone/battery` | `/saltybot/phone/battery` | `sensor_msgs/BatteryState` | | *(internal)* | `/saltybot/phone/bridge/status` | `std_msgs/String` (JSON) | ### Architecture - paho-mqtt `loop_start()` runs MQTT network in its own thread; `on_message` callback **only enqueues** `(topic, payload)` — no rclpy calls from MQTT thread - ROS2 50 Hz timer drains queue and publishes — all ROS2 operations on executor thread - Auto-reconnect: paho `reconnect_delay_set(5s → 20s)` ### Message details **IMU** — accel + gyro with configurable diagonal covariance; `orientation_covariance[0] = -1` (unknown, REP-145) **NavSatFix** — full 3×3 position covariance from `accuracy_m`; STATUS_FIX for gps/fused/network providers; `COVARIANCE_TYPE_APPROXIMATED` **BatteryState** — `percentage` [0–1]; `temperature` in Kelvin; Android health string → `POWER_SUPPLY_HEALTH_*`; `voltage`/`current` = NaN (unavailable on Android) ### Timestamp alignment Default: ROS2 wall clock (consistent with all other nodes). `use_phone_timestamp:=true` uses the phone's Unix epoch `ts` field when clock drift < `warn_drift_s` (default 1.0 s); warns and falls back to ROS2 clock if drift exceeds threshold. ### Input validation - IMU: finite value check on all 6 DOF values - GPS: latitude ∈ [−90, 90], longitude ∈ [−180, 180] - Battery: percentage ∈ [0, 100] - Malformed JSON and validation failures → DEBUG log + error counter ### Status topic (0.2 Hz) ```json {"mqtt_connected":true,"rx":{"saltybot/phone/imu":150,...},"pub":{...},"err":{...},"age_s":{"imu":0.2,"gps":1.0,"battery":0.9},"queue_depth":0} ``` ## Test plan - [ ] `python3 -m py_compile saltybot_phone/mqtt_ros2_bridge_node.py` — clean - [ ] On phone: `python3 phone/sensor_dashboard.py --broker <jetson_ip>` - [ ] On Jetson: `ros2 launch saltybot_phone mqtt_bridge.launch.py mqtt_host:=localhost` - [ ] `ros2 topic hz /saltybot/phone/imu` → ~5 Hz - [ ] `ros2 topic echo /saltybot/phone/gps` → valid lat/lon/covariance - [ ] `ros2 topic echo /saltybot/phone/battery` → percentage ∈ [0,1], temp in K - [ ] `ros2 topic echo /saltybot/phone/bridge/status` → JSON with mqtt_connected:true - [ ] Kill MQTT broker → `mqtt_connected:false` in status; restart → auto-reconnect - [ ] `use_phone_timestamp:=true` with > 1s clock skew → warning logged, ROS2 clock used 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-jetson added 2 commits 2026-03-14 15:00:13 -04:00 
Add saltybot_phone/mqtt_ros2_bridge_node.py — ROS2 node bridging the three
MQTT topics published by phone/sensor_dashboard.py into typed ROS2 messages:

  saltybot/phone/imu     → /saltybot/phone/imu     sensor_msgs/Imu
  saltybot/phone/gps     → /saltybot/phone/gps     sensor_msgs/NavSatFix
  saltybot/phone/battery → /saltybot/phone/battery sensor_msgs/BatteryState
  (status)               → /saltybot/phone/bridge/status std_msgs/String

Key design:
- paho-mqtt loop_start() runs in dedicated network thread; on_message
  enqueues (topic, payload) pairs into a thread-safe queue
- ROS2 timer drains queue at 50 Hz — all publishing stays on executor
  thread, avoiding any rclpy threading concerns
- Timestamp alignment: uses ROS2 wall clock by default; opt-in
  use_phone_timestamp param uses phone epoch ts when drift < warn_drift_s
- IMU: populates accel + gyro with diagonal covariance; orientation_cov[0]=-1
  (unknown per REP-145)
- GPS: NavSatStatus.STATUS_FIX for gps/fused/network providers; full 3×3
  position covariance from accuracy_m; COVARIANCE_TYPE_APPROXIMATED
- Battery: pct→percentage [0-1], temp Kelvin, health/status mapped from
  Android health strings, voltage/current=NaN (unavailable on Android)
- Input validation: finite value checks on IMU, lat/lon range on GPS,
  pct [0-100] on battery; bad messages logged at DEBUG and counted
- Status topic at 0.2 Hz: JSON {mqtt_connected, rx/pub/err counts,
  age_s per sensor, queue_depth}
- Auto-reconnect via paho reconnect_delay_set (5 s → 20 s max)

Add launch/mqtt_bridge.launch.py with args: mqtt_host, mqtt_port,
reconnect_delay_s, use_phone_timestamp, warn_drift_s, imu_accel_cov,
imu_gyro_cov.

Register mqtt_ros2_bridge console script in setup.py.
Add python3-paho-mqtt exec_depend to package.xml.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit bb5eff1382 into main 2026-03-14 15:55:23 -04:00
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#605
No description provided.
Delete Branch "sl-android/issue-601-mqtt-ros2-bridge"

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?