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

feat: Add iOS phone GPS MQTT-to-ROS2 bridge topic (Issue #681)

Browse Source 
- Add _TOPIC_IOS_GPS = 'saltybot/ios/gps' constant
- Subscribe to saltybot/ios/gps in _on_mqtt_connect
- Dispatch to _handle_ios_gps() in _dispatch()
- _handle_ios_gps(): same logic as _handle_gps(), frame_id='ios_gps',
  publishes to /saltybot/ios/gps via self._ios_gps_pub
- Add rx/pub/err/last_rx_ts counters for the new topic
- Add /saltybot/ios/gps to rosbridge_params.yaml topics_glob

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-jetson 2026-04-04 11:11:38 -04:00
parent 43fb3f1147
commit 29a4d8cf55
2 changed files with 62 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml
View File

@ -53,7 +53,8 @@ rosbridge_websocket:
"/vesc/left/state",
"/vesc/right/state",
"/tf",
"/tf_static"]
"/tf_static",
"/saltybot/ios/gps"]
services_glob: "[]" # no service calls via WebSocket
params_glob: "[]" # no parameter access via WebSocket

67
jetson/ros2_ws/src/saltybot_phone/saltybot_phone/mqtt_ros2_bridge_node.py
View File

@ -68,6 +68,7 @@ except ImportError:
_TOPIC_IMU = "saltybot/phone/imu"
_TOPIC_GPS = "saltybot/phone/gps"
_TOPIC_BATTERY = "saltybot/phone/battery"
_TOPIC_IOS_GPS = "saltybot/ios/gps"
# ── BatteryState health/power-supply constants ───────────────────────────────
@ -130,15 +131,16 @@ class MqttRos2BridgeNode(Node):
self._gps_pub = self.create_publisher(NavSatFix, "/saltybot/phone/gps", 10)
self._bat_pub = self.create_publisher(BatteryState, "/saltybot/phone/battery", 10)
self._status_pub = self.create_publisher(String, "/saltybot/phone/bridge/status", 10)
self._ios_gps_pub = self.create_publisher(NavSatFix, "/saltybot/ios/gps", 10)
# ── Message queue (MQTT thread → ROS2 executor thread) ────────────────
self._q: queue.Queue[tuple[str, bytes]] = queue.Queue(maxsize=200)
# ── Stats ─────────────────────────────────────────────────────────────
self._rx_count = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0}
self._pub_count = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0}
self._err_count = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0}
self._last_rx_ts = {_TOPIC_IMU: 0.0, _TOPIC_GPS: 0.0, _TOPIC_BATTERY: 0.0}
self._rx_count = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0, _TOPIC_IOS_GPS: 0}
self._pub_count = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0, _TOPIC_IOS_GPS: 0}
self._err_count = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0, _TOPIC_IOS_GPS: 0}
self._last_rx_ts = {_TOPIC_IMU: 0.0, _TOPIC_GPS: 0.0, _TOPIC_BATTERY: 0.0, _TOPIC_IOS_GPS: 0.0}
self._mqtt_connected = False
# ── Timers ────────────────────────────────────────────────────────────
@ -201,10 +203,10 @@ class MqttRos2BridgeNode(Node):
if rc == 0:
self._mqtt_connected = True
# Subscribe to all phone sensor topics
for topic in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY):
for topic in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY, _TOPIC_IOS_GPS):
client.subscribe(topic, qos=0)
self.get_logger().info(
"MQTT connected to %s:%d — subscribed to 3 phone topics",
"MQTT connected to %s:%d — subscribed to 4 phone topics",
self._p["mqtt_host"], self._p["mqtt_port"],
)
else:
@ -256,6 +258,8 @@ class MqttRos2BridgeNode(Node):
self._handle_gps(data)
elif topic == _TOPIC_BATTERY:
self._handle_battery(data)
elif topic == _TOPIC_IOS_GPS:
self._handle_ios_gps(data)
except (KeyError, TypeError, ValueError) as e:
if topic in self._err_count:
self._err_count[topic] += 1
@ -392,6 +396,55 @@ class MqttRos2BridgeNode(Node):
self._gps_pub.publish(msg)
self._pub_count[_TOPIC_GPS] += 1
# ── iOS GPS handler ───────────────────────────────────────────────────────
def _handle_ios_gps(self, data: dict) -> None:
"""
JSON: {"ts":..., "lat","lon","alt_m","accuracy_m","speed_ms","bearing_deg","provider"}
Same schema as phone GPS; publishes to /saltybot/ios/gps with frame_id 'ios_gps'.
"""
lat = float(data["lat"])
lon = float(data["lon"])
alt = float(data.get("alt_m", 0.0))
acc = float(data.get("accuracy_m", -1.0))
if not (-90.0 <= lat <= 90.0):
raise ValueError(f"invalid latitude: {lat}")
if not (-180.0 <= lon <= 180.0):
raise ValueError(f"invalid longitude: {lon}")
msg = NavSatFix()
msg.header = self._make_header(data, "ios_gps")
msg.latitude = lat
msg.longitude = lon
msg.altitude = alt
provider = data.get("provider", "unknown").lower()
msg.status.service = NavSatStatus.SERVICE_GPS
if provider in ("gps", "fused"):
msg.status.status = NavSatStatus.STATUS_FIX
elif provider == "network":
msg.status.status = NavSatStatus.STATUS_FIX
msg.status.service = NavSatStatus.SERVICE_COMPASS
else:
msg.status.status = NavSatStatus.STATUS_NO_FIX
if acc > 0.0:
h_var = acc * acc
v_var = h_var * self._p["gps_alt_cov_factor"]
msg.position_covariance = [
h_var, 0.0, 0.0,
0.0, h_var, 0.0,
0.0, 0.0, v_var,
]
msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
else:
msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self._ios_gps_pub.publish(msg)
self._pub_count[_TOPIC_IOS_GPS] += 1
# ── Battery handler ───────────────────────────────────────────────────────
def _handle_battery(self, data: dict) -> None:
@ -444,7 +497,7 @@ class MqttRos2BridgeNode(Node):
now = time.time()
ages = {
t: round(now - self._last_rx_ts[t], 1) if self._last_rx_ts[t] > 0 else -1.0
for t in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY)
for t in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY, _TOPIC_IOS_GPS)
}
body = {
"mqtt_connected": self._mqtt_connected,