Add cockpit_bridge node for Cockpit data-lake injection
- New node bridges ROV topics to Cockpit via Generic WebSocket on :9001
- Sends variableName=value format (Cockpit v1.18+ protocol)
- Variables land in data lake as external/rov-{failsafe,depth,voltage,heading,ms,mp}
- Subscribes: /rov/failsafe, /rov/depth, /mavros/battery,
/mavros/mavros/compass_hdg, /rov/mission/status
- Added to mission.launch.py and rov_mission entry points
- Verified: Cockpit connects, all 6 external/rov-* vars appear in data lake
This commit is contained in:
parent
d7f6d3f93e
commit
12b5161436
@ -36,6 +36,12 @@ def _launch_with_config(context, *args, **kwargs):
|
|||||||
name='mission_executor',
|
name='mission_executor',
|
||||||
output='screen',
|
output='screen',
|
||||||
parameters=[config_file],
|
parameters=[config_file],
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package='rov_mission',
|
||||||
|
executable='cockpit_bridge',
|
||||||
|
name='cockpit_bridge',
|
||||||
|
output='screen',
|
||||||
)
|
)
|
||||||
]
|
]
|
||||||
|
|
||||||
|
|||||||
164
src/rov_mission/rov_mission/cockpit_bridge.py
Normal file
164
src/rov_mission/rov_mission/cockpit_bridge.py
Normal file
@ -0,0 +1,164 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
cockpit_bridge.py - Cockpit Generic WebSocket data injector.
|
||||||
|
|
||||||
|
Subscribes to key ROV topics and serves them to Cockpit's Generic WebSocket
|
||||||
|
connection in the required 'variableName=value' format, one message per send.
|
||||||
|
|
||||||
|
Cockpit setup: Settings -> General -> Generic WebSocket Connections
|
||||||
|
Add: ws://192.168.1.101:9001
|
||||||
|
Variables appear in data lake as: rov-failsafe, rov-depth, rov-voltage,
|
||||||
|
rov-heading, rov-ms, rov-mp
|
||||||
|
"""
|
||||||
|
|
||||||
|
import asyncio
|
||||||
|
import threading
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from rclpy.qos import QoSProfile, ReliabilityPolicy
|
||||||
|
from std_msgs.msg import Float64
|
||||||
|
from sensor_msgs.msg import BatteryState
|
||||||
|
from rov_interfaces.msg import FailsafeStatus, MissionStatus
|
||||||
|
import websockets
|
||||||
|
|
||||||
|
# Port Cockpit connects to
|
||||||
|
WS_PORT = 9001
|
||||||
|
|
||||||
|
# QoS for MAVROS topics - must be BEST_EFFORT
|
||||||
|
best_effort_qos = QoSProfile(
|
||||||
|
depth=10,
|
||||||
|
reliability=ReliabilityPolicy.BEST_EFFORT
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class CockpitBridge(Node):
|
||||||
|
"""ROS2 node that bridges ROV topics to Cockpit via Generic WebSocket."""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('cockpit_bridge')
|
||||||
|
|
||||||
|
# Current values - updated by ROS2 callbacks, read by WebSocket server
|
||||||
|
self._values = {
|
||||||
|
'rov-failsafe': -1,
|
||||||
|
'rov-depth': 0.0,
|
||||||
|
'rov-voltage': 0.0,
|
||||||
|
'rov-heading': 0.0,
|
||||||
|
'rov-ms': -1,
|
||||||
|
'rov-mp': 0,
|
||||||
|
}
|
||||||
|
|
||||||
|
# Connected WebSocket clients
|
||||||
|
self._ws_clients: set = set()
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._loop = None
|
||||||
|
|
||||||
|
# Subscribers
|
||||||
|
self.create_subscription(
|
||||||
|
FailsafeStatus, '/rov/failsafe', self._failsafe_cb, 10)
|
||||||
|
self.create_subscription(
|
||||||
|
Float64, '/rov/depth', self._depth_cb, 10)
|
||||||
|
self.create_subscription(
|
||||||
|
BatteryState, '/mavros/battery', self._battery_cb, best_effort_qos)
|
||||||
|
self.create_subscription(
|
||||||
|
Float64, '/mavros/mavros/compass_hdg', self._heading_cb, best_effort_qos)
|
||||||
|
self.create_subscription(
|
||||||
|
MissionStatus, '/rov/mission/status', self._mission_cb, 10)
|
||||||
|
|
||||||
|
# Broadcast timer at 2 Hz
|
||||||
|
self.create_timer(0.5, self._publish_to_clients)
|
||||||
|
|
||||||
|
self.get_logger().info(f'CockpitBridge started on port {WS_PORT}')
|
||||||
|
|
||||||
|
def _failsafe_cb(self, msg: FailsafeStatus):
|
||||||
|
"""Update failsafe state from FailsafeStatus message."""
|
||||||
|
with self._lock:
|
||||||
|
self._values['rov-failsafe'] = int(msg.failsafe_state)
|
||||||
|
|
||||||
|
def _depth_cb(self, msg: Float64):
|
||||||
|
"""Update depth in metres from Float64 message."""
|
||||||
|
with self._lock:
|
||||||
|
self._values['rov-depth'] = round(float(msg.data), 3)
|
||||||
|
|
||||||
|
def _battery_cb(self, msg: BatteryState):
|
||||||
|
"""Update battery voltage from BatteryState message."""
|
||||||
|
with self._lock:
|
||||||
|
self._values['rov-voltage'] = round(float(msg.voltage), 2)
|
||||||
|
|
||||||
|
def _heading_cb(self, msg: Float64):
|
||||||
|
"""Update compass heading in degrees from Float64 message."""
|
||||||
|
with self._lock:
|
||||||
|
self._values['rov-heading'] = round(float(msg.data), 1)
|
||||||
|
|
||||||
|
def _mission_cb(self, msg: MissionStatus):
|
||||||
|
"""Update mission state and progress from MissionStatus message."""
|
||||||
|
with self._lock:
|
||||||
|
self._values['rov-ms'] = int(msg.state)
|
||||||
|
self._values['rov-mp'] = int(msg.progress_percent)
|
||||||
|
|
||||||
|
def _publish_to_clients(self):
|
||||||
|
"""Timer callback - broadcast all current values to Cockpit clients."""
|
||||||
|
with self._lock:
|
||||||
|
messages = [f'{k}={v}' for k, v in self._values.items()]
|
||||||
|
if not self._ws_clients or self._loop is None:
|
||||||
|
return
|
||||||
|
asyncio.run_coroutine_threadsafe(
|
||||||
|
self._broadcast(messages), self._loop)
|
||||||
|
|
||||||
|
async def _broadcast(self, messages: list):
|
||||||
|
"""Send each variable as a separate WebSocket message to all clients."""
|
||||||
|
disconnected = set()
|
||||||
|
for client in list(self._ws_clients):
|
||||||
|
try:
|
||||||
|
for msg in messages:
|
||||||
|
await client.send(msg)
|
||||||
|
except Exception:
|
||||||
|
disconnected.add(client)
|
||||||
|
self._ws_clients -= disconnected
|
||||||
|
|
||||||
|
async def _ws_handler(self, websocket):
|
||||||
|
"""Handle an incoming Cockpit WebSocket connection."""
|
||||||
|
self.get_logger().info(f'Cockpit connected: {websocket.remote_address}')
|
||||||
|
self._ws_clients.add(websocket)
|
||||||
|
try:
|
||||||
|
await websocket.wait_closed()
|
||||||
|
finally:
|
||||||
|
self._ws_clients.discard(websocket)
|
||||||
|
self.get_logger().info(f'Cockpit disconnected: {websocket.remote_address}')
|
||||||
|
|
||||||
|
def set_event_loop(self, loop):
|
||||||
|
"""Store asyncio event loop reference for cross-thread scheduling."""
|
||||||
|
self._loop = loop
|
||||||
|
|
||||||
|
|
||||||
|
async def ws_server(node: CockpitBridge):
|
||||||
|
"""Run the WebSocket server indefinitely."""
|
||||||
|
async with websockets.serve(node._ws_handler, '0.0.0.0', WS_PORT):
|
||||||
|
node.get_logger().info(f'WebSocket listening on 0.0.0.0:{WS_PORT}')
|
||||||
|
await asyncio.get_event_loop().create_future()
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
"""Entry point - runs ROS2 node and WebSocket server concurrently."""
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = CockpitBridge()
|
||||||
|
|
||||||
|
loop = asyncio.new_event_loop()
|
||||||
|
node.set_event_loop(loop)
|
||||||
|
|
||||||
|
def run_ws():
|
||||||
|
asyncio.set_event_loop(loop)
|
||||||
|
loop.run_until_complete(ws_server(node))
|
||||||
|
|
||||||
|
ws_thread = threading.Thread(target=run_ws, daemon=True)
|
||||||
|
ws_thread.start()
|
||||||
|
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
loop.stop()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
@ -19,6 +19,7 @@ setup(
|
|||||||
entry_points={
|
entry_points={
|
||||||
'console_scripts': [
|
'console_scripts': [
|
||||||
'mission_executor = rov_mission.mission_executor:main',
|
'mission_executor = rov_mission.mission_executor:main',
|
||||||
|
'cockpit_bridge = rov_mission.cockpit_bridge:main',
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user