diff --git a/src/rov_mission/rov_mission/cockpit_bridge.py b/src/rov_mission/rov_mission/cockpit_bridge.py index 699ace8..034ef89 100644 --- a/src/rov_mission/rov_mission/cockpit_bridge.py +++ b/src/rov_mission/rov_mission/cockpit_bridge.py @@ -16,6 +16,8 @@ Variables in the Cockpit data lake (all prefixed 'external/' by Cockpit): rov-ms — mission state: 0=IDLE 1=RUNNING 2=PAUSED 3=COMPLETE 4=ABORTED rov-mp — mission progress percent (int 0-100) rov-recording — recorder active: 1=recording, 0=not recording (no-go indicator) + rov-altitude — Ping2 downward sonar altitude in metres (float, 2 dp). + -1.0 when no sonar data received (sentinel for W6 NO SONAR state). """ import asyncio @@ -24,7 +26,7 @@ import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy from std_msgs.msg import Bool, Float64 -from sensor_msgs.msg import BatteryState +from sensor_msgs.msg import BatteryState, Range from rov_interfaces.msg import FailsafeStatus, MissionStatus import websockets @@ -83,6 +85,12 @@ class CockpitBridge(Node): # and widgets can reflect recording health. # Default 0 (not recording) until recording_manager confirms active. 'rov-recording': 0, + + # Downward Ping2 sonar altitude in metres. + # -1.0 is the sentinel value for W6 — displayed as NO SONAR. + # Updated by _altitude_cb when /ping2/altitude messages arrive. + # Remains -1.0 until the Ping2 sonar is connected and publishing. + 'rov-altitude': -1.0, } # Set of currently connected Cockpit WebSocket clients. @@ -132,6 +140,15 @@ class CockpitBridge(Node): Bool, '/rov/recording/active', self._recording_cb, 10) + # Downward Ping2 sonar altitude from the ping2 driver node. + # sensor_msgs/Range.range is the distance in metres to the nearest + # echo return (hull surface or seafloor when pointing downward). + # Topic will not publish until the Ping2 hardware is connected; + # rov-altitude stays at the -1.0 sentinel (W6 shows NO SONAR). + self.create_subscription( + Range, '/ping2/altitude', + self._altitude_cb, 10) + # ------------------------------------------------------------------ # Timers # ------------------------------------------------------------------ @@ -183,6 +200,32 @@ class CockpitBridge(Node): with self._lock: self._values['rov-recording'] = 1 if msg.data else 0 + def _altitude_cb(self, msg: Range): + """ + Update downward sonar altitude from Ping2 driver node. + + sensor_msgs/Range.range is the measured distance in metres to the + nearest echo return. When the Ping2 is pointing downward, this is + the vehicle's altitude above the hull surface or seafloor. + + A value of range_max (100m for the Ping2) indicates no echo was + received within the operating range — treated the same as no data + and published as the -1.0 sentinel so W6 shows NO SONAR. + + The Ping2 range_max is 100.0m. A genuine 100m reading is extremely + unlikely in jacket inspection operations (max jacket height ~100m, + typical working depth 5-50m) so treating it as no-return is safe. + """ + with self._lock: + raw = float(msg.range) + + # Treat no-return (range_max) as sentinel — W6 shows NO SONAR + if raw >= msg.range_max: + self._values['rov-altitude'] = -1.0 + else: + # Round to 2dp for display — Ping2 is ±1% accuracy at range + self._values['rov-altitude'] = round(raw, 2) + # ------------------------------------------------------------------ # WebSocket broadcast — called from ROS2 timer at 2 Hz # ------------------------------------------------------------------