add imu controller to cart_example_velocity

ros-controls · Jun 8, 2021 · 3b28b4e · 3b28b4e
1 parent 03e7d7e
commit 3b28b4e
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml
@@ -8,7 +8,14 @@ controller_manager:
     joint_state_controller:
       type: joint_state_controller/JointStateController
 
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
 velocity_controller:
   ros__parameters:
     joints:
       - slider_to_cart
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: cart_imu_sensor
+    frame_id: imu
diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py
@@ -65,6 +65,11 @@ def generate_launch_description():
         output='screen'
     )
 
+    load_imu_sensor_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_start_controller', 'imu_sensor_broadcaster'],
+        output='screen'
+    )
+
     return LaunchDescription([
         RegisterEventHandler(
             event_handler=OnProcessExit(
@@ -78,6 +83,12 @@ def generate_launch_description():
                 on_exit=[load_velocity_controller],
             )
         ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_velocity_controller,
+                on_exit=[load_imu_sensor_broadcaster],
+            )
+        ),
         gazebo,
         node_robot_state_publisher,
         spawn_entity,