15586 imu fuse plugin

[suchkristenwow]

This PR adds an IMU (fuse_models::Imu3D) to the hangar_sim state estimation pipeline, fusing IMU data with the wheel odom (from Mujoco).

In ur5e_ridgeback.xml

• imu_link is added to the Ridgeback chassis to match the actual IMU location from the ridgeback.urdf.xacro
• 3 sensors: framequat (orientation), gyro (angular vel), and accelerometer (linear acceleration)

In picknik_ur_mujoco_ros2_contro.xacro

• added imu_site sensor to bridge Mujoco sensor data into the rso2_control hardware interface

Controller config: picknik_ur.ros2_control.yaml & config.yaml

• Register imu_sensor_broadcaster/IMUSensorBroadcaster in the controller manager
• imu_sensor_broadcaster is added to controllers_active_at_startup

Fuse Configuration

• Wheel Odometry:

  • differential is true: only fuses relative changes between consecutive messages
  • uses twist covariance instead of the pose covariance which has terrible covariance
  • independent: false - consecutive odom messages come from the same integrator

• IMU:

  • differential: false
  • No linear acceleration dimensions: removed because otherwise the estimate flies up into space, I'm guessing because of gravity compensation but not sure
  • subscribes to /imu_sensor_broadcaster/imu_reliable (which is just /imu_sensor_broadcaster/imu with QOS set to reliable)

Screencast from 2026-02-23 12-56-23.webm
^In this video, the blue is odom_reliable (the republished gt odometry) and red is odom_filtered (the product of fuse)

Screencast from 2026-02-23 13-49-46.webm
^In this video, I change odom_reliable > /platform_velocity_controller/odom which for some reason has a completely messed up covariance

[suchkristenwow]

Documenting discussion with Griz:

• Griz was asking if I accidentally was double-publishing the TF between odom and base_footprint with this PR
• I'm not but the fuse state estimator might have been getting double-launched, once in agent_bridge.launch.xml (via fuse.launch.py) and again with robot_drivers_to_persist_sim.launch.py
• I thought it made more sense to keep it in robot_drivers_to_persist_simlaunch.py since

1. that's where the robot infrastructure lives (agent_bridge is for the moveit_studio agent bridge, as the name would suggest) and
2. I can get rid of fuse.launch.py

Now the fuse state estimator will now only launch from robot_drivers_to_persist_sim.launch.py

[suchkristenwow]

Screencast from 2026-02-23 14-50-57.webm
^ the commit d926b93 uses the mecanum wheel odom topic and the estimate is just terrible but better than the config I was using when using the ground truth odometry message from Mujoco.

[suchkristenwow]

Screencast from 2026-02-24 13-53-24.webm
This video is the result of this commit: d7fc501

At 0:15 I toggle on the covariance for the mecanum wheel odometry message.

Blue is the gt odometry from Mujoco, red is the filtered odom, green is the mecanum wheel odom

This makes sense though since the mecanum wheel odom message shouldn't have info about z, roll, or pitch so effectively it's saying, don't listen to me about these.