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-ms — mission state: 0=IDLE 1=RUNNING 2=PAUSED 3=COMPLETE 4=ABORTED
|
||||||
rov-mp — mission progress percent (int 0-100)
|
rov-mp — mission progress percent (int 0-100)
|
||||||
rov-recording — recorder active: 1=recording, 0=not recording (no-go indicator)
|
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
|
import asyncio
|
||||||
@ -24,7 +26,7 @@ import rclpy
|
|||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
from rclpy.qos import QoSProfile, ReliabilityPolicy
|
from rclpy.qos import QoSProfile, ReliabilityPolicy
|
||||||
from std_msgs.msg import Bool, Float64
|
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
|
from rov_interfaces.msg import FailsafeStatus, MissionStatus
|
||||||
import websockets
|
import websockets
|
||||||
|
|
||||||
@ -83,6 +85,12 @@ class CockpitBridge(Node):
|
|||||||
# and widgets can reflect recording health.
|
# and widgets can reflect recording health.
|
||||||
# Default 0 (not recording) until recording_manager confirms active.
|
# Default 0 (not recording) until recording_manager confirms active.
|
||||||
'rov-recording': 0,
|
'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.
|
# Set of currently connected Cockpit WebSocket clients.
|
||||||
@ -132,6 +140,15 @@ class CockpitBridge(Node):
|
|||||||
Bool, '/rov/recording/active',
|
Bool, '/rov/recording/active',
|
||||||
self._recording_cb, 10)
|
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
|
# Timers
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
@ -183,6 +200,32 @@ class CockpitBridge(Node):
|
|||||||
with self._lock:
|
with self._lock:
|
||||||
self._values['rov-recording'] = 1 if msg.data else 0
|
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
|
# WebSocket broadcast — called from ROS2 timer at 2 Hz
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user