15586 imu fuse plugin

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
 
@@ -282,7 +282,14 @@ 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.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: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
 
 
 velocity_force_controller:

src/hangar_sim/config/fuse/fuse.yaml

@@ -1,5 +1,5 @@
 # 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
@@ -15,7 +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
-      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]
+      # 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:
@@ -25,32 +28,48 @@ 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
+      # 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 — 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
+    # accumulated drift from corrupting the estimate.
     wheel_odom_sensor:
-      # Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE)
-      topic: /odom_reliable
+      topic: /platform_velocity_controller/odom
       queue_size: 10
-      # 2D position from wheels (no z)
-      position_dimensions: ['x', 'y']
-      # Heading only
+      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']
-      # Forward velocity
-      linear_velocity_dimensions: ['x']
-      # Turn rate
+      linear_velocity_dimensions: ['x', 'y']
       angular_velocity_dimensions: ['yaw']
-      # Use relative constraints (pose changes rather than absolute poses)
       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]
-      # Order: vx, vy, vz, vroll, vpitch, vyaw
-      twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05]
+      independent: false
+      use_twist_covariance: true
+
+    # 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
+    imu_sensor:
+      topic: /imu_sensor_broadcaster/imu_reliable
+      orientation_dimensions: ['roll', 'pitch', 'yaw']
+      angular_velocity_dimensions: ['yaw']
+      queue_size: 10
 
     # Publish filtered odometry
     publishers:
@@ -59,8 +78,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'

src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro

@@ -16,6 +16,18 @@
           <param name="mujoco_viewer">${mujoco_viewer}</param>
           <param name="mujoco_keyframe">default</param>
       </hardware>
+      <sensor name="imu_site">
+        <state_interface name="orientation.x"/>
+        <state_interface name="orientation.y"/>
+        <state_interface name="orientation.z"/>
+        <state_interface name="orientation.w"/>
+        <state_interface name="angular_velocity.x"/>
+        <state_interface name="angular_velocity.y"/>
+        <state_interface name="angular_velocity.z"/>
+        <state_interface name="linear_acceleration.x"/>
+        <state_interface name="linear_acceleration.y"/>
+        <state_interface name="linear_acceleration.z"/>
+      </sensor>
     </ros2_control>
   </xacro:macro>
 </robot>

src/hangar_sim/launch/agent_bridge.launch.xml

@@ -3,5 +3,4 @@
   <include
     file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
   />
-  <include file="$(find-pkg-share hangar_sim)/launch/fuse.launch.py" />
 </launch>

src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py

@@ -268,34 +268,14 @@ 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(
+    # 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",
     )
 
-    # 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()
 
@@ -324,8 +304,19 @@ 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)
+    ld.add_action(sensor_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

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