diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml index 8188db8..cd215f0 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml @@ -59,7 +59,8 @@ rosbridge_websocket: "/saltybot/phone/battery", "/saltybot/phone/bridge/status", "/gps/fix", - "/gps/vel"] + "/gps/vel", + "/saltybot/ios/gps"] services_glob: "[]" # no service calls via WebSocket params_glob: "[]" # no parameter access via WebSocket diff --git a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/mqtt_ros2_bridge_node.py b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/mqtt_ros2_bridge_node.py index bfa9e27..e8932de 100644 --- a/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/mqtt_ros2_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_phone/saltybot_phone/mqtt_ros2_bridge_node.py @@ -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 ──────────────────────────────────────────────────────────── @@ -162,8 +164,8 @@ class MqttRos2BridgeNode(Node): self._mqtt_connect() self.get_logger().info( - "MQTT→ROS2 bridge started — broker=%s:%d use_phone_ts=%s", - p["mqtt_host"], p["mqtt_port"], p["use_phone_timestamp"], + f"MQTT→ROS2 bridge started — broker={p['mqtt_host']}:{p['mqtt_port']}" + f" use_phone_ts={p['use_phone_ts']}" ) # ── Param helper ───────────────────────────────────────────────────────── @@ -195,26 +197,26 @@ class MqttRos2BridgeNode(Node): ) self._mqtt.loop_start() except Exception as e: - self.get_logger().warning("MQTT initial connect error: %s", str(e)) + self.get_logger().warning(f"MQTT initial connect error: {e}") def _on_mqtt_connect(self, client, userdata, flags, rc) -> None: 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", - self._p["mqtt_host"], self._p["mqtt_port"], + f"MQTT connected to {self._p['mqtt_host']}:{self._p['mqtt_port']}" + " — subscribed to 4 phone topics" ) else: - self.get_logger().warning("MQTT connect failed rc=%d", rc) + self.get_logger().warning(f"MQTT connect failed rc={rc}") def _on_mqtt_disconnect(self, client, userdata, rc) -> None: self._mqtt_connected = False if rc != 0: self.get_logger().warning( - "MQTT disconnected (rc=%d) — paho will reconnect", rc + f"MQTT disconnected (rc={rc}) — paho will reconnect" ) def _on_mqtt_message(self, client, userdata, message) -> None: @@ -226,7 +228,7 @@ class MqttRos2BridgeNode(Node): try: self._q.put_nowait((topic, message.payload)) except queue.Full: - self.get_logger().debug("MQTT queue full — dropping %s", topic) + self.get_logger().debug(f"MQTT queue full — dropping {topic}") # ── Queue drain (ROS2 executor thread) ──────────────────────────────────── @@ -246,7 +248,7 @@ class MqttRos2BridgeNode(Node): self._err_count.get(topic, {}) # just access if topic in self._err_count: self._err_count[topic] += 1 - self.get_logger().debug("JSON decode error on %s: %s", topic, e) + self.get_logger().debug(f"JSON decode error on {topic}: {e}") return try: @@ -256,10 +258,12 @@ 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 - self.get_logger().debug("Payload error on %s: %s — %s", topic, e, data) + self.get_logger().debug(f"Payload error on {topic}: {e} — {data}") # ── Timestamp helper ────────────────────────────────────────────────────── @@ -281,8 +285,7 @@ class MqttRos2BridgeNode(Node): drift = abs(now_epoch - phone_ts) if drift > self._p["warn_drift_s"]: self.get_logger().warning( - "Phone clock drift %.2f s (frame_id=%s) — using ROS2 clock", - drift, frame_id, + f"Phone clock drift {drift:.2f} s (frame_id={frame_id}) — using ROS2 clock" ) hdr.stamp = self.get_clock().now().to_msg() else: @@ -392,6 +395,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 +496,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,