From c94412ddaee561a6518b11e8b78bff1f5a41170f Mon Sep 17 00:00:00 2001 From: Grant du Toit Date: Wed, 17 Jun 2026 18:28:06 +0000 Subject: [PATCH] Fix full-stack launch: MAVROS fcu_url + plugin_allowlist, timestamped bag dir, BEST_EFFORT QoS on MAVROS subs, battery<0 unknown guard - rov_full.launch.py: fcu_url to udp://@:14550 (was bad local bind); add plugin_allowlist (sys_status,sys_time,command,imu,global_position,setpoint_velocity) to stop duplicate-subscription crash on companion_process_status/debug_value - foxglove_mcap.launch.py: timestamp bag output dir (was fixed /data/bags/dive, failed on second launch) - failsafe_monitor/depth_node/state_estimator: qos_profile_sensor_data on MAVROS-sourced subs (MAVROS publishes BEST_EFFORT; default RELIABLE blocked all messages) - failsafe_monitor: treat battery_percent<0 as UNKNOWN not critical (SITL has no battery; was forcing permanent EMERGENCY_SURFACE) --- .../launch/foxglove_mcap.launch.py | 3 ++- src/rov_bringup/launch/rov_full.launch.py | 3 ++- .../rov_control/failsafe_monitor.py | 24 +++++++++++++------ .../rov_navigation/depth_node.py | 3 ++- .../rov_navigation/state_estimator.py | 3 ++- 5 files changed, 25 insertions(+), 11 deletions(-) diff --git a/src/rov_bringup/launch/foxglove_mcap.launch.py b/src/rov_bringup/launch/foxglove_mcap.launch.py index 306ead2..bdc889f 100644 --- a/src/rov_bringup/launch/foxglove_mcap.launch.py +++ b/src/rov_bringup/launch/foxglove_mcap.launch.py @@ -14,6 +14,7 @@ Storage: Bags written to /data/bags/ on NVMe. Create before first launch: sudo mkdir -p /data/bags && sudo chown ubuntu:ubuntu /data/bags """ +import datetime from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo from launch.conditions import IfCondition @@ -25,7 +26,7 @@ def generate_launch_description(): bag_output_arg = DeclareLaunchArgument( 'bag_output_dir', - default_value='/data/bags/dive', + default_value='/data/bags/dive_' + datetime.datetime.now().strftime('%Y_%m_%d-%H_%M_%S'), description='Base path for MCAP bag. Timestamp appended automatically.', ) diff --git a/src/rov_bringup/launch/rov_full.launch.py b/src/rov_bringup/launch/rov_full.launch.py index 0f5e772..850eeff 100644 --- a/src/rov_bringup/launch/rov_full.launch.py +++ b/src/rov_bringup/launch/rov_full.launch.py @@ -45,10 +45,11 @@ def generate_launch_description(): name='mavros', output='screen', parameters=[{ - 'fcu_url': ['udp://', LaunchConfiguration('blueos_ip'), ':14550@14555'], + 'fcu_url': ['udp://@', LaunchConfiguration('blueos_ip'), ':14550'], 'gcs_url': '', 'target_system_id': 1, 'target_component_id': 1, + 'plugin_allowlist': ['sys_status', 'sys_time', 'command', 'imu', 'global_position', 'setpoint_velocity'], }], ), diff --git a/src/rov_control/rov_control/failsafe_monitor.py b/src/rov_control/rov_control/failsafe_monitor.py index 8c239c8..511d58d 100644 --- a/src/rov_control/rov_control/failsafe_monitor.py +++ b/src/rov_control/rov_control/failsafe_monitor.py @@ -36,6 +36,7 @@ Topics published: import rclpy from rclpy.node import Node from rclpy.duration import Duration +from rclpy.qos import qos_profile_sensor_data from std_msgs.msg import Bool, Float32 from sensor_msgs.msg import BatteryState from mavros_msgs.msg import State @@ -144,15 +145,15 @@ class FailsafeMonitor(Node): self.create_subscription( BatteryState, '/mavros/battery', - self._battery_callback, 10) + self._battery_callback, qos_profile_sensor_data) self.create_subscription( State, '/mavros/state', - self._state_callback, 10) + self._state_callback, qos_profile_sensor_data) self.create_subscription( Float32, '/mavros/global_position/rel_alt', - self._altitude_callback, 10) + self._altitude_callback, qos_profile_sensor_data) self.create_subscription( Bool, '/rov/mission/abort', @@ -266,9 +267,17 @@ class FailsafeMonitor(Node): # --- Battery --- pct = self.battery_percent - self.flag_emergency_battery = pct < self._battery_emerg_pct - self.flag_critical_battery = pct < self._battery_critical_pct - self.flag_low_battery = pct < self._battery_return_pct + if pct < 0.0: + # Negative percentage = no valid battery telemetry (e.g. SITL with + # no simulated battery). Treat as UNKNOWN, not critically low — + # do not trip battery failsafes on absent data. + self.flag_emergency_battery = False + self.flag_critical_battery = False + self.flag_low_battery = False + else: + self.flag_emergency_battery = pct < self._battery_emerg_pct + self.flag_critical_battery = pct < self._battery_critical_pct + self.flag_low_battery = pct < self._battery_return_pct # --- Depth --- d = self.current_depth_m @@ -376,7 +385,8 @@ class FailsafeMonitor(Node): return # Priority 8: Low battery warning — informational only - if self.battery_percent < self._battery_warning_pct: + # (skip when battery_percent < 0: no valid telemetry, e.g. SITL) + if 0.0 <= self.battery_percent < self._battery_warning_pct: self.get_logger().warn( f'Battery warning: {self.battery_percent*100:.0f}% — ' 'complete current task, no new panels' diff --git a/src/rov_navigation/rov_navigation/depth_node.py b/src/rov_navigation/rov_navigation/depth_node.py index cf47411..32b4bd4 100644 --- a/src/rov_navigation/rov_navigation/depth_node.py +++ b/src/rov_navigation/rov_navigation/depth_node.py @@ -12,6 +12,7 @@ Topics published: import rclpy from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data from std_msgs.msg import Float64 @@ -23,7 +24,7 @@ class DepthNode(Node): self.sub = self.create_subscription( Float64, '/mavros/global_position/rel_alt', - self.alt_callback, 10) + self.alt_callback, qos_profile_sensor_data) self.pub = self.create_publisher(Float64, '/rov/depth', 10) diff --git a/src/rov_navigation/rov_navigation/state_estimator.py b/src/rov_navigation/rov_navigation/state_estimator.py index 96722f4..17f9394 100644 --- a/src/rov_navigation/rov_navigation/state_estimator.py +++ b/src/rov_navigation/rov_navigation/state_estimator.py @@ -15,6 +15,7 @@ Topics published: import rclpy from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry from std_msgs.msg import Float64 @@ -34,7 +35,7 @@ class StateEstimator(Node): # --- Subscribers --- self.imu_sub = self.create_subscription( - Imu, '/imu/data', self.imu_callback, 10) + Imu, '/imu/data', self.imu_callback, qos_profile_sensor_data) self.depth_sub = self.create_subscription( Float64, '/rov/depth', self.depth_callback, 10)