PickNikRobotics · suchkristenwow · Feb 6, 2026 · Feb 10, 2026
diff --git a/src/hangar_sim/config/config.yaml b/src/hangar_sim/config/config.yaml
@@ -86,6 +86,7 @@ ros2_control:
   # [Optional, default=[]]
   controllers_active_at_startup:
     - "force_torque_sensor_broadcaster"
+    - "imu_sensor_broadcaster"
     - "joint_state_broadcaster"
     - "platform_velocity_controller"
     - "vacuum_gripper"

diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
@@ -11,6 +11,8 @@ controller_manager:
       type: clearpath_mecanum_drive_controller/MecanumDriveController
     platform_velocity_controller_nav2:
       type: clearpath_mecanum_drive_controller/MecanumDriveController
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
     vacuum_gripper:
       type: position_controllers/GripperActionController
     velocity_force_controller:
@@ -277,6 +279,18 @@ force_torque_sensor_broadcaster:
       - torque.z
     frame_id: tool0
 
+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:
   ros__parameters:

diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml
@@ -25,6 +25,9 @@ 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
@@ -52,15 +55,30 @@ state_estimator:
       # Order: vx, vy, vz, vroll, vpitch, vyaw
       twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05]
 
+    imu_sensor:
+      topic: /imu_sensor_broadcaster/imu
+      # Orientation from IMU
+      orientation_dimensions: ['roll', 'pitch', 'yaw']
+      # Angular velocity from gyroscope
+      angular_velocity_dimensions: ['roll', 'pitch', 'yaw']
+      # Linear acceleration from accelerometer
+      linear_acceleration_dimensions: ['x', 'y', 'z']
+      # Transform accelerations into base frame
+      acceleration_target_frame: 'ridgeback_base_link'
+      twist_target_frame: 'ridgeback_base_link'
+      # Remove gravity from acceleration measurements
+      remove_gravitational_acceleration: true
+      queue_size: 10
+
     # Publish filtered odometry
     publishers:
       filtered_publisher:
         type: fuse_models::Odometry3DPublisher
 
     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
@@ -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>
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -260,6 +260,15 @@
           <body name="rear_right_wheel_link" pos="-0.319 -0.2755 0.12">
             <include file="rear_right_wheel_link.xml" />
           </body>
+          <!-- IMU: position from ridgeback.urdf.xacro imu_joint, relative to chassis_link -->
+          <body name="imu_link" pos="0.2085 -0.2902 0.1681">
+            <inertial
+              mass="0.001"
+              pos="0 0 0"
+              diaginertia="1e-09 1e-09 1e-09"
+            />
+            <site name="imu_site" pos="0 0 0" size="0.01" rgba="0 1 0 1" />
+          </body>
           <body name="ham_assem" pos="0 0 0.278813" euler="1.5708 -1.57079 0">
             <geom
               name="ham_assem"
@@ -574,5 +583,8 @@
   <sensor>
     <force site="tcp_fts_sensor" />
     <torque site="tcp_fts_sensor" />
+    <framequat objtype="site" objname="imu_site" />
+    <gyro site="imu_site" />
+    <accelerometer site="imu_site" />
   </sensor>
 </mujoco>
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
@@ -41,13 +41,11 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     LaunchConfiguration,
-    PathJoinSubstitution,
     PythonExpression,
 )
 from launch_ros.actions import Node
 from launch_ros.actions import PushRosNamespace
 from launch_ros.descriptions import ParameterFile
-from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import RewrittenYaml, ReplaceString
 
 
@@ -284,17 +282,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 +313,5 @@ 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)
-    ld.add_action(fuse_state_estimator)
 
     return ld