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',
|
||||
output='screen',
|
||||
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={
|
||||
'console_scripts': [
|
||||
'mission_executor = rov_mission.mission_executor:main',
|
||||
'cockpit_bridge = rov_mission.cockpit_bridge:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user