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 + + + + + + + + + + + + diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml index d370ff313..8eb82040d 100644 --- a/src/hangar_sim/description/ur5e_ridgeback.xml +++ b/src/hangar_sim/description/ur5e_ridgeback.xml @@ -260,6 +260,15 @@ + + + + + + + + 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..39f71b133 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 @@ -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