sl-android
|
dd13569413
|
feat: MQTT-to-ROS2 phone sensor bridge (Issue #601)
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>
|
2026-03-14 14:59:02 -04:00 |
|