From 3b9b7f23c6b658519a2e496233b2b0cdada7ebde Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 10 Feb 2026 16:48:55 -0700 Subject: [PATCH 1/5] adding IMU fuse plugin to hangar_sim adding IMU fuse plugin to hangar_sim tuned covariances ... drift is noticeable --- .../control/picknik_ur.ros2_control.yaml | 7 +++ src/hangar_sim/config/fuse/fuse.yaml | 43 ++++++++++++++----- .../picknik_ur_mujoco_ros2_control.xacro | 12 ++++++ .../robot_drivers_to_persist_sim.launch.py | 24 +++++------ 4 files changed, 64 insertions(+), 22 deletions(-) diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml index 995815a5f..16ce2c161 100644 --- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml @@ -283,6 +283,13 @@ imu_sensor_broadcaster: ros__parameters: sensor_name: imu_site frame_id: imu_link + # Static covariance values (row-major 3x3 matrices) + # Orientation covariance (rad^2) + static_covariance_orientation: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001] + # Angular velocity covariance (rad/s)^2 + static_covariance_angular_velocity: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001] + # Linear acceleration covariance (m/s^2)^2 + static_covariance_linear_acceleration: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] velocity_force_controller: diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml index db8a0d8a1..5725be244 100644 --- a/src/hangar_sim/config/fuse/fuse.yaml +++ b/src/hangar_sim/config/fuse/fuse.yaml @@ -5,7 +5,7 @@ state_estimator: # Fixed-lag smoother configuration optimization_frequency: 20.0 transaction_timeout: 0.01 - lag_duration: 0.5 + lag_duration: 1.0 # Motion model for mobile base (3D omnidirectional) motion_models: @@ -15,7 +15,8 @@ state_estimator: mobile_base_motion: # Process noise for state variables # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az - process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0, 1.0, 1.0] + # Ground robot: z, roll, pitch tightly constrained + process_noise_diagonal: [0.005, 0.005, 0.0001, 0.0001, 0.0001, 0.005, 0.005, 0.005, 0.0001, 0.0001, 0.0001, 0.005, 0.05, 0.05, 0.001] sensor_models: initial_localization_sensor: @@ -25,32 +26,54 @@ state_estimator: wheel_odom_sensor: type: fuse_models::Odometry3D motion_models: [mobile_base_motion] + imu_sensor: + type: fuse_models::Imu3D + motion_models: [mobile_base_motion] initial_localization_sensor: publish_on_startup: true # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az + # Moderate x,y,yaw sigma: some initial position uncertainty + # Tight z,roll,pitch: ground robot constraints + initial_sigma: [1.0, 1.0, 0.01, 0.01, 0.01, 0.5, 0.1, 0.1, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1, 0.01] wheel_odom_sensor: # Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE) topic: /odom_reliable queue_size: 10 - # 2D position from wheels (no z) - position_dimensions: ['x', 'y'] + # Position from wheels (z=0 from odom_zero_z anchors the ground plane) + position_dimensions: ['x', 'y', 'z'] # Heading only orientation_dimensions: ['yaw'] # Forward velocity linear_velocity_dimensions: ['x'] # Turn rate angular_velocity_dimensions: ['yaw'] - # Use relative constraints (pose changes rather than absolute poses) + # Use relative constraints (physically correct - wheel odom errors are correlated) differential: true # Covariances - use large values for unused dimensions # Order: x, y, z, roll, pitch, yaw - pose_covariance_diagonal: [0.05, 0.05, 1e9, 1e9, 1e9, 0.05] + pose_covariance_diagonal: [0.01, 0.01, 0.0001, 1e9, 1e9, 0.01] # Order: vx, vy, vz, vroll, vpitch, vyaw - twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05] + twist_covariance_diagonal: [0.01, 1e9, 1e9, 1e9, 1e9, 0.01] + + imu_sensor: + topic: /imu_sensor_broadcaster/imu + # Orientation: all axes, but in differential mode to avoid + # frame mismatch (MuJoCo framequat is in world frame, not odom frame). + # Differential uses orientation *changes* between readings, so no + # absolute reference frame is needed. + orientation_dimensions: ['roll', 'pitch', 'yaw'] + differential: true + # Angular velocity from gyroscope + angular_velocity_dimensions: ['roll', 'pitch', 'yaw'] + # No linear acceleration - accelerometer biases double-integrate + # into position drift. For a ground robot with wheel odom providing + # position, the accelerometer is a net negative for accuracy. + twist_target_frame: 'ridgeback_base_link' + queue_size: 10 # Publish filtered odometry publishers: @@ -59,8 +82,8 @@ state_estimator: filtered_publisher: topic: 'odom_filtered' - base_link_frame_id: 'base_footprint' - base_link_output_frame_id: 'base_footprint' + base_link_frame_id: 'ridgeback_base_link' + base_link_output_frame_id: 'ridgeback_base_link' odom_frame_id: 'odom' map_frame_id: 'map' world_frame_id: 'odom' diff --git a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro index 85c866dd2..dd0b93e96 100644 --- a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro +++ b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro @@ -16,6 +16,18 @@ ${mujoco_viewer} default + + + + + + + + + + + + diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py index 92d81f6c8..9f86025cc 100644 --- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -284,18 +284,6 @@ def generate_launch_description(): output="log", ) - # Fuse state estimator for mobile base localization - hangar_sim_pkg = FindPackageShare("hangar_sim") - fuse_state_estimator = Node( - package="fuse_optimizers", - executable="fixed_lag_smoother_node", - name="state_estimator", - parameters=[ - PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"]) - ], - output="screen", - ) - # Create the launch description and populate ld = LaunchDescription() @@ -326,6 +314,18 @@ def generate_launch_description(): ld.add_action(static_tf_map_to_odom) ld.add_action(odom_to_joint_state_repub) ld.add_action(odom_qos_relay) + + # Fuse state estimator for mobile base localization + hangar_sim_pkg = FindPackageShare("hangar_sim") + fuse_state_estimator = Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimator", + parameters=[ + PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"]) + ], + output="screen", + ) ld.add_action(fuse_state_estimator) return ld From 870d582734c3c6804e18357f95e4e05e208261ac Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Wed, 18 Feb 2026 16:55:51 -0700 Subject: [PATCH 2/5] removed double launch stuff this result looks right but I'm not convinced --- .../control/picknik_ur.ros2_control.yaml | 4 +- src/hangar_sim/config/fuse/fuse.yaml | 54 ++++++++----------- src/hangar_sim/launch/agent_bridge.launch.xml | 1 - src/hangar_sim/launch/fuse.launch.py | 38 ------------- 4 files changed, 24 insertions(+), 73 deletions(-) delete mode 100644 src/hangar_sim/launch/fuse.launch.py diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml index 16ce2c161..92a74293b 100644 --- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml @@ -282,12 +282,12 @@ force_torque_sensor_broadcaster: imu_sensor_broadcaster: ros__parameters: sensor_name: imu_site - frame_id: imu_link + frame_id: ridgeback_base_link # Static covariance values (row-major 3x3 matrices) # Orientation covariance (rad^2) static_covariance_orientation: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001] # Angular velocity covariance (rad/s)^2 - static_covariance_angular_velocity: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001] + static_covariance_angular_velocity: [100.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 100.0] # Linear acceleration covariance (m/s^2)^2 static_covariance_linear_acceleration: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml index 5725be244..71e988b4f 100644 --- a/src/hangar_sim/config/fuse/fuse.yaml +++ b/src/hangar_sim/config/fuse/fuse.yaml @@ -1,11 +1,11 @@ # Mobile base localization configuration using fuse -# Using 3D odometry model for wheel odometry-based pose estimation +# Ground vehicle state estimation: wheel odometry + IMU state_estimator: ros__parameters: # Fixed-lag smoother configuration optimization_frequency: 20.0 transaction_timeout: 0.01 - lag_duration: 1.0 + lag_duration: 0.5 # Motion model for mobile base (3D omnidirectional) motion_models: @@ -15,8 +15,9 @@ state_estimator: mobile_base_motion: # Process noise for state variables # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az - # Ground robot: z, roll, pitch tightly constrained - process_noise_diagonal: [0.005, 0.005, 0.0001, 0.0001, 0.0001, 0.005, 0.005, 0.005, 0.0001, 0.0001, 0.0001, 0.005, 0.05, 0.05, 0.001] + # Ground robot: z, roll, pitch (and their velocities) should be small + # since the robot stays on the ground plane and level. + process_noise_diagonal: [0.25, 0.25, 0.001, 0.001, 0.001, 0.25, 0.1, 0.1, 0.001, 0.001, 0.001, 0.1, 0.001, 0.001, 0.001] sensor_models: initial_localization_sensor: @@ -34,46 +35,35 @@ state_estimator: publish_on_startup: true # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az - # Moderate x,y,yaw sigma: some initial position uncertainty - # Tight z,roll,pitch: ground robot constraints - initial_sigma: [1.0, 1.0, 0.01, 0.01, 0.01, 0.5, 0.1, 0.1, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1, 0.01] + initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + # Wheel odometry — absolute mode + # Odom frame is the reference frame by definition, so absolute is correct. wheel_odom_sensor: - # Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE) topic: /odom_reliable queue_size: 10 - # Position from wheels (z=0 from odom_zero_z anchors the ground plane) position_dimensions: ['x', 'y', 'z'] - # Heading only orientation_dimensions: ['yaw'] - # Forward velocity linear_velocity_dimensions: ['x'] - # Turn rate angular_velocity_dimensions: ['yaw'] - # Use relative constraints (physically correct - wheel odom errors are correlated) - differential: true - # Covariances - use large values for unused dimensions + differential: false # Order: x, y, z, roll, pitch, yaw - pose_covariance_diagonal: [0.01, 0.01, 0.0001, 1e9, 1e9, 0.01] + pose_covariance_diagonal: [0.5, 0.5, 0.01, 1e9, 1e9, 0.05] # Order: vx, vy, vz, vroll, vpitch, vyaw - twist_covariance_diagonal: [0.01, 1e9, 1e9, 1e9, 1e9, 0.01] + twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05] + # IMU sensor + # - Roll/pitch orientation from gravity vector (absolute, keeps robot level) + # - Yaw angular velocity from gyroscope + # - Z linear acceleration (gravity-compensated, constrains vertical dynamics) imu_sensor: topic: /imu_sensor_broadcaster/imu - # Orientation: all axes, but in differential mode to avoid - # frame mismatch (MuJoCo framequat is in world frame, not odom frame). - # Differential uses orientation *changes* between readings, so no - # absolute reference frame is needed. - orientation_dimensions: ['roll', 'pitch', 'yaw'] - differential: true - # Angular velocity from gyroscope - angular_velocity_dimensions: ['roll', 'pitch', 'yaw'] - # No linear acceleration - accelerometer biases double-integrate - # into position drift. For a ground robot with wheel odom providing - # position, the accelerometer is a net negative for accuracy. - twist_target_frame: 'ridgeback_base_link' + orientation_dimensions: ['roll', 'pitch'] + angular_velocity_dimensions: ['yaw'] + linear_acceleration_dimensions: ['z'] + remove_gravitational_acceleration: true queue_size: 10 + throttle_period: 0.1 # Publish filtered odometry publishers: @@ -82,8 +72,8 @@ state_estimator: filtered_publisher: topic: 'odom_filtered' - base_link_frame_id: 'ridgeback_base_link' - base_link_output_frame_id: 'ridgeback_base_link' + base_link_frame_id: 'base_footprint' + base_link_output_frame_id: 'base_footprint' odom_frame_id: 'odom' map_frame_id: 'map' world_frame_id: 'odom' diff --git a/src/hangar_sim/launch/agent_bridge.launch.xml b/src/hangar_sim/launch/agent_bridge.launch.xml index 3cc960c03..b2aa75434 100644 --- a/src/hangar_sim/launch/agent_bridge.launch.xml +++ b/src/hangar_sim/launch/agent_bridge.launch.xml @@ -3,5 +3,4 @@ - diff --git a/src/hangar_sim/launch/fuse.launch.py b/src/hangar_sim/launch/fuse.launch.py deleted file mode 100644 index 089f93f53..000000000 --- a/src/hangar_sim/launch/fuse.launch.py +++ /dev/null @@ -1,38 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2024 PickNik Robotics -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - pkg_dir = FindPackageShare("hangar_sim") - - return LaunchDescription( - [ - # Run the fuse fixed-lag smoother for mobile base state estimation - Node( - package="fuse_optimizers", - executable="fixed_lag_smoother_node", - name="state_estimator", - parameters=[ - PathJoinSubstitution([pkg_dir, "config", "fuse", "fuse.yaml"]) - ], - ), - ] - ) From d810d3b00303ad130070a0f7d0fcaec21427459e Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Mon, 23 Feb 2026 12:41:28 -0700 Subject: [PATCH 3/5] tracking more tightly --- src/hangar_sim/config/fuse/fuse.yaml | 12 +++++------- .../sim/robot_drivers_to_persist_sim.launch.py | 9 --------- 2 files changed, 5 insertions(+), 16 deletions(-) diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml index 71e988b4f..277d247db 100644 --- a/src/hangar_sim/config/fuse/fuse.yaml +++ b/src/hangar_sim/config/fuse/fuse.yaml @@ -17,7 +17,8 @@ state_estimator: # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az # Ground robot: z, roll, pitch (and their velocities) should be small # since the robot stays on the ground plane and level. - process_noise_diagonal: [0.25, 0.25, 0.001, 0.001, 0.001, 0.25, 0.1, 0.1, 0.001, 0.001, 0.001, 0.1, 0.001, 0.001, 0.001] + #process_noise_diagonal: [0.25, 0.25, 0.001, 0.001, 0.001, 0.25, 0.1, 0.1, 0.001, 0.001, 0.001, 0.1, 0.001, 0.001, 0.001] + process_noise_diagonal: [2.0, 2.0, 0.1, 0.1, 0.1, 2.0, 2.0, 1.0, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.001] sensor_models: initial_localization_sensor: @@ -47,10 +48,7 @@ state_estimator: linear_velocity_dimensions: ['x'] angular_velocity_dimensions: ['yaw'] differential: false - # Order: x, y, z, roll, pitch, yaw - pose_covariance_diagonal: [0.5, 0.5, 0.01, 1e9, 1e9, 0.05] - # Order: vx, vy, vz, vroll, vpitch, vyaw - twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05] + # Uses covariance from the odom message directly (no override) # IMU sensor # - Roll/pitch orientation from gravity vector (absolute, keeps robot level) @@ -72,8 +70,8 @@ state_estimator: filtered_publisher: topic: 'odom_filtered' - base_link_frame_id: 'base_footprint' - base_link_output_frame_id: 'base_footprint' + base_link_frame_id: 'ridgeback_base_link' + base_link_output_frame_id: 'ridgeback_base_link' odom_frame_id: 'odom' map_frame_id: 'map' world_frame_id: 'odom' diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py index 9f86025cc..e155a7a14 100644 --- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -268,14 +268,6 @@ def generate_launch_description(): arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"], ) - # Publish odometry as joint state messages - odom_to_joint_state_repub = Node( - package="hangar_sim", - executable="odometry_joint_state_publisher.py", - name="odometry_joint_state_publisher", - output="log", - ) - # QoS relay to bridge BEST_EFFORT odom to RELIABLE for fuse odom_qos_relay = Node( package="hangar_sim", @@ -312,7 +304,6 @@ def generate_launch_description(): ld.add_action(static_tf_world_to_map) ld.add_action(static_tf_map_to_odom) - ld.add_action(odom_to_joint_state_repub) ld.add_action(odom_qos_relay) # Fuse state estimator for mobile base localization From d926b93761cfe2c4fad7f88754ab66e3dd5c98d2 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Mon, 23 Feb 2026 14:57:05 -0700 Subject: [PATCH 4/5] changed config for mecanum wheel odom --- .../control/picknik_ur.ros2_control.yaml | 4 +-- src/hangar_sim/config/fuse/fuse.yaml | 26 ++++++++++++++----- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml index 92a74293b..30cbf4d19 100644 --- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml @@ -53,8 +53,8 @@ platform_velocity_controller: reference_timeout: 0.1 use_stamped_vel: False - pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03] + pose_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 0.5] + twist_covariance_diagonal: [0.1, 0.1, 0.1, 1000000.0, 1000000.0, 0.5] cmd_vel_timeout: 0.5 diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml index 277d247db..c2ca2e5fc 100644 --- a/src/hangar_sim/config/fuse/fuse.yaml +++ b/src/hangar_sim/config/fuse/fuse.yaml @@ -17,8 +17,10 @@ state_estimator: # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az # Ground robot: z, roll, pitch (and their velocities) should be small # since the robot stays on the ground plane and level. - #process_noise_diagonal: [0.25, 0.25, 0.001, 0.001, 0.001, 0.25, 0.1, 0.1, 0.001, 0.001, 0.001, 0.1, 0.001, 0.001, 0.001] - process_noise_diagonal: [2.0, 2.0, 0.1, 0.1, 0.1, 2.0, 2.0, 1.0, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.001] + #trying this with the actual wheel odom + process_noise_diagonal: [0.25, 0.25, 0.001, 0.001, 0.001, 0.25, 0.1, 0.1, 0.001, 0.001, 0.001, 0.1, 0.001, 0.001, 0.001] + #this one works with the ground truth odom + #process_noise_diagonal: [2.0, 2.0, 0.1, 0.1, 0.1, 2.0, 2.0, 1.0, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.001] sensor_models: initial_localization_sensor: @@ -38,17 +40,27 @@ state_estimator: initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - # Wheel odometry — absolute mode - # Odom frame is the reference frame by definition, so absolute is correct. + # Wheel odometry — differential mode + # Using differential mode because platform_velocity_controller/odom is + # wheel-integrated odometry that drifts over time. Differential mode fuses + # only the relative change between consecutive messages, preventing + # accumulated drift from corrupting the estimate. wheel_odom_sensor: - topic: /odom_reliable + topic: /platform_velocity_controller/odom queue_size: 10 + pose_loss: + type: fuse_loss::HuberLoss + a: 1.0 + linear_velocity_loss: + type: fuse_loss::HuberLoss + a: 1.0 position_dimensions: ['x', 'y', 'z'] orientation_dimensions: ['yaw'] linear_velocity_dimensions: ['x'] angular_velocity_dimensions: ['yaw'] - differential: false - # Uses covariance from the odom message directly (no override) + differential: true + independent: false + use_twist_covariance: true # IMU sensor # - Roll/pitch orientation from gravity vector (absolute, keeps robot level) From 0b359524f3395a31d97aef4a9f09a8be9bcc0190 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 24 Feb 2026 13:58:22 -0700 Subject: [PATCH 5/5] added qos relay for the IMU --- .../control/picknik_ur.ros2_control.yaml | 4 +-- src/hangar_sim/config/fuse/fuse.yaml | 30 ++++++++----------- .../robot_drivers_to_persist_sim.launch.py | 8 ++--- src/hangar_sim/script/odom_qos_relay.py | 29 +++++++++++++----- 4 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml index 30cbf4d19..c03b2cccd 100644 --- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml @@ -287,9 +287,9 @@ imu_sensor_broadcaster: # Orientation covariance (rad^2) static_covariance_orientation: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001] # Angular velocity covariance (rad/s)^2 - static_covariance_angular_velocity: [100.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 100.0] + static_covariance_angular_velocity: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] # Linear acceleration covariance (m/s^2)^2 - static_covariance_linear_acceleration: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] + static_covariance_linear_acceleration: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] velocity_force_controller: diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml index c2ca2e5fc..fbc429d41 100644 --- a/src/hangar_sim/config/fuse/fuse.yaml +++ b/src/hangar_sim/config/fuse/fuse.yaml @@ -15,12 +15,10 @@ state_estimator: mobile_base_motion: # Process noise for state variables # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az - # Ground robot: z, roll, pitch (and their velocities) should be small - # since the robot stays on the ground plane and level. - #trying this with the actual wheel odom - process_noise_diagonal: [0.25, 0.25, 0.001, 0.001, 0.001, 0.25, 0.1, 0.1, 0.001, 0.001, 0.001, 0.1, 0.001, 0.001, 0.001] - #this one works with the ground truth odom - #process_noise_diagonal: [2.0, 2.0, 0.1, 0.1, 0.1, 2.0, 2.0, 1.0, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.001] + # z, roll, pitch and their derivatives are near-zero for a ground robot. + #process_noise_diagonal: [0.25, 0.25, 0.00001, 0.00001, 0.00001, 0.25 + # -, 0.1, 0.1, 0.00001, 0.00001, 0.0001, 0.1, 0.0001, 0.0001, 0.00001] + process_noise_diagonal: [1.0, 1.0, 0.0001, 0.0001, 0.0001, 0.5, 0.5, 0.5, 0.0001, 0.0001, 0.0001, 0.25, 0.00001, 0.00001, 0.00001] sensor_models: initial_localization_sensor: @@ -38,9 +36,11 @@ state_estimator: publish_on_startup: true # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az + # z and its derivatives start with very tight sigma — ground robot stays on the plane. + initial_sigma: [0.1, 0.1, 0.001, 0.01, 0.01, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.1, 0.001, 0.001, 0.001] - # Wheel odometry — differential mode + # Wheel odometry — provides translation (x, y) and yaw # Using differential mode because platform_velocity_controller/odom is # wheel-integrated odometry that drifts over time. Differential mode fuses # only the relative change between consecutive messages, preventing @@ -56,24 +56,20 @@ state_estimator: a: 1.0 position_dimensions: ['x', 'y', 'z'] orientation_dimensions: ['yaw'] - linear_velocity_dimensions: ['x'] + linear_velocity_dimensions: ['x', 'y'] angular_velocity_dimensions: ['yaw'] differential: true independent: false use_twist_covariance: true - # IMU sensor - # - Roll/pitch orientation from gravity vector (absolute, keeps robot level) + # IMU sensor — provides orientation and yaw rate + # - Roll/pitch/yaw orientation (absolute, covariance 0.001 vs wheel odom 0.5) # - Yaw angular velocity from gyroscope - # - Z linear acceleration (gravity-compensated, constrains vertical dynamics) imu_sensor: - topic: /imu_sensor_broadcaster/imu - orientation_dimensions: ['roll', 'pitch'] + topic: /imu_sensor_broadcaster/imu_reliable + orientation_dimensions: ['roll', 'pitch', 'yaw'] angular_velocity_dimensions: ['yaw'] - linear_acceleration_dimensions: ['z'] - remove_gravitational_acceleration: true queue_size: 10 - throttle_period: 0.1 # Publish filtered odometry publishers: diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py index e155a7a14..c4961a50a 100644 --- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -268,11 +268,11 @@ def generate_launch_description(): arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"], ) - # QoS relay to bridge BEST_EFFORT odom to RELIABLE for fuse - odom_qos_relay = Node( + # QoS relay to bridge BEST_EFFORT odom and IMU to RELIABLE for fuse + sensor_qos_relay = Node( package="hangar_sim", executable="odom_qos_relay.py", - name="odom_qos_relay", + name="sensor_qos_relay", output="log", ) @@ -304,7 +304,7 @@ def generate_launch_description(): ld.add_action(static_tf_world_to_map) ld.add_action(static_tf_map_to_odom) - ld.add_action(odom_qos_relay) + ld.add_action(sensor_qos_relay) # Fuse state estimator for mobile base localization hangar_sim_pkg = FindPackageShare("hangar_sim") diff --git a/src/hangar_sim/script/odom_qos_relay.py b/src/hangar_sim/script/odom_qos_relay.py index 11c588922..e0bbdada0 100755 --- a/src/hangar_sim/script/odom_qos_relay.py +++ b/src/hangar_sim/script/odom_qos_relay.py @@ -28,12 +28,13 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -"""Relay node to bridge BEST_EFFORT odometry to RELIABLE QoS for fuse.""" +"""Relay node to bridge BEST_EFFORT sensor topics to RELIABLE QoS for fuse.""" import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry +from sensor_msgs.msg import Imu from rclpy.qos import ( QoSProfile, @@ -43,11 +44,11 @@ ) -class OdomQoSRelay(Node): - """Relay node that subscribes to odometry with BEST_EFFORT and republishes with RELIABLE QoS.""" +class SensorQoSRelay(Node): + """Relay node that subscribes to sensor topics with BEST_EFFORT and republishes with RELIABLE QoS.""" def __init__(self): - super().__init__("odom_qos_relay") + super().__init__("sensor_qos_relay") # Subscribe with BEST_EFFORT to match mujoco_system publisher qos_sub = QoSProfile( @@ -65,18 +66,30 @@ def __init__(self): depth=10, ) - self.sub = self.create_subscription( + # Odometry relay + self.odom_sub = self.create_subscription( Odometry, "/odom", self.odom_callback, qos_sub ) - self.pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub) + self.odom_pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub) + + # IMU relay + self.imu_sub = self.create_subscription( + Imu, "/imu_sensor_broadcaster/imu", self.imu_callback, qos_sub + ) + self.imu_pub = self.create_publisher( + Imu, "/imu_sensor_broadcaster/imu_reliable", qos_pub + ) def odom_callback(self, msg): - self.pub.publish(msg) + self.odom_pub.publish(msg) + + def imu_callback(self, msg): + self.imu_pub.publish(msg) def main(args=None): rclpy.init(args=args) - node = OdomQoSRelay() + node = SensorQoSRelay() rclpy.spin(node) node.destroy_node() rclpy.shutdown()