Compare commits
No commits in common. "885a66f24b179881d1ed0fdf9033fe863485c740" and "b1e8da4403beba99094a277bc0677519819d2cbf" have entirely different histories.
885a66f24b
...
b1e8da4403
@ -59,8 +59,7 @@ rosbridge_websocket:
|
||||
"/saltybot/phone/battery",
|
||||
"/saltybot/phone/bridge/status",
|
||||
"/gps/fix",
|
||||
"/gps/vel",
|
||||
"/saltybot/ios/gps"]
|
||||
"/gps/vel"]
|
||||
|
||||
services_glob: "[]" # no service calls via WebSocket
|
||||
params_glob: "[]" # no parameter access via WebSocket
|
||||
|
||||
@ -68,7 +68,6 @@ 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 ───────────────────────────────
|
||||
|
||||
@ -131,16 +130,15 @@ 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, _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._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._mqtt_connected = False
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────────
|
||||
@ -164,8 +162,8 @@ class MqttRos2BridgeNode(Node):
|
||||
self._mqtt_connect()
|
||||
|
||||
self.get_logger().info(
|
||||
f"MQTT→ROS2 bridge started — broker={p['mqtt_host']}:{p['mqtt_port']}"
|
||||
f" use_phone_ts={p['use_phone_ts']}"
|
||||
"MQTT→ROS2 bridge started — broker=%s:%d use_phone_ts=%s",
|
||||
p["mqtt_host"], p["mqtt_port"], p["use_phone_timestamp"],
|
||||
)
|
||||
|
||||
# ── Param helper ─────────────────────────────────────────────────────────
|
||||
@ -197,26 +195,26 @@ class MqttRos2BridgeNode(Node):
|
||||
)
|
||||
self._mqtt.loop_start()
|
||||
except Exception as e:
|
||||
self.get_logger().warning(f"MQTT initial connect error: {e}")
|
||||
self.get_logger().warning("MQTT initial connect error: %s", str(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, _TOPIC_IOS_GPS):
|
||||
for topic in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY):
|
||||
client.subscribe(topic, qos=0)
|
||||
self.get_logger().info(
|
||||
f"MQTT connected to {self._p['mqtt_host']}:{self._p['mqtt_port']}"
|
||||
" — subscribed to 4 phone topics"
|
||||
"MQTT connected to %s:%d — subscribed to 3 phone topics",
|
||||
self._p["mqtt_host"], self._p["mqtt_port"],
|
||||
)
|
||||
else:
|
||||
self.get_logger().warning(f"MQTT connect failed rc={rc}")
|
||||
self.get_logger().warning("MQTT connect failed rc=%d", rc)
|
||||
|
||||
def _on_mqtt_disconnect(self, client, userdata, rc) -> None:
|
||||
self._mqtt_connected = False
|
||||
if rc != 0:
|
||||
self.get_logger().warning(
|
||||
f"MQTT disconnected (rc={rc}) — paho will reconnect"
|
||||
"MQTT disconnected (rc=%d) — paho will reconnect", rc
|
||||
)
|
||||
|
||||
def _on_mqtt_message(self, client, userdata, message) -> None:
|
||||
@ -228,7 +226,7 @@ class MqttRos2BridgeNode(Node):
|
||||
try:
|
||||
self._q.put_nowait((topic, message.payload))
|
||||
except queue.Full:
|
||||
self.get_logger().debug(f"MQTT queue full — dropping {topic}")
|
||||
self.get_logger().debug("MQTT queue full — dropping %s", topic)
|
||||
|
||||
# ── Queue drain (ROS2 executor thread) ────────────────────────────────────
|
||||
|
||||
@ -248,7 +246,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(f"JSON decode error on {topic}: {e}")
|
||||
self.get_logger().debug("JSON decode error on %s: %s", topic, e)
|
||||
return
|
||||
|
||||
try:
|
||||
@ -258,12 +256,10 @@ 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(f"Payload error on {topic}: {e} — {data}")
|
||||
self.get_logger().debug("Payload error on %s: %s — %s", topic, e, data)
|
||||
|
||||
# ── Timestamp helper ──────────────────────────────────────────────────────
|
||||
|
||||
@ -285,7 +281,8 @@ class MqttRos2BridgeNode(Node):
|
||||
drift = abs(now_epoch - phone_ts)
|
||||
if drift > self._p["warn_drift_s"]:
|
||||
self.get_logger().warning(
|
||||
f"Phone clock drift {drift:.2f} s (frame_id={frame_id}) — using ROS2 clock"
|
||||
"Phone clock drift %.2f s (frame_id=%s) — using ROS2 clock",
|
||||
drift, frame_id,
|
||||
)
|
||||
hdr.stamp = self.get_clock().now().to_msg()
|
||||
else:
|
||||
@ -395,55 +392,6 @@ 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:
|
||||
@ -496,7 +444,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, _TOPIC_IOS_GPS)
|
||||
for t in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY)
|
||||
}
|
||||
body = {
|
||||
"mqtt_connected": self._mqtt_connected,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user