diff --git a/src/hangar_sim/config/config.yaml b/src/hangar_sim/config/config.yaml
index 9908d25a0..0e9ce13a1 100644
--- 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
index 2bb749f96..1d815f90f 100644
--- 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
index db8a0d8a1..64163ac56 100644
--- 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,6 +55,21 @@ 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:
@@ -59,8 +77,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
+