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
This commit is contained in:
Grant du Toit 2026-06-20 13:55:12 +00:00
parent 7d4e6dfc23
commit 8a12e3bf50
6 changed files with 48 additions and 80 deletions

View File

@ -6,7 +6,7 @@ Implements depth hold and heading hold modes.
Topics subscribed: Topics subscribed:
/rov/cmd_vel (geometry_msgs/Twist) velocity setpoint from mission planner /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 /rov/failsafe (rov_interfaces/FailsafeStatus) emergency override
Topics published: Topics published:
@ -41,7 +41,7 @@ class MotionController(Node):
self.cmd_sub = self.create_subscription( self.cmd_sub = self.create_subscription(
Twist, '/rov/cmd_vel', self.cmd_callback, 10) Twist, '/rov/cmd_vel', self.cmd_callback, 10)
self.state_sub = self.create_subscription( 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( self.failsafe_sub = self.create_subscription(
FailsafeStatus, '/rov/failsafe', self.failsafe_callback, 10) FailsafeStatus, '/rov/failsafe', self.failsafe_callback, 10)

View File

@ -21,7 +21,7 @@ Breadcrumb system:
Used by return-to-safe to reverse the entry path safely. Used by return-to-safe to reverse the entry path safely.
Topics subscribed: 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 /rov/failsafe (rov_interfaces/FailsafeStatus) safety override
Topics published: Topics published:
@ -196,7 +196,7 @@ class MissionExecutor(Node):
# ------------------------------------------------------------------ # ------------------------------------------------------------------
self.create_subscription( self.create_subscription(
Odometry, '/rov/state', Odometry, '/odometry/filtered',
self._state_callback, 10) self._state_callback, 10)
self.create_subscription( self.create_subscription(

View File

@ -1,6 +1,6 @@
""" """
Navigation stack launch file. Navigation stack launch file.
Launches: state_estimator, depth_node, robot_localization EKF Launches: depth_node, robot_localization EKF
""" """
from launch import LaunchDescription from launch import LaunchDescription
@ -25,13 +25,6 @@ def generate_launch_description():
parameters=[ekf_config], parameters=[ekf_config],
), ),
# State estimator
Node(
package='rov_navigation',
executable='state_estimator',
name='state_estimator',
output='screen',
),
# Depth node # Depth node
Node( Node(

View File

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

View File

@ -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()

View File

@ -18,7 +18,6 @@ setup(
zip_safe=True, zip_safe=True,
entry_points={ entry_points={
'console_scripts': [ 'console_scripts': [
'state_estimator = rov_navigation.state_estimator:main',
'depth_node = rov_navigation.depth_node:main', 'depth_node = rov_navigation.depth_node:main',
], ],
}, },