From 5d75a9d30dfd03a477dc94b8da96a2c937fc0088 Mon Sep 17 00:00:00 2001 From: Grant du Toit Date: Sat, 4 Jul 2026 07:36:45 +0000 Subject: [PATCH] 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 --- .../launch/foxglove_mcap.launch.py | 84 ++-- src/rov_mission/rov_mission/cockpit_bridge.py | 235 ++++++++-- .../rov_mission/mission_executor.py | 97 ++++- .../rov_mission/recording_manager.py | 402 ++++++++++++++++++ src/rov_mission/setup.py | 7 + 5 files changed, 735 insertions(+), 90 deletions(-) create mode 100755 src/rov_mission/rov_mission/recording_manager.py diff --git a/src/rov_bringup/launch/foxglove_mcap.launch.py b/src/rov_bringup/launch/foxglove_mcap.launch.py index bdc889f..92958f5 100644 --- a/src/rov_bringup/launch/foxglove_mcap.launch.py +++ b/src/rov_bringup/launch/foxglove_mcap.launch.py @@ -1,75 +1,79 @@ """ 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. Standalone: ros2 launch rov_bringup foxglove_mcap.launch.py - ros2 launch rov_bringup foxglove_mcap.launch.py record:=false Foxglove connection from Edge PC: Foxglove Studio -> Open Connection -> Foxglove WebSocket -> ws://rov-brain.local:8765 -Storage: - Bags written to /data/bags/ on NVMe. Create before first launch: +Recording: + Managed by recording_manager node (rov_mission package). + Bags written to /data/bags/dive_/ on NVMe. + Create /data/bags before first launch if it does not exist: 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.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): - bag_output_arg = DeclareLaunchArgument( - 'bag_output_dir', - default_value='/data/bags/dive_' + datetime.datetime.now().strftime('%Y_%m_%d-%H_%M_%S'), - description='Base path for MCAP bag. Timestamp appended automatically.', - ) - - 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 + # Exposes all ROS2 topics over a WebSocket for Foxglove Studio. + # Visualisation only — does NOT record MCAP (DIR-9). + # Connect Foxglove Studio to: ws://rov-brain.local:8765 + # ----------------------------------------------------------------------- foxglove_bridge_node = Node( package='foxglove_bridge', executable='foxglove_bridge', name='foxglove_bridge', parameters=[{ + # Listen on all interfaces — required when connecting over tether '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, - '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', ) - # 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/ - mcap_recorder = ExecuteProcess( - cmd=[ - 'ros2', 'bag', 'record', - '--all', - '--storage', 'mcap', - '--output', LaunchConfiguration('bag_output_dir'), - '--max-bag-size', '0', # no size limit — one bag per dive - ], + # ----------------------------------------------------------------------- + # Recording Manager + # Dedicated continuous recorder node (DIR-9). + # Manages ros2 bag record subprocess internally. + # Starts recording on node startup — before any mission can begin. + # Publishes /rov/recording/active as the mission start no-go gate. + # ----------------------------------------------------------------------- + recording_manager_node = Node( + package='rov_mission', + executable='recording_manager', + name='recording_manager', output='screen', - condition=IfCondition(LaunchConfiguration('record')), ) return LaunchDescription([ - bag_output_arg, - record_arg, - LogInfo(msg='foxglove_bridge: connect Foxglove Studio to ws://:8765'), foxglove_bridge_node, - mcap_recorder, + recording_manager_node, ]) diff --git a/src/rov_mission/rov_mission/cockpit_bridge.py b/src/rov_mission/rov_mission/cockpit_bridge.py index e61202c..699ace8 100644 --- a/src/rov_mission/rov_mission/cockpit_bridge.py +++ b/src/rov_mission/rov_mission/cockpit_bridge.py @@ -7,8 +7,15 @@ 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 + +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 @@ -16,15 +23,16 @@ import threading import rclpy from rclpy.node import Node 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 rov_interfaces.msg import FailsafeStatus, MissionStatus import websockets -# Port Cockpit connects to +# Port Cockpit connects to as a Generic WebSocket client 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( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT @@ -32,45 +40,116 @@ best_effort_qos = QoSProfile( 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): 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 = { - 'rov-failsafe': -1, - 'rov-depth': 0.0, - 'rov-voltage': 0.0, - 'rov-heading': 0.0, - 'rov-ms': -1, - 'rov-mp': 0, + # Failsafe state integer: 0=GREEN, 1=AMBER, 2=RED + # -1 initial value indicates no data received yet + 'rov-failsafe': -1, + + # Depth in metres below surface (positive = deeper) + '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() + + # Protects _values and _ws_clients access across the ROS2 spin + # thread and the asyncio WebSocket thread. 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 + # ------------------------------------------------------------------ # 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.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): - """Update failsafe state from FailsafeStatus message.""" + """Update failsafe state integer from FailsafeStatus message.""" with self._lock: self._values['rov-failsafe'] = int(msg.failsafe_state) @@ -90,65 +169,143 @@ class CockpitBridge(Node): self._values['rov-heading'] = round(float(msg.data), 1) 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: 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.""" + def _recording_cb(self, msg: Bool): + """ + 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: + 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()] + + # Skip broadcast if no clients are connected or loop not yet set 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.""" + """ + 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() for client in list(self._ws_clients): try: for msg in messages: await client.send(msg) except Exception: + # Connection dropped — remove from active client set disconnected.add(client) self._ws_clients -= disconnected + # ------------------------------------------------------------------ + # WebSocket connection handler + # ------------------------------------------------------------------ + 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) + """ + Handle an incoming Cockpit WebSocket connection. + + 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: await websocket.wait_closed() finally: - self._ws_clients.discard(websocket) - self.get_logger().info(f'Cockpit disconnected: {websocket.remote_address}') + with self._lock: + 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.""" + """ + 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 +# --------------------------------------------------------------------------- +# WebSocket server coroutine +# --------------------------------------------------------------------------- + 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): - 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() +# --------------------------------------------------------------------------- +# Entry point +# --------------------------------------------------------------------------- + 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) node = CockpitBridge() + # Create a dedicated asyncio event loop for the WebSocket server thread loop = asyncio.new_event_loop() node.set_event_loop(loop) def run_ws(): + """Thread target: set the loop and run the WebSocket server.""" asyncio.set_event_loop(loop) 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.start() diff --git a/src/rov_mission/rov_mission/mission_executor.py b/src/rov_mission/rov_mission/mission_executor.py index 8ba076a..efd845f 100644 --- a/src/rov_mission/rov_mission/mission_executor.py +++ b/src/rov_mission/rov_mission/mission_executor.py @@ -20,17 +20,23 @@ Breadcrumb system: Circular buffer of 1800 entries (1 hour of operation). 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: - /odometry/filtered (nav_msgs/Odometry) — fused state from EKF - /rov/failsafe (rov_interfaces/FailsafeStatus) — safety override + /odometry/filtered (nav_msgs/Odometry) — fused state from EKF + /rov/failsafe (rov_interfaces/FailsafeStatus) — safety override + /rov/recording/active (std_msgs/Bool) — recorder no-go gate Topics published: - /rov/cmd_vel (geometry_msgs/Twist) — velocity setpoints - /rov/mission/status (rov_interfaces/MissionStatus) — mission state + /rov/cmd_vel (geometry_msgs/Twist) — velocity setpoints + /rov/mission/status (rov_interfaces/MissionStatus) — mission state /rov/mission/waypoint (rov_interfaces/InspectionWaypoint) — current target Services provided: - /rov/mission/command (rov_interfaces/MissionCommand) — start/pause/abort/resume + /rov/mission/command (rov_interfaces/MissionCommand) — start/pause/abort/resume """ import math @@ -43,7 +49,7 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist, Point 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.srv import MissionCommand @@ -123,7 +129,8 @@ class BreadcrumbBuffer: class MissionExecutor(Node): """ 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 @@ -177,6 +184,18 @@ class MissionExecutor(Node): self.last_failsafe_action = FailsafeStatus.ACTION_NONE 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 # ------------------------------------------------------------------ @@ -195,24 +214,36 @@ class MissionExecutor(Node): # Subscribers # ------------------------------------------------------------------ + # Fused odometry from the EKF — provides position and heading self.create_subscription( Odometry, '/odometry/filtered', self._state_callback, 10) + # Failsafe status from the failsafe monitor — drives mission interrupts self.create_subscription( FailsafeStatus, '/rov/failsafe', 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 # ------------------------------------------------------------------ + # Velocity setpoints to the motion controller self.cmd_pub = self.create_publisher( Twist, '/rov/cmd_vel', 10) + # Mission state broadcast — consumed by cockpit_bridge and Foxglove self.status_pub = self.create_publisher( MissionStatus, '/rov/mission/status', 10) + # Current waypoint target — consumed by motion controller and Foxglove self.waypoint_pub = self.create_publisher( InspectionWaypoint, '/rov/mission/waypoint', 10) @@ -220,6 +251,7 @@ class MissionExecutor(Node): # Service # ------------------------------------------------------------------ + # Mission command service: START / LOAD / PAUSE / RESUME / ABORT self.create_service( MissionCommand, '/rov/mission/command', self._command_callback) @@ -228,10 +260,10 @@ class MissionExecutor(Node): # Timers # ------------------------------------------------------------------ - # Mission loop at 10 Hz + # Mission execution loop at 10 Hz — drives waypoint sequencing 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.get_logger().info('MissionExecutor started') @@ -306,6 +338,27 @@ class MissionExecutor(Node): 'Failsafe cleared (GREEN) — resuming mission') 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 # ------------------------------------------------------------------ @@ -314,6 +367,10 @@ class MissionExecutor(Node): """ Handle mission commands: START, PAUSE, RESUME, ABORT, LOAD. 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() @@ -328,6 +385,24 @@ class MissionExecutor(Node): response.message = 'Cannot START — no waypoints loaded' 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 self.entry_point = ( self.current_position if self.current_position is not None @@ -597,13 +672,13 @@ class MissionExecutor(Node): self.STATE_ABORTED: MissionStatus.STATE_ABORTED, }.get(self.state, MissionStatus.STATE_IDLE) - # Progress + # Progress as 0-100 percentage total = len(self.waypoints) msg.progress_percent = ( (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: wp_id = (self.waypoints[self.current_wp_index].waypoint_id if self.current_wp_index < total else 'complete') diff --git a/src/rov_mission/rov_mission/recording_manager.py b/src/rov_mission/rov_mission/recording_manager.py new file mode 100755 index 0000000..9c2a583 --- /dev/null +++ b/src/rov_mission/rov_mission/recording_manager.py @@ -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() diff --git a/src/rov_mission/setup.py b/src/rov_mission/setup.py index 77a0582..fb52fb5 100644 --- a/src/rov_mission/setup.py +++ b/src/rov_mission/setup.py @@ -18,8 +18,15 @@ setup( zip_safe=True, entry_points={ 'console_scripts': [ + # Mission state machine — waypoint sequencing, failsafe integration 'mission_executor = rov_mission.mission_executor:main', + + # Cockpit data bridge — ROS2 topics -> WebSocket -> Cockpit data lake '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', ], }, )