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:
parent
5d75a9d30d
commit
06403533f2
@ -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
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
Loading…
Reference in New Issue
Block a user