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:
parent
7d4e6dfc23
commit
8a12e3bf50
@ -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)
|
||||
|
||||
|
||||
@ -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(
|
||||
|
||||
@ -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(
|
||||
|
||||
43
src/rov_navigation/launch/navigation.launch.py.bak
Normal file
43
src/rov_navigation/launch/navigation.launch.py.bak
Normal 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',
|
||||
),
|
||||
])
|
||||
@ -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()
|
||||
@ -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',
|
||||
],
|
||||
},
|
||||
|
||||
Loading…
Reference in New Issue
Block a user