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:
|
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)
|
||||||
|
|
||||||
|
|||||||
@ -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(
|
||||||
|
|||||||
@ -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(
|
||||||
|
|||||||
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,
|
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',
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user