W6: Add rov-altitude to cockpit_bridge for Ping2 downward sonar

- Add Range subscription on /ping2/altitude (sensor_msgs/Range)
- Publish rov-altitude to data lake (-1.0 sentinel = NO SONAR)
- Treats range_max (100m) as no-return, same as no-data sentinel
This commit is contained in:
Grant du Toit 2026-07-04 11:27:39 +00:00
parent 5d75a9d30d
commit 06403533f2

View File

@ -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
# ------------------------------------------------------------------