Phase 3 Stage 1: Recording Manager (DIR-9)
- Add recording_manager node: continuous rosbag2 MCAP recorder, zstd compression, 500MB bag splitting, 10GB retention policy, 1Hz health monitoring and restart on subprocess exit - Add /rov/recording/active no-go gate to mission_executor START handler - Add rov-recording variable to cockpit_bridge (data lake visibility) - Replace foxglove_mcap ExecuteProcess with recording_manager Node - Register recording_manager entry point in setup.py - Confirmed: data: true on /rov/recording/active in full stack
This commit is contained in:
parent
2cb7b74777
commit
5d75a9d30d
@ -1,75 +1,79 @@
|
|||||||
"""
|
"""
|
||||||
foxglove_mcap.launch.py
|
foxglove_mcap.launch.py
|
||||||
Launches foxglove_bridge (WebSocket :8765) and MCAP bag recording.
|
Launches foxglove_bridge (WebSocket :8765) and the recording_manager node.
|
||||||
|
|
||||||
Included automatically by rov_full.launch.py.
|
Included automatically by rov_full.launch.py.
|
||||||
|
|
||||||
Standalone:
|
Standalone:
|
||||||
ros2 launch rov_bringup foxglove_mcap.launch.py
|
ros2 launch rov_bringup foxglove_mcap.launch.py
|
||||||
ros2 launch rov_bringup foxglove_mcap.launch.py record:=false
|
|
||||||
|
|
||||||
Foxglove connection from Edge PC:
|
Foxglove connection from Edge PC:
|
||||||
Foxglove Studio -> Open Connection -> Foxglove WebSocket -> ws://rov-brain.local:8765
|
Foxglove Studio -> Open Connection -> Foxglove WebSocket -> ws://rov-brain.local:8765
|
||||||
|
|
||||||
Storage:
|
Recording:
|
||||||
Bags written to /data/bags/ on NVMe. Create before first launch:
|
Managed by recording_manager node (rov_mission package).
|
||||||
|
Bags written to /data/bags/dive_<timestamp>/ on NVMe.
|
||||||
|
Create /data/bags before first launch if it does not exist:
|
||||||
sudo mkdir -p /data/bags && sudo chown ubuntu:ubuntu /data/bags
|
sudo mkdir -p /data/bags && sudo chown ubuntu:ubuntu /data/bags
|
||||||
|
|
||||||
|
Recording behaviour (DIR-9):
|
||||||
|
- recording_manager starts ros2 bag record on node startup.
|
||||||
|
- Records continuously, independent of mission state.
|
||||||
|
- Publishes /rov/recording/active (Bool) — hard no-go gate for mission start.
|
||||||
|
- Retention: deletes oldest dive_* dirs when /data free space < 10 GB.
|
||||||
|
- MCAP format, zstd compression, 500 MB bag segments.
|
||||||
"""
|
"""
|
||||||
import datetime
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo
|
|
||||||
from launch.conditions import IfCondition
|
|
||||||
from launch.substitutions import LaunchConfiguration
|
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
|
|
||||||
bag_output_arg = DeclareLaunchArgument(
|
# -----------------------------------------------------------------------
|
||||||
'bag_output_dir',
|
# Foxglove Bridge
|
||||||
default_value='/data/bags/dive_' + datetime.datetime.now().strftime('%Y_%m_%d-%H_%M_%S'),
|
# Exposes all ROS2 topics over a WebSocket for Foxglove Studio.
|
||||||
description='Base path for MCAP bag. Timestamp appended automatically.',
|
# Visualisation only — does NOT record MCAP (DIR-9).
|
||||||
)
|
# Connect Foxglove Studio to: ws://rov-brain.local:8765
|
||||||
|
# -----------------------------------------------------------------------
|
||||||
record_arg = DeclareLaunchArgument(
|
|
||||||
'record',
|
|
||||||
default_value='true',
|
|
||||||
description='Set false to run foxglove_bridge without recording.',
|
|
||||||
)
|
|
||||||
|
|
||||||
# Exposes all ROS2 topics over WebSocket
|
|
||||||
# Edge PC connects via Foxglove Studio -> ws://rov-brain.local:8765
|
|
||||||
foxglove_bridge_node = Node(
|
foxglove_bridge_node = Node(
|
||||||
package='foxglove_bridge',
|
package='foxglove_bridge',
|
||||||
executable='foxglove_bridge',
|
executable='foxglove_bridge',
|
||||||
name='foxglove_bridge',
|
name='foxglove_bridge',
|
||||||
parameters=[{
|
parameters=[{
|
||||||
|
# Listen on all interfaces — required when connecting over tether
|
||||||
'port': 8765,
|
'port': 8765,
|
||||||
'address': '0.0.0.0', # all interfaces — required for tether access
|
'address': '0.0.0.0',
|
||||||
|
|
||||||
|
# TLS disabled for LAN/tether use; enable for public internet
|
||||||
'tls': False,
|
'tls': False,
|
||||||
'send_buffer_limit': 10000000, # 10MB — headroom for image topics
|
|
||||||
'max_update_ms': 100, # 10Hz cap — prevents WebSocket flooding
|
# 10 MB send buffer — headroom for compressed image topics
|
||||||
|
'send_buffer_limit': 10000000,
|
||||||
|
|
||||||
|
# 10 Hz cap on topic updates — prevents WebSocket flooding on
|
||||||
|
# high-rate topics (IMU, EKF at 50+ Hz) without dropping data
|
||||||
|
# for operator-relevant topics (mission status, depth at ≤10 Hz)
|
||||||
|
'max_update_ms': 100,
|
||||||
}],
|
}],
|
||||||
output='screen',
|
output='screen',
|
||||||
)
|
)
|
||||||
|
|
||||||
# Records all topics to NVMe in MCAP format (Foxglove native)
|
# -----------------------------------------------------------------------
|
||||||
# Output dir is auto-timestamped, e.g.: /data/bags/dive_2026_05_07-14_22_05/
|
# Recording Manager
|
||||||
mcap_recorder = ExecuteProcess(
|
# Dedicated continuous recorder node (DIR-9).
|
||||||
cmd=[
|
# Manages ros2 bag record subprocess internally.
|
||||||
'ros2', 'bag', 'record',
|
# Starts recording on node startup — before any mission can begin.
|
||||||
'--all',
|
# Publishes /rov/recording/active as the mission start no-go gate.
|
||||||
'--storage', 'mcap',
|
# -----------------------------------------------------------------------
|
||||||
'--output', LaunchConfiguration('bag_output_dir'),
|
recording_manager_node = Node(
|
||||||
'--max-bag-size', '0', # no size limit — one bag per dive
|
package='rov_mission',
|
||||||
],
|
executable='recording_manager',
|
||||||
|
name='recording_manager',
|
||||||
output='screen',
|
output='screen',
|
||||||
condition=IfCondition(LaunchConfiguration('record')),
|
|
||||||
)
|
)
|
||||||
|
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
bag_output_arg,
|
|
||||||
record_arg,
|
|
||||||
LogInfo(msg='foxglove_bridge: connect Foxglove Studio to ws://<RPi5-IP>:8765'),
|
|
||||||
foxglove_bridge_node,
|
foxglove_bridge_node,
|
||||||
mcap_recorder,
|
recording_manager_node,
|
||||||
])
|
])
|
||||||
|
|||||||
@ -7,8 +7,15 @@ connection in the required 'variableName=value' format, one message per send.
|
|||||||
|
|
||||||
Cockpit setup: Settings -> General -> Generic WebSocket Connections
|
Cockpit setup: Settings -> General -> Generic WebSocket Connections
|
||||||
Add: ws://192.168.1.101:9001
|
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
|
Variables in the Cockpit data lake (all prefixed 'external/' by Cockpit):
|
||||||
|
rov-failsafe — failsafe state: 0=GREEN, 1=AMBER, 2=RED
|
||||||
|
rov-depth — depth in metres (float, 3 dp)
|
||||||
|
rov-voltage — battery voltage (float, 2 dp)
|
||||||
|
rov-heading — compass heading in degrees (float, 1 dp)
|
||||||
|
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)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import asyncio
|
import asyncio
|
||||||
@ -16,15 +23,16 @@ import threading
|
|||||||
import rclpy
|
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 Float64
|
from std_msgs.msg import Bool, Float64
|
||||||
from sensor_msgs.msg import BatteryState
|
from sensor_msgs.msg import BatteryState
|
||||||
from rov_interfaces.msg import FailsafeStatus, MissionStatus
|
from rov_interfaces.msg import FailsafeStatus, MissionStatus
|
||||||
import websockets
|
import websockets
|
||||||
|
|
||||||
# Port Cockpit connects to
|
# Port Cockpit connects to as a Generic WebSocket client
|
||||||
WS_PORT = 9001
|
WS_PORT = 9001
|
||||||
|
|
||||||
# QoS for MAVROS topics - must be BEST_EFFORT
|
# QoS for MAVROS topics — ArduSub publishes with BEST_EFFORT reliability;
|
||||||
|
# using RELIABLE here would cause subscriptions to silently receive nothing.
|
||||||
best_effort_qos = QoSProfile(
|
best_effort_qos = QoSProfile(
|
||||||
depth=10,
|
depth=10,
|
||||||
reliability=ReliabilityPolicy.BEST_EFFORT
|
reliability=ReliabilityPolicy.BEST_EFFORT
|
||||||
@ -32,45 +40,116 @@ best_effort_qos = QoSProfile(
|
|||||||
|
|
||||||
|
|
||||||
class CockpitBridge(Node):
|
class CockpitBridge(Node):
|
||||||
"""ROS2 node that bridges ROV topics to Cockpit via Generic WebSocket."""
|
"""
|
||||||
|
ROS2 node that bridges ROV topics to Cockpit via Generic WebSocket.
|
||||||
|
|
||||||
|
Maintains a dict of current variable values, updated by ROS2 topic
|
||||||
|
callbacks, and broadcasts all values to connected Cockpit clients
|
||||||
|
at 2 Hz over a plain-text WebSocket in 'variable-name=value' format.
|
||||||
|
"""
|
||||||
|
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
super().__init__('cockpit_bridge')
|
super().__init__('cockpit_bridge')
|
||||||
|
|
||||||
# Current values - updated by ROS2 callbacks, read by WebSocket server
|
# ------------------------------------------------------------------
|
||||||
|
# Current variable values
|
||||||
|
# Updated by ROS2 callbacks; read by the WebSocket broadcast timer.
|
||||||
|
# All access to this dict from callbacks and the timer is protected
|
||||||
|
# by self._lock to prevent data races across threads.
|
||||||
|
# ------------------------------------------------------------------
|
||||||
self._values = {
|
self._values = {
|
||||||
'rov-failsafe': -1,
|
# Failsafe state integer: 0=GREEN, 1=AMBER, 2=RED
|
||||||
'rov-depth': 0.0,
|
# -1 initial value indicates no data received yet
|
||||||
'rov-voltage': 0.0,
|
'rov-failsafe': -1,
|
||||||
'rov-heading': 0.0,
|
|
||||||
'rov-ms': -1,
|
# Depth in metres below surface (positive = deeper)
|
||||||
'rov-mp': 0,
|
'rov-depth': 0.0,
|
||||||
|
|
||||||
|
# Battery voltage in volts
|
||||||
|
'rov-voltage': 0.0,
|
||||||
|
|
||||||
|
# Compass heading in degrees (0-360)
|
||||||
|
'rov-heading': 0.0,
|
||||||
|
|
||||||
|
# Mission state integer per MissionStatus constants
|
||||||
|
# -1 initial value indicates no data received yet
|
||||||
|
'rov-ms': -1,
|
||||||
|
|
||||||
|
# Mission progress 0-100 percent
|
||||||
|
'rov-mp': 0,
|
||||||
|
|
||||||
|
# Recording active: 1 when rosbag2 recorder is healthy, 0 otherwise.
|
||||||
|
# Surfaced in Cockpit so operators can see the no-go gate state
|
||||||
|
# and widgets can reflect recording health.
|
||||||
|
# Default 0 (not recording) until recording_manager confirms active.
|
||||||
|
'rov-recording': 0,
|
||||||
}
|
}
|
||||||
|
|
||||||
# Connected WebSocket clients
|
# Set of currently connected Cockpit WebSocket clients.
|
||||||
|
# Modified from the asyncio thread; broadcast called from ROS2 timer.
|
||||||
self._ws_clients: set = set()
|
self._ws_clients: set = set()
|
||||||
|
|
||||||
|
# Protects _values and _ws_clients access across the ROS2 spin
|
||||||
|
# thread and the asyncio WebSocket thread.
|
||||||
self._lock = threading.Lock()
|
self._lock = threading.Lock()
|
||||||
|
|
||||||
|
# Reference to the asyncio event loop running in the WebSocket thread.
|
||||||
|
# Set by set_event_loop() before the WebSocket thread starts.
|
||||||
self._loop = None
|
self._loop = None
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
# Subscribers
|
# 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
|
# Failsafe state from the failsafe monitor node
|
||||||
|
self.create_subscription(
|
||||||
|
FailsafeStatus, '/rov/failsafe',
|
||||||
|
self._failsafe_cb, 10)
|
||||||
|
|
||||||
|
# Depth from the pressure/depth sensor node
|
||||||
|
self.create_subscription(
|
||||||
|
Float64, '/rov/depth',
|
||||||
|
self._depth_cb, 10)
|
||||||
|
|
||||||
|
# Battery state from MAVROS (ArduSub reports battery over MAVLink)
|
||||||
|
self.create_subscription(
|
||||||
|
BatteryState, '/mavros/battery',
|
||||||
|
self._battery_cb, best_effort_qos)
|
||||||
|
|
||||||
|
# Compass heading from MAVROS (GLOBAL_POSITION_INT / VFR_HUD)
|
||||||
|
self.create_subscription(
|
||||||
|
Float64, '/mavros/mavros/compass_hdg',
|
||||||
|
self._heading_cb, best_effort_qos)
|
||||||
|
|
||||||
|
# Mission state and progress from mission_executor
|
||||||
|
self.create_subscription(
|
||||||
|
MissionStatus, '/rov/mission/status',
|
||||||
|
self._mission_cb, 10)
|
||||||
|
|
||||||
|
# Recording active flag from recording_manager (DIR-9 no-go gate)
|
||||||
|
# Surfaced to Cockpit so operators can see the recorder state.
|
||||||
|
self.create_subscription(
|
||||||
|
Bool, '/rov/recording/active',
|
||||||
|
self._recording_cb, 10)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Timers
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# Broadcast all current values to Cockpit clients at 2 Hz.
|
||||||
|
# 2 Hz is sufficient for UI feedback; higher rates increase WebSocket
|
||||||
|
# load without meaningful operator benefit.
|
||||||
self.create_timer(0.5, self._publish_to_clients)
|
self.create_timer(0.5, self._publish_to_clients)
|
||||||
|
|
||||||
self.get_logger().info(f'CockpitBridge started on port {WS_PORT}')
|
self.get_logger().info(f'CockpitBridge started on port {WS_PORT}')
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Subscriber callbacks
|
||||||
|
# All callbacks acquire _lock before writing to _values.
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
def _failsafe_cb(self, msg: FailsafeStatus):
|
def _failsafe_cb(self, msg: FailsafeStatus):
|
||||||
"""Update failsafe state from FailsafeStatus message."""
|
"""Update failsafe state integer from FailsafeStatus message."""
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._values['rov-failsafe'] = int(msg.failsafe_state)
|
self._values['rov-failsafe'] = int(msg.failsafe_state)
|
||||||
|
|
||||||
@ -90,65 +169,143 @@ class CockpitBridge(Node):
|
|||||||
self._values['rov-heading'] = round(float(msg.data), 1)
|
self._values['rov-heading'] = round(float(msg.data), 1)
|
||||||
|
|
||||||
def _mission_cb(self, msg: MissionStatus):
|
def _mission_cb(self, msg: MissionStatus):
|
||||||
"""Update mission state and progress from MissionStatus message."""
|
"""Update mission state and progress percent from MissionStatus."""
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._values['rov-ms'] = int(msg.state)
|
self._values['rov-ms'] = int(msg.state)
|
||||||
self._values['rov-mp'] = int(msg.progress_percent)
|
self._values['rov-mp'] = int(msg.progress_percent)
|
||||||
|
|
||||||
def _publish_to_clients(self):
|
def _recording_cb(self, msg: Bool):
|
||||||
"""Timer callback - broadcast all current values to Cockpit clients."""
|
"""
|
||||||
|
Update recording active state from recording_manager.
|
||||||
|
Converts Bool to int (1/0) for Cockpit data lake compatibility —
|
||||||
|
the data lake stores numeric values and widgets read them as numbers.
|
||||||
|
"""
|
||||||
with self._lock:
|
with self._lock:
|
||||||
|
self._values['rov-recording'] = 1 if msg.data else 0
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# WebSocket broadcast — called from ROS2 timer at 2 Hz
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def _publish_to_clients(self):
|
||||||
|
"""
|
||||||
|
Timer callback — snapshot current values and schedule a broadcast
|
||||||
|
to all connected Cockpit WebSocket clients.
|
||||||
|
|
||||||
|
Builds the message list under the lock, then schedules the async
|
||||||
|
broadcast on the WebSocket event loop using run_coroutine_threadsafe.
|
||||||
|
This is the correct pattern for posting work from a non-asyncio
|
||||||
|
thread into a running asyncio event loop.
|
||||||
|
"""
|
||||||
|
with self._lock:
|
||||||
|
# Build one 'key=value' string per variable
|
||||||
messages = [f'{k}={v}' for k, v in self._values.items()]
|
messages = [f'{k}={v}' for k, v in self._values.items()]
|
||||||
|
|
||||||
|
# Skip broadcast if no clients are connected or loop not yet set
|
||||||
if not self._ws_clients or self._loop is None:
|
if not self._ws_clients or self._loop is None:
|
||||||
return
|
return
|
||||||
|
|
||||||
asyncio.run_coroutine_threadsafe(
|
asyncio.run_coroutine_threadsafe(
|
||||||
self._broadcast(messages), self._loop)
|
self._broadcast(messages), self._loop)
|
||||||
|
|
||||||
async def _broadcast(self, messages: list):
|
async def _broadcast(self, messages: list):
|
||||||
"""Send each variable as a separate WebSocket message to all clients."""
|
"""
|
||||||
|
Send each variable as a separate WebSocket message to all clients.
|
||||||
|
|
||||||
|
Sending variables as individual messages (not JSON or concatenated)
|
||||||
|
is required by Cockpit's Generic WebSocket protocol — it processes
|
||||||
|
one 'variable-name=value' pair per message.
|
||||||
|
|
||||||
|
Clients that fail to receive are silently removed from the set.
|
||||||
|
"""
|
||||||
disconnected = set()
|
disconnected = set()
|
||||||
for client in list(self._ws_clients):
|
for client in list(self._ws_clients):
|
||||||
try:
|
try:
|
||||||
for msg in messages:
|
for msg in messages:
|
||||||
await client.send(msg)
|
await client.send(msg)
|
||||||
except Exception:
|
except Exception:
|
||||||
|
# Connection dropped — remove from active client set
|
||||||
disconnected.add(client)
|
disconnected.add(client)
|
||||||
self._ws_clients -= disconnected
|
self._ws_clients -= disconnected
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# WebSocket connection handler
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
async def _ws_handler(self, websocket):
|
async def _ws_handler(self, websocket):
|
||||||
"""Handle an incoming Cockpit WebSocket connection."""
|
"""
|
||||||
self.get_logger().info(f'Cockpit connected: {websocket.remote_address}')
|
Handle an incoming Cockpit WebSocket connection.
|
||||||
self._ws_clients.add(websocket)
|
|
||||||
|
Adds the client to the broadcast set and waits until the connection
|
||||||
|
closes. Cockpit initiates the connection; we do not send anything
|
||||||
|
here — the broadcast timer handles all outbound messages.
|
||||||
|
"""
|
||||||
|
self.get_logger().info(
|
||||||
|
f'Cockpit connected: {websocket.remote_address}')
|
||||||
|
with self._lock:
|
||||||
|
self._ws_clients.add(websocket)
|
||||||
try:
|
try:
|
||||||
await websocket.wait_closed()
|
await websocket.wait_closed()
|
||||||
finally:
|
finally:
|
||||||
self._ws_clients.discard(websocket)
|
with self._lock:
|
||||||
self.get_logger().info(f'Cockpit disconnected: {websocket.remote_address}')
|
self._ws_clients.discard(websocket)
|
||||||
|
self.get_logger().info(
|
||||||
|
f'Cockpit disconnected: {websocket.remote_address}')
|
||||||
|
|
||||||
def set_event_loop(self, loop):
|
def set_event_loop(self, loop):
|
||||||
"""Store asyncio event loop reference for cross-thread scheduling."""
|
"""
|
||||||
|
Store the asyncio event loop reference.
|
||||||
|
Must be called before the WebSocket thread starts so that
|
||||||
|
_publish_to_clients can schedule coroutines on the correct loop.
|
||||||
|
"""
|
||||||
self._loop = loop
|
self._loop = loop
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# WebSocket server coroutine
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
async def ws_server(node: CockpitBridge):
|
async def ws_server(node: CockpitBridge):
|
||||||
"""Run the WebSocket server indefinitely."""
|
"""
|
||||||
|
Run the WebSocket server indefinitely on WS_PORT.
|
||||||
|
|
||||||
|
Cockpit connects to ws://192.168.1.101:9001 and remains connected
|
||||||
|
for the duration of the session. The server accepts any number of
|
||||||
|
concurrent connections (useful for multiple operator devices).
|
||||||
|
"""
|
||||||
async with websockets.serve(node._ws_handler, '0.0.0.0', WS_PORT):
|
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}')
|
node.get_logger().info(
|
||||||
|
f'WebSocket listening on 0.0.0.0:{WS_PORT}')
|
||||||
|
# Run indefinitely — the future is never resolved
|
||||||
await asyncio.get_event_loop().create_future()
|
await asyncio.get_event_loop().create_future()
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# Entry point
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
def main(args=None):
|
def main(args=None):
|
||||||
"""Entry point - runs ROS2 node and WebSocket server concurrently."""
|
"""
|
||||||
|
Entry point — runs the ROS2 node and WebSocket server concurrently.
|
||||||
|
|
||||||
|
The ROS2 executor (rclpy.spin) and the asyncio WebSocket server run
|
||||||
|
in separate threads. The asyncio loop is created explicitly so that
|
||||||
|
the node can post broadcast work to it from the ROS2 spin thread.
|
||||||
|
"""
|
||||||
rclpy.init(args=args)
|
rclpy.init(args=args)
|
||||||
node = CockpitBridge()
|
node = CockpitBridge()
|
||||||
|
|
||||||
|
# Create a dedicated asyncio event loop for the WebSocket server thread
|
||||||
loop = asyncio.new_event_loop()
|
loop = asyncio.new_event_loop()
|
||||||
node.set_event_loop(loop)
|
node.set_event_loop(loop)
|
||||||
|
|
||||||
def run_ws():
|
def run_ws():
|
||||||
|
"""Thread target: set the loop and run the WebSocket server."""
|
||||||
asyncio.set_event_loop(loop)
|
asyncio.set_event_loop(loop)
|
||||||
loop.run_until_complete(ws_server(node))
|
loop.run_until_complete(ws_server(node))
|
||||||
|
|
||||||
|
# Start the WebSocket server in a daemon thread so it exits
|
||||||
|
# automatically if the main thread (ROS2 spin) ends.
|
||||||
ws_thread = threading.Thread(target=run_ws, daemon=True)
|
ws_thread = threading.Thread(target=run_ws, daemon=True)
|
||||||
ws_thread.start()
|
ws_thread.start()
|
||||||
|
|
||||||
|
|||||||
@ -20,17 +20,23 @@ Breadcrumb system:
|
|||||||
Circular buffer of 1800 entries (1 hour of operation).
|
Circular buffer of 1800 entries (1 hour of operation).
|
||||||
Used by return-to-safe to reverse the entry path safely.
|
Used by return-to-safe to reverse the entry path safely.
|
||||||
|
|
||||||
|
Recording no-go gate (DIR-9):
|
||||||
|
Mission START is blocked if the recording_manager is not active.
|
||||||
|
This mirrors ArduPilot's pre-arm "Logging failed" check. The gate is
|
||||||
|
enforced in the START handler of _command_callback.
|
||||||
|
|
||||||
Topics subscribed:
|
Topics subscribed:
|
||||||
/odometry/filtered (nav_msgs/Odometry) — fused state from EKF
|
/odometry/filtered (nav_msgs/Odometry) — fused state from EKF
|
||||||
/rov/failsafe (rov_interfaces/FailsafeStatus) — safety override
|
/rov/failsafe (rov_interfaces/FailsafeStatus) — safety override
|
||||||
|
/rov/recording/active (std_msgs/Bool) — recorder no-go gate
|
||||||
|
|
||||||
Topics published:
|
Topics published:
|
||||||
/rov/cmd_vel (geometry_msgs/Twist) — velocity setpoints
|
/rov/cmd_vel (geometry_msgs/Twist) — velocity setpoints
|
||||||
/rov/mission/status (rov_interfaces/MissionStatus) — mission state
|
/rov/mission/status (rov_interfaces/MissionStatus) — mission state
|
||||||
/rov/mission/waypoint (rov_interfaces/InspectionWaypoint) — current target
|
/rov/mission/waypoint (rov_interfaces/InspectionWaypoint) — current target
|
||||||
|
|
||||||
Services provided:
|
Services provided:
|
||||||
/rov/mission/command (rov_interfaces/MissionCommand) — start/pause/abort/resume
|
/rov/mission/command (rov_interfaces/MissionCommand) — start/pause/abort/resume
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import math
|
import math
|
||||||
@ -43,7 +49,7 @@ import rclpy
|
|||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
from geometry_msgs.msg import Twist, Point
|
from geometry_msgs.msg import Twist, Point
|
||||||
from nav_msgs.msg import Odometry
|
from nav_msgs.msg import Odometry
|
||||||
from std_msgs.msg import Header
|
from std_msgs.msg import Bool, Header
|
||||||
from rov_interfaces.msg import MissionStatus, FailsafeStatus, InspectionWaypoint
|
from rov_interfaces.msg import MissionStatus, FailsafeStatus, InspectionWaypoint
|
||||||
from rov_interfaces.srv import MissionCommand
|
from rov_interfaces.srv import MissionCommand
|
||||||
|
|
||||||
@ -123,7 +129,8 @@ class BreadcrumbBuffer:
|
|||||||
class MissionExecutor(Node):
|
class MissionExecutor(Node):
|
||||||
"""
|
"""
|
||||||
Executes inspection missions from a waypoint sequence.
|
Executes inspection missions from a waypoint sequence.
|
||||||
Integrates with the failsafe monitor and maintains breadcrumb buffer.
|
Integrates with the failsafe monitor, breadcrumb buffer, and
|
||||||
|
the recording_manager no-go gate (DIR-9).
|
||||||
"""
|
"""
|
||||||
|
|
||||||
# Internal mission states
|
# Internal mission states
|
||||||
@ -177,6 +184,18 @@ class MissionExecutor(Node):
|
|||||||
self.last_failsafe_action = FailsafeStatus.ACTION_NONE
|
self.last_failsafe_action = FailsafeStatus.ACTION_NONE
|
||||||
self.last_assessment_state = FailsafeStatus.ASSESSMENT_GREEN
|
self.last_assessment_state = FailsafeStatus.ASSESSMENT_GREEN
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Recording no-go gate state (DIR-9)
|
||||||
|
# Initialised False so that if recording_manager has not yet
|
||||||
|
# published, mission start is blocked until a True message arrives.
|
||||||
|
# This prevents a race condition at stack startup where mission_executor
|
||||||
|
# might receive a START command before the recorder has confirmed active.
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# True when recording_manager has confirmed the recorder is healthy.
|
||||||
|
# Set by _recording_cb from /rov/recording/active subscription.
|
||||||
|
self._recording_active: bool = False
|
||||||
|
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
# Breadcrumb buffer
|
# Breadcrumb buffer
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
@ -195,24 +214,36 @@ class MissionExecutor(Node):
|
|||||||
# Subscribers
|
# Subscribers
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# Fused odometry from the EKF — provides position and heading
|
||||||
self.create_subscription(
|
self.create_subscription(
|
||||||
Odometry, '/odometry/filtered',
|
Odometry, '/odometry/filtered',
|
||||||
self._state_callback, 10)
|
self._state_callback, 10)
|
||||||
|
|
||||||
|
# Failsafe status from the failsafe monitor — drives mission interrupts
|
||||||
self.create_subscription(
|
self.create_subscription(
|
||||||
FailsafeStatus, '/rov/failsafe',
|
FailsafeStatus, '/rov/failsafe',
|
||||||
self._failsafe_callback, 10)
|
self._failsafe_callback, 10)
|
||||||
|
|
||||||
|
# Recording active flag from recording_manager (DIR-9 no-go gate).
|
||||||
|
# The gate is enforced in the START command handler; this subscription
|
||||||
|
# keeps _recording_active up to date at all times.
|
||||||
|
self.create_subscription(
|
||||||
|
Bool, '/rov/recording/active',
|
||||||
|
self._recording_cb, 10)
|
||||||
|
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
# Publishers
|
# Publishers
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# Velocity setpoints to the motion controller
|
||||||
self.cmd_pub = self.create_publisher(
|
self.cmd_pub = self.create_publisher(
|
||||||
Twist, '/rov/cmd_vel', 10)
|
Twist, '/rov/cmd_vel', 10)
|
||||||
|
|
||||||
|
# Mission state broadcast — consumed by cockpit_bridge and Foxglove
|
||||||
self.status_pub = self.create_publisher(
|
self.status_pub = self.create_publisher(
|
||||||
MissionStatus, '/rov/mission/status', 10)
|
MissionStatus, '/rov/mission/status', 10)
|
||||||
|
|
||||||
|
# Current waypoint target — consumed by motion controller and Foxglove
|
||||||
self.waypoint_pub = self.create_publisher(
|
self.waypoint_pub = self.create_publisher(
|
||||||
InspectionWaypoint, '/rov/mission/waypoint', 10)
|
InspectionWaypoint, '/rov/mission/waypoint', 10)
|
||||||
|
|
||||||
@ -220,6 +251,7 @@ class MissionExecutor(Node):
|
|||||||
# Service
|
# Service
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# Mission command service: START / LOAD / PAUSE / RESUME / ABORT
|
||||||
self.create_service(
|
self.create_service(
|
||||||
MissionCommand, '/rov/mission/command',
|
MissionCommand, '/rov/mission/command',
|
||||||
self._command_callback)
|
self._command_callback)
|
||||||
@ -228,10 +260,10 @@ class MissionExecutor(Node):
|
|||||||
# Timers
|
# Timers
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
# Mission loop at 10 Hz
|
# Mission execution loop at 10 Hz — drives waypoint sequencing
|
||||||
self.create_timer(0.1, self._mission_loop)
|
self.create_timer(0.1, self._mission_loop)
|
||||||
|
|
||||||
# Status publisher at 2 Hz
|
# Status publish at 2 Hz — drives cockpit_bridge and Foxglove updates
|
||||||
self.create_timer(0.5, self._publish_status)
|
self.create_timer(0.5, self._publish_status)
|
||||||
|
|
||||||
self.get_logger().info('MissionExecutor started')
|
self.get_logger().info('MissionExecutor started')
|
||||||
@ -306,6 +338,27 @@ class MissionExecutor(Node):
|
|||||||
'Failsafe cleared (GREEN) — resuming mission')
|
'Failsafe cleared (GREEN) — resuming mission')
|
||||||
self.state = self.STATE_RUNNING
|
self.state = self.STATE_RUNNING
|
||||||
|
|
||||||
|
def _recording_cb(self, msg: Bool):
|
||||||
|
"""
|
||||||
|
Update the recording active state from recording_manager.
|
||||||
|
|
||||||
|
This is the receiver side of the DIR-9 no-go gate. The gate itself
|
||||||
|
is enforced in _command_callback when a START command arrives.
|
||||||
|
Logging is intentionally sparse — state changes only — to avoid
|
||||||
|
spamming the journal at 1 Hz.
|
||||||
|
"""
|
||||||
|
was_active = self._recording_active
|
||||||
|
self._recording_active = msg.data
|
||||||
|
|
||||||
|
# Log state transitions only — not every heartbeat
|
||||||
|
if self._recording_active and not was_active:
|
||||||
|
self.get_logger().info(
|
||||||
|
'Recording active — mission START no-go gate cleared')
|
||||||
|
elif not self._recording_active and was_active:
|
||||||
|
self.get_logger().warn(
|
||||||
|
'Recording INACTIVE — mission START no-go gate ENGAGED; '
|
||||||
|
'check recording_manager')
|
||||||
|
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
# Service callback — mission commands
|
# Service callback — mission commands
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
@ -314,6 +367,10 @@ class MissionExecutor(Node):
|
|||||||
"""
|
"""
|
||||||
Handle mission commands: START, PAUSE, RESUME, ABORT, LOAD.
|
Handle mission commands: START, PAUSE, RESUME, ABORT, LOAD.
|
||||||
LOAD accepts a YAML waypoint file path via parameters[0].
|
LOAD accepts a YAML waypoint file path via parameters[0].
|
||||||
|
|
||||||
|
START enforces the recording no-go gate (DIR-9): if the
|
||||||
|
recording_manager has not confirmed active, START is rejected
|
||||||
|
with a clear error message so the operator knows the cause.
|
||||||
"""
|
"""
|
||||||
cmd = request.command.upper()
|
cmd = request.command.upper()
|
||||||
|
|
||||||
@ -328,6 +385,24 @@ class MissionExecutor(Node):
|
|||||||
response.message = 'Cannot START — no waypoints loaded'
|
response.message = 'Cannot START — no waypoints loaded'
|
||||||
return response
|
return response
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Recording no-go gate (DIR-9)
|
||||||
|
# Block mission start if the recorder is not confirmed active.
|
||||||
|
# This is the hard gate — equivalent to ArduPilot's pre-arm
|
||||||
|
# "Logging failed" check. The operator must resolve the recording
|
||||||
|
# fault (check recording_manager journal) before starting a mission.
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
if not self._recording_active:
|
||||||
|
response.success = False
|
||||||
|
response.message = (
|
||||||
|
'Cannot START — recording not active (DIR-9 no-go gate). '
|
||||||
|
'Check recording_manager: '
|
||||||
|
'journalctl -u argonaut.service | grep recording_manager'
|
||||||
|
)
|
||||||
|
self.get_logger().error(
|
||||||
|
'Mission START blocked — recording_manager not active (DIR-9)')
|
||||||
|
return response
|
||||||
|
|
||||||
# Record entry point at mission start
|
# Record entry point at mission start
|
||||||
self.entry_point = (
|
self.entry_point = (
|
||||||
self.current_position if self.current_position is not None
|
self.current_position if self.current_position is not None
|
||||||
@ -597,13 +672,13 @@ class MissionExecutor(Node):
|
|||||||
self.STATE_ABORTED: MissionStatus.STATE_ABORTED,
|
self.STATE_ABORTED: MissionStatus.STATE_ABORTED,
|
||||||
}.get(self.state, MissionStatus.STATE_IDLE)
|
}.get(self.state, MissionStatus.STATE_IDLE)
|
||||||
|
|
||||||
# Progress
|
# Progress as 0-100 percentage
|
||||||
total = len(self.waypoints)
|
total = len(self.waypoints)
|
||||||
msg.progress_percent = (
|
msg.progress_percent = (
|
||||||
(self.current_wp_index / total * 100.0) if total > 0 else 0.0
|
(self.current_wp_index / total * 100.0) if total > 0 else 0.0
|
||||||
)
|
)
|
||||||
|
|
||||||
# Current task description
|
# Human-readable task description for operator display
|
||||||
if self.state == self.STATE_RUNNING and total > 0:
|
if self.state == self.STATE_RUNNING and total > 0:
|
||||||
wp_id = (self.waypoints[self.current_wp_index].waypoint_id
|
wp_id = (self.waypoints[self.current_wp_index].waypoint_id
|
||||||
if self.current_wp_index < total else 'complete')
|
if self.current_wp_index < total else 'complete')
|
||||||
|
|||||||
402
src/rov_mission/rov_mission/recording_manager.py
Executable file
402
src/rov_mission/rov_mission/recording_manager.py
Executable file
@ -0,0 +1,402 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
recording_manager.py — Argonaut 3 continuous recording manager.
|
||||||
|
|
||||||
|
Manages a rosbag2 MCAP recorder as a subprocess. Recording starts when
|
||||||
|
the node starts and runs continuously, independent of mission state.
|
||||||
|
|
||||||
|
Design intent (DIR-9):
|
||||||
|
- Recording is a dedicated, always-on process, separate from
|
||||||
|
mission_executor.
|
||||||
|
- Recorder-active is a hard no-go gate for mission start — equivalent
|
||||||
|
to ArduPilot's pre-arm "Logging failed" check.
|
||||||
|
- The recorder is never gated on mission state. It runs from node
|
||||||
|
startup through the entire dive and beyond.
|
||||||
|
|
||||||
|
Behaviour:
|
||||||
|
- Starts ros2 bag record on node startup (all topics, MCAP, zstd).
|
||||||
|
- Health check at 1 Hz; restarts subprocess after RESTART_DELAY_S if
|
||||||
|
it exits unexpectedly.
|
||||||
|
- Publishes /rov/recording/active (Bool) — consumed by mission_executor
|
||||||
|
as a hard no-go gate.
|
||||||
|
- Publishes /rov/recording/bag_dir (String) — diagnostic visibility.
|
||||||
|
- Retention policy: deletes oldest dive_* bag dirs when /data free
|
||||||
|
space falls below FREE_SPACE_THRESHOLD_BYTES (10 GB).
|
||||||
|
- Clean SIGINT shutdown on node destroy so rosbag2 can finalise the
|
||||||
|
MCAP index before the process exits.
|
||||||
|
|
||||||
|
Bags written to: /data/bags/dive_YYYY_MM_DD-HH_MM_SS/
|
||||||
|
"""
|
||||||
|
|
||||||
|
import os
|
||||||
|
import signal
|
||||||
|
import shutil
|
||||||
|
import subprocess
|
||||||
|
import datetime
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from std_msgs.msg import Bool, String
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# Constants
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
|
# Root directory on the NVMe data partition where bags are stored.
|
||||||
|
# Must exist before the node starts — created by node startup if missing.
|
||||||
|
BAG_ROOT = '/data/bags'
|
||||||
|
|
||||||
|
# Free-space floor on /data. When free space drops below this, the oldest
|
||||||
|
# dive_* bag directory is deleted. 10 GB gives comfortable headroom for an
|
||||||
|
# ongoing recording session while the cleanup runs.
|
||||||
|
FREE_SPACE_THRESHOLD_BYTES = 10 * 1024 * 1024 * 1024 # 10 GB
|
||||||
|
|
||||||
|
# rosbag2 splits to a new file segment at this size.
|
||||||
|
# 500 MB keeps individual MCAP files manageable for transfer and indexing.
|
||||||
|
# Multiple segments are written into the same output directory.
|
||||||
|
MAX_BAG_SIZE_BYTES = 500 * 1024 * 1024 # 500 MB
|
||||||
|
|
||||||
|
# Seconds to wait after a subprocess exit before attempting a restart.
|
||||||
|
# Prevents rapid crash-loop if the recorder fails repeatedly.
|
||||||
|
RESTART_DELAY_S = 5.0
|
||||||
|
|
||||||
|
# zstd gives better compression ratio than lz4 at acceptable CPU cost on RPi5.
|
||||||
|
COMPRESSION_FORMAT = 'zstd'
|
||||||
|
|
||||||
|
# File-level compression: compress each MCAP segment file as a whole.
|
||||||
|
# Preferred over message-level ('message') as it gives better ratio for
|
||||||
|
# sequential reads and keeps the MCAP index intact for Foxglove playback.
|
||||||
|
COMPRESSION_MODE = 'file'
|
||||||
|
|
||||||
|
|
||||||
|
class RecordingManager(Node):
|
||||||
|
"""
|
||||||
|
Manages continuous MCAP bag recording as a guarded subprocess.
|
||||||
|
|
||||||
|
Recording starts on node startup and runs independently of mission
|
||||||
|
state. Publishes /rov/recording/active which mission_executor checks
|
||||||
|
as a hard no-go gate before allowing any mission to start (DIR-9).
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('recording_manager')
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Internal state
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# The active ros2 bag record subprocess. None when not running.
|
||||||
|
self._proc: Optional[subprocess.Popen] = None
|
||||||
|
|
||||||
|
# Absolute path to the directory being written by the current
|
||||||
|
# subprocess. Used to protect the current bag from retention deletion
|
||||||
|
# and to populate the /rov/recording/bag_dir topic.
|
||||||
|
self._bag_dir: str = ''
|
||||||
|
|
||||||
|
# Whether the recorder subprocess is currently healthy and running.
|
||||||
|
# This is the value published on /rov/recording/active.
|
||||||
|
self._active: bool = False
|
||||||
|
|
||||||
|
# Wall-clock ROS time (seconds) when the subprocess last exited.
|
||||||
|
# Used to enforce RESTART_DELAY_S before attempting a restart.
|
||||||
|
self._exit_time: float = 0.0
|
||||||
|
|
||||||
|
# True when we are waiting out the restart delay after an exit.
|
||||||
|
self._restart_pending: bool = False
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Publishers
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# /rov/recording/active (Bool)
|
||||||
|
# True → recorder subprocess is alive and writing.
|
||||||
|
# False → recorder is down; mission start will be blocked.
|
||||||
|
self._active_pub = self.create_publisher(
|
||||||
|
Bool, '/rov/recording/active', 10)
|
||||||
|
|
||||||
|
# /rov/recording/bag_dir (String)
|
||||||
|
# Path to the current bag output directory. Empty when not recording.
|
||||||
|
# Surfaced in Foxglove and cockpit_bridge for operator awareness.
|
||||||
|
self._dir_pub = self.create_publisher(
|
||||||
|
String, '/rov/recording/bag_dir', 10)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Timers
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# Health check and status publish at 1 Hz. Low frequency is
|
||||||
|
# sufficient — subprocess failures are rare and a 1-second detection
|
||||||
|
# lag is acceptable.
|
||||||
|
self.create_timer(1.0, self._health_tick)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Startup
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
# Ensure /data/bags/ exists on the NVMe data partition.
|
||||||
|
# If /data is not mounted this will create the directory on the SD
|
||||||
|
# card instead — not catastrophic but bags will fill the OS drive.
|
||||||
|
os.makedirs(BAG_ROOT, exist_ok=True)
|
||||||
|
self.get_logger().info(
|
||||||
|
f'RecordingManager started — bag root: {BAG_ROOT}')
|
||||||
|
|
||||||
|
# Start the recorder immediately so it is active before any
|
||||||
|
# mission_executor START command can arrive.
|
||||||
|
self._start_recorder()
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Recorder subprocess management
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def _make_bag_dir(self) -> str:
|
||||||
|
"""
|
||||||
|
Return a new timestamped output path under BAG_ROOT.
|
||||||
|
|
||||||
|
Format: /data/bags/dive_YYYY_MM_DD-HH_MM_SS
|
||||||
|
|
||||||
|
The dive_ prefix is important — the retention policy only deletes
|
||||||
|
directories with this prefix so other content on /data is protected.
|
||||||
|
"""
|
||||||
|
ts = datetime.datetime.now().strftime('%Y_%m_%d-%H_%M_%S')
|
||||||
|
return os.path.join(BAG_ROOT, f'dive_{ts}')
|
||||||
|
|
||||||
|
def _start_recorder(self):
|
||||||
|
"""
|
||||||
|
Launch ros2 bag record as a subprocess.
|
||||||
|
|
||||||
|
Records all topics to a fresh timestamped directory in MCAP format
|
||||||
|
with zstd file-level compression and 500 MB segment splitting.
|
||||||
|
|
||||||
|
The retention policy is applied before each start so we never begin
|
||||||
|
a new recording into a full partition.
|
||||||
|
"""
|
||||||
|
# Enforce minimum free space before starting a new bag directory.
|
||||||
|
self._apply_retention_policy()
|
||||||
|
|
||||||
|
self._bag_dir = self._make_bag_dir()
|
||||||
|
|
||||||
|
# Build the ros2 bag record command.
|
||||||
|
# --all : capture every topic currently available
|
||||||
|
# --storage mcap : MCAP container (Foxglove native, indexed)
|
||||||
|
# --compression-* : zstd file-level compression
|
||||||
|
# --max-bag-size : split segments at 500 MB; all go into _bag_dir
|
||||||
|
# --output : destination directory (created by rosbag2)
|
||||||
|
cmd = [
|
||||||
|
'ros2', 'bag', 'record',
|
||||||
|
'--all',
|
||||||
|
'--storage', 'mcap',
|
||||||
|
'--compression-mode', COMPRESSION_MODE,
|
||||||
|
'--compression-format', COMPRESSION_FORMAT,
|
||||||
|
'--max-bag-size', str(MAX_BAG_SIZE_BYTES),
|
||||||
|
'--output', self._bag_dir,
|
||||||
|
]
|
||||||
|
|
||||||
|
try:
|
||||||
|
# Inherit stdout/stderr so ros2 bag record log output appears
|
||||||
|
# in the argonaut.service journal alongside all other nodes.
|
||||||
|
self._proc = subprocess.Popen(cmd)
|
||||||
|
self._active = True
|
||||||
|
self._restart_pending = False
|
||||||
|
self.get_logger().info(
|
||||||
|
f'Recorder started — PID {self._proc.pid}, '
|
||||||
|
f'output: {self._bag_dir}')
|
||||||
|
except Exception as exc:
|
||||||
|
# Subprocess launch itself failed (e.g. ros2 not on PATH).
|
||||||
|
# Mark inactive, schedule a retry after RESTART_DELAY_S.
|
||||||
|
self._active = False
|
||||||
|
self._restart_pending = True
|
||||||
|
self._exit_time = self.get_clock().now().nanoseconds / 1e9
|
||||||
|
self.get_logger().error(f'Recorder failed to start: {exc}')
|
||||||
|
|
||||||
|
def _stop_recorder(self):
|
||||||
|
"""
|
||||||
|
Send SIGINT to the recorder for a clean, indexed shutdown.
|
||||||
|
|
||||||
|
rosbag2 finalises the MCAP index on SIGINT. SIGKILL leaves the bag
|
||||||
|
in an unindexed state which prevents Foxglove from opening it.
|
||||||
|
Falls back to SIGKILL after a 5-second timeout.
|
||||||
|
"""
|
||||||
|
if self._proc is None:
|
||||||
|
return # Nothing to stop
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.get_logger().info(
|
||||||
|
f'Sending SIGINT to recorder (PID {self._proc.pid})')
|
||||||
|
self._proc.send_signal(signal.SIGINT)
|
||||||
|
# Allow up to 5 seconds for rosbag2 to flush and close cleanly.
|
||||||
|
self._proc.wait(timeout=5.0)
|
||||||
|
self.get_logger().info('Recorder stopped cleanly')
|
||||||
|
except subprocess.TimeoutExpired:
|
||||||
|
# Did not exit in time — force kill to unblock shutdown.
|
||||||
|
self.get_logger().warn(
|
||||||
|
'Recorder did not stop within 5s — sending SIGKILL')
|
||||||
|
self._proc.kill()
|
||||||
|
self._proc.wait()
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().error(f'Error stopping recorder: {exc}')
|
||||||
|
finally:
|
||||||
|
# Always clear the process reference regardless of how it ended.
|
||||||
|
self._proc = None
|
||||||
|
self._active = False
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Health check timer — 1 Hz
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def _health_tick(self):
|
||||||
|
"""
|
||||||
|
Called at 1 Hz. Checks subprocess health, handles restart logic,
|
||||||
|
applies retention policy, and publishes status.
|
||||||
|
|
||||||
|
State machine:
|
||||||
|
subprocess running and healthy → _active = True, publish True
|
||||||
|
subprocess exited unexpectedly → mark pending, start delay timer
|
||||||
|
delay elapsed → restart subprocess
|
||||||
|
"""
|
||||||
|
now_s = self.get_clock().now().nanoseconds / 1e9
|
||||||
|
|
||||||
|
if self._proc is not None:
|
||||||
|
# Non-blocking poll: returns None if still running, else exit code.
|
||||||
|
ret = self._proc.poll()
|
||||||
|
if ret is not None:
|
||||||
|
# Subprocess has exited — log it and enter restart-pending state.
|
||||||
|
self.get_logger().error(
|
||||||
|
f'Recorder exited unexpectedly '
|
||||||
|
f'(PID {self._proc.pid}, code {ret}) — '
|
||||||
|
f'restarting in {RESTART_DELAY_S}s')
|
||||||
|
self._proc = None
|
||||||
|
self._active = False
|
||||||
|
self._restart_pending = True
|
||||||
|
self._exit_time = now_s
|
||||||
|
elif self._restart_pending:
|
||||||
|
# No subprocess running; check if the restart delay has elapsed.
|
||||||
|
if now_s - self._exit_time >= RESTART_DELAY_S:
|
||||||
|
self.get_logger().info(
|
||||||
|
f'Restart delay ({RESTART_DELAY_S}s) elapsed — '
|
||||||
|
'restarting recorder')
|
||||||
|
self._start_recorder()
|
||||||
|
|
||||||
|
# Apply retention on every tick. The disk_usage stat call is cheap
|
||||||
|
# and the full deletion logic only runs when space is actually low.
|
||||||
|
self._apply_retention_policy()
|
||||||
|
|
||||||
|
# Always publish current status so subscribers get fresh data.
|
||||||
|
self._publish_status()
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Status publishing
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def _publish_status(self):
|
||||||
|
"""
|
||||||
|
Publish /rov/recording/active and /rov/recording/bag_dir at 1 Hz.
|
||||||
|
|
||||||
|
/rov/recording/active is the primary signal consumed by
|
||||||
|
mission_executor as the recording no-go gate.
|
||||||
|
"""
|
||||||
|
# Active flag — the no-go gate signal
|
||||||
|
active_msg = Bool()
|
||||||
|
active_msg.data = self._active
|
||||||
|
self._active_pub.publish(active_msg)
|
||||||
|
|
||||||
|
# Current bag directory path — empty string when not recording
|
||||||
|
dir_msg = String()
|
||||||
|
dir_msg.data = self._bag_dir if self._active else ''
|
||||||
|
self._dir_pub.publish(dir_msg)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Retention policy
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def _apply_retention_policy(self):
|
||||||
|
"""
|
||||||
|
Delete the oldest dive_* bag directory when free space on BAG_ROOT
|
||||||
|
falls below FREE_SPACE_THRESHOLD_BYTES (10 GB).
|
||||||
|
|
||||||
|
Repeats deletion until either the threshold is met or no more
|
||||||
|
deletable bags remain (the currently-active bag is never deleted).
|
||||||
|
|
||||||
|
Only directories whose names start with 'dive_' are candidates
|
||||||
|
for deletion — all other content under /data is protected.
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
usage = shutil.disk_usage(BAG_ROOT)
|
||||||
|
if usage.free >= FREE_SPACE_THRESHOLD_BYTES:
|
||||||
|
return # Sufficient free space — nothing to do
|
||||||
|
|
||||||
|
# Collect all dive_* bag directories.
|
||||||
|
candidates = [
|
||||||
|
os.path.join(BAG_ROOT, entry)
|
||||||
|
for entry in os.listdir(BAG_ROOT)
|
||||||
|
if entry.startswith('dive_')
|
||||||
|
and os.path.isdir(os.path.join(BAG_ROOT, entry))
|
||||||
|
]
|
||||||
|
|
||||||
|
# Sort oldest-first by directory name. Since names are
|
||||||
|
# timestamped as dive_YYYY_MM_DD-HH_MM_SS, lexicographic
|
||||||
|
# order == chronological order. This is more reliable than
|
||||||
|
# mtime which changes as files are written into a directory.
|
||||||
|
candidates.sort(key=lambda p: os.path.basename(p))
|
||||||
|
|
||||||
|
for bag_path in candidates:
|
||||||
|
# Never delete the bag directory currently being written.
|
||||||
|
if bag_path == self._bag_dir:
|
||||||
|
continue
|
||||||
|
|
||||||
|
self.get_logger().warn(
|
||||||
|
f'Low disk space — deleting oldest bag: {bag_path}')
|
||||||
|
shutil.rmtree(bag_path, ignore_errors=True)
|
||||||
|
|
||||||
|
# Re-check after each deletion; stop as soon as we have
|
||||||
|
# enough free space to avoid unnecessary deletions.
|
||||||
|
if shutil.disk_usage(BAG_ROOT).free >= FREE_SPACE_THRESHOLD_BYTES:
|
||||||
|
break
|
||||||
|
|
||||||
|
except Exception as exc:
|
||||||
|
# Log but do not raise — a retention failure should not crash
|
||||||
|
# the recording manager or block the health tick.
|
||||||
|
self.get_logger().error(f'Retention policy error: {exc}')
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Shutdown
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def destroy_node(self):
|
||||||
|
"""
|
||||||
|
Override destroy_node to stop the recorder cleanly before the
|
||||||
|
node is torn down.
|
||||||
|
|
||||||
|
Called by the ROS2 executor on shutdown. Sends SIGINT to the
|
||||||
|
recorder subprocess so rosbag2 can finalise the MCAP index.
|
||||||
|
Without this the last bag segment would be left unindexed and
|
||||||
|
would not open in Foxglove.
|
||||||
|
"""
|
||||||
|
self.get_logger().info(
|
||||||
|
'RecordingManager shutting down — finalising recorder')
|
||||||
|
self._stop_recorder()
|
||||||
|
super().destroy_node()
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# Entry point
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
"""ROS2 node entry point — called by ros2 run and argonaut.service."""
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = RecordingManager()
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
try:
|
||||||
|
rclpy.shutdown()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
@ -18,8 +18,15 @@ setup(
|
|||||||
zip_safe=True,
|
zip_safe=True,
|
||||||
entry_points={
|
entry_points={
|
||||||
'console_scripts': [
|
'console_scripts': [
|
||||||
|
# Mission state machine — waypoint sequencing, failsafe integration
|
||||||
'mission_executor = rov_mission.mission_executor:main',
|
'mission_executor = rov_mission.mission_executor:main',
|
||||||
|
|
||||||
|
# Cockpit data bridge — ROS2 topics -> WebSocket -> Cockpit data lake
|
||||||
'cockpit_bridge = rov_mission.cockpit_bridge:main',
|
'cockpit_bridge = rov_mission.cockpit_bridge:main',
|
||||||
|
|
||||||
|
# Continuous MCAP recorder — DIR-9 foundational node
|
||||||
|
# Must be active before any mission can start (no-go gate)
|
||||||
|
'recording_manager = rov_mission.recording_manager:main',
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user