administrator/rov-autonomy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Phase 3 Stage 1: Recording Manager (DIR-9)

Browse Source 
- 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:
Grant du Toit 2026-07-04 07:36:45 +00:00
parent 2cb7b74777
commit 5d75a9d30d
5 changed files with 735 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
src/rov_bringup/launch/foxglove_mcap.launch.py
View File

@ -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_<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
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://<RPi5-IP>:8765'),
foxglove_bridge_node,
mcap_recorder,
recording_manager_node,
])

219
src/rov_mission/rov_mission/cockpit_bridge.py
View File

@ -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 = {
# 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}')
"""
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:
with self._lock:
self._ws_clients.discard(websocket)
self.get_logger().info(f'Cockpit disconnected: {websocket.remote_address}')
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()

87
src/rov_mission/rov_mission/mission_executor.py
View File

@ -20,9 +20,15 @@ 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
/rov/recording/active (std_msgs/Bool) — recorder no-go gate
Topics published:
/rov/cmd_vel (geometry_msgs/Twist) — velocity setpoints
@ -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')

402
src/rov_mission/rov_mission/recording_manager.py Executable file
View 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()

7
src/rov_mission/setup.py
View File

@ -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',
],
},
)