From 8a12e3bf509c81e6022913a05bcbf69feb9925cc Mon Sep 17 00:00:00 2001 From: Grant du Toit Date: Sat, 20 Jun 2026 13:55:12 +0000 Subject: [PATCH] Retire stub state_estimator; consumers use EKF /odometry/filtered - state_estimator was an unimplemented stub (empty callbacks, never published /rov/state) - removed node, entry point, launch block - motion_controller + mission_executor: /rov/state -> /odometry/filtered (the working robot_localization EKF output) - Eliminates redundant/competing state estimators; single source of truth - Verified: EKF publishes ~11Hz, both consumers subscribed --- .../rov_control/motion_controller.py | 4 +- .../rov_mission/mission_executor.py | 4 +- .../launch/navigation.launch.py | 9 +-- .../launch/navigation.launch.py.bak | 43 ++++++++++++ .../rov_navigation/state_estimator.py | 67 ------------------- src/rov_navigation/setup.py | 1 - 6 files changed, 48 insertions(+), 80 deletions(-) create mode 100644 src/rov_navigation/launch/navigation.launch.py.bak delete mode 100644 src/rov_navigation/rov_navigation/state_estimator.py diff --git a/src/rov_control/rov_control/motion_controller.py b/src/rov_control/rov_control/motion_controller.py index 4c29303..4bde3e7 100644 --- a/src/rov_control/rov_control/motion_controller.py +++ b/src/rov_control/rov_control/motion_controller.py @@ -6,7 +6,7 @@ Implements depth hold and heading hold modes. Topics subscribed: /rov/cmd_vel (geometry_msgs/Twist) — velocity setpoint from mission planner - /rov/state (nav_msgs/Odometry) — current state from navigation stack + /odometry/filtered (nav_msgs/Odometry) — fused state from EKF /rov/failsafe (rov_interfaces/FailsafeStatus) — emergency override Topics published: @@ -41,7 +41,7 @@ class MotionController(Node): self.cmd_sub = self.create_subscription( Twist, '/rov/cmd_vel', self.cmd_callback, 10) self.state_sub = self.create_subscription( - Odometry, '/rov/state', self.state_callback, 10) + Odometry, '/odometry/filtered', self.state_callback, 10) self.failsafe_sub = self.create_subscription( FailsafeStatus, '/rov/failsafe', self.failsafe_callback, 10) diff --git a/src/rov_mission/rov_mission/mission_executor.py b/src/rov_mission/rov_mission/mission_executor.py index d968f63..8ba076a 100644 --- a/src/rov_mission/rov_mission/mission_executor.py +++ b/src/rov_mission/rov_mission/mission_executor.py @@ -21,7 +21,7 @@ Breadcrumb system: Used by return-to-safe to reverse the entry path safely. Topics subscribed: - /rov/state (nav_msgs/Odometry) — current position + /odometry/filtered (nav_msgs/Odometry) — fused state from EKF /rov/failsafe (rov_interfaces/FailsafeStatus) — safety override Topics published: @@ -196,7 +196,7 @@ class MissionExecutor(Node): # ------------------------------------------------------------------ self.create_subscription( - Odometry, '/rov/state', + Odometry, '/odometry/filtered', self._state_callback, 10) self.create_subscription( diff --git a/src/rov_navigation/launch/navigation.launch.py b/src/rov_navigation/launch/navigation.launch.py index 1cd9ed9..205ed22 100644 --- a/src/rov_navigation/launch/navigation.launch.py +++ b/src/rov_navigation/launch/navigation.launch.py @@ -1,6 +1,6 @@ """ Navigation stack launch file. -Launches: state_estimator, depth_node, robot_localization EKF +Launches: depth_node, robot_localization EKF """ from launch import LaunchDescription @@ -25,13 +25,6 @@ def generate_launch_description(): parameters=[ekf_config], ), - # State estimator - Node( - package='rov_navigation', - executable='state_estimator', - name='state_estimator', - output='screen', - ), # Depth node Node( diff --git a/src/rov_navigation/launch/navigation.launch.py.bak b/src/rov_navigation/launch/navigation.launch.py.bak new file mode 100644 index 0000000..1cd9ed9 --- /dev/null +++ b/src/rov_navigation/launch/navigation.launch.py.bak @@ -0,0 +1,43 @@ +""" +Navigation stack launch file. +Launches: state_estimator, depth_node, robot_localization EKF +""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('rov_navigation') + ekf_config = os.path.join(pkg_dir, 'config', 'ekf.yaml') + + return LaunchDescription([ + # EKF from robot_localization + Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters=[ekf_config], + ), + + # State estimator + Node( + package='rov_navigation', + executable='state_estimator', + name='state_estimator', + output='screen', + ), + + # Depth node + Node( + package='rov_navigation', + executable='depth_node', + name='depth_node', + output='screen', + ), + ]) diff --git a/src/rov_navigation/rov_navigation/state_estimator.py b/src/rov_navigation/rov_navigation/state_estimator.py deleted file mode 100644 index 17f9394..0000000 --- a/src/rov_navigation/rov_navigation/state_estimator.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python3 -""" -State estimator node. -Fuses IMU, depth, and (future) DVL data via robot_localization EKF. -Publishes /rov/state as nav_msgs/Odometry. - -Topics subscribed: - /imu/data (sensor_msgs/Imu) — Xsens IMU - /mavros/imu/data (sensor_msgs/Imu) — ArduSub IMU (backup) - /rov/depth (std_msgs/Float64) — depth from pressure sensor - -Topics published: - /rov/state (nav_msgs/Odometry) — fused state estimate -""" - -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 - - -class StateEstimator(Node): - """Fuses sensor inputs into a single state estimate.""" - - def __init__(self): - super().__init__('state_estimator') - - # --- Parameters --- - self.declare_parameter('use_xsens', True) - self.declare_parameter('use_mavros_imu', False) - self.use_xsens = self.get_parameter('use_xsens').value - self.use_mavros_imu = self.get_parameter('use_mavros_imu').value - - # --- Subscribers --- - self.imu_sub = self.create_subscription( - Imu, '/imu/data', self.imu_callback, qos_profile_sensor_data) - self.depth_sub = self.create_subscription( - Float64, '/rov/depth', self.depth_callback, 10) - - # --- Publishers --- - self.state_pub = self.create_publisher(Odometry, '/rov/state', 10) - - self.get_logger().info('StateEstimator node started') - - def imu_callback(self, msg: Imu): - """Handle incoming IMU data.""" - # TODO: forward to EKF or process directly - pass - - def depth_callback(self, msg: Float64): - """Handle incoming depth measurement.""" - # TODO: incorporate depth into state estimate - pass - - -def main(args=None): - rclpy.init(args=args) - node = StateEstimator() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/rov_navigation/setup.py b/src/rov_navigation/setup.py index d95fa7a..c507ef3 100644 --- a/src/rov_navigation/setup.py +++ b/src/rov_navigation/setup.py @@ -18,7 +18,6 @@ setup( zip_safe=True, entry_points={ 'console_scripts': [ - 'state_estimator = rov_navigation.state_estimator:main', 'depth_node = rov_navigation.depth_node:main', ], },