move_groupとcontrollerを追加

rt-net · Oct 24, 2023 · cd63d0d · cd63d0d
1 parent 698f9de
commit cd63d0d
Showing 1 changed file with 60 additions and 7 deletions.
diff --git a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py
@@ -18,6 +18,8 @@
 from sciurus17_description.robot_description_loader import RobotDescriptionLoader
 from launch import LaunchDescription
 from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_ros.actions import SetParameter
 
@@ -51,14 +53,58 @@ def generate_launch_description():
 
     description_loader = RobotDescriptionLoader()
     description_loader.use_gazebo = 'true'
+    description_loader.gz_control_config_package = 'sciurus17_control'
+    description_loader.gz_control_config_file_path = 'config/sciurus17_controllers.yaml'
     description = description_loader.load()
 
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        output='screen',
-        parameters=[{'robot_description': description}]
-    )
+    move_group = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                get_package_share_directory('sciurus17_moveit_config'),
+                '/launch/move_group.launch.py']),
+            launch_arguments={'loaded_description': description}.items()
+        )
+
+    spawn_joint_state_broadcaster = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner joint_state_broadcaster'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_right_arm_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner right_arm_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_right_hand_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner right_hand_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_left_arm_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner left_arm_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_left_hand_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner left_hand_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_neck_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner neck_controller'],
+                shell=True,
+                output='screen',
+            )
+
+    spawn_waist_yaw_controller = ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner waist_yaw_controller'],
+                shell=True,
+                output='screen',
+            )
 
     bridge = Node(
                 package='ros_gz_bridge',
@@ -71,6 +117,13 @@ def generate_launch_description():
         SetParameter(name='use_sim_time', value=True),
         ign_gazebo,
         ignition_spawn_entity,
-        robot_state_publisher,
+        spawn_joint_state_broadcaster,
+        spawn_right_arm_controller,
+        spawn_right_hand_controller,
+        spawn_left_arm_controller,
+        spawn_left_hand_controller,
+        spawn_neck_controller,
+        spawn_waist_yaw_controller,
+        move_group,
         bridge
     ])