load and launch the controllers

Signed-off-by: ahcorde <[email protected]>

rrbot_gazebo/launch/rrbot_world.launch.py

@@ -18,7 +18,8 @@
 
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 import xacro
@@ -86,6 +87,16 @@ def generate_launch_description():
                                    '-entity', 'rrbot'],
                         output='screen')
 
+    load_joint_state_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'],
+        output='screen'
+    )
+
+    load_joint_trajectory_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'],
+        output='screen'
+    )
+
     # Static TF
     static_tf = Node(package='tf2_ros',
                      executable='static_transform_publisher',
@@ -94,6 +105,18 @@ def generate_launch_description():
                      arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link1'])
 
     return LaunchDescription([
+      RegisterEventHandler(
+          event_handler=OnProcessExit(
+              target_action=spawn_entity,
+              on_exit=[load_joint_state_controller],
+          )
+      ),
+      RegisterEventHandler(
+          event_handler=OnProcessExit(
+              target_action=load_joint_state_controller,
+              on_exit=[load_joint_trajectory_controller],
+          )
+      ),
       gazebo,
       node_robot_state_publisher,
       static_tf,