run_move_froup.lauch.pyの整理

crane_plus_moveit_config/launch/run_move_group.launch.py

@@ -18,26 +18,13 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
 import yaml
 from moveit_configs_utils import MoveItConfigsBuilder
-
 from moveit_configs_utils.launches import generate_move_group_launch
 from moveit_configs_utils.launches import generate_moveit_rviz_launch
-
 from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
-
 from moveit_configs_utils.launches import generate_rsp_launch
-
-from moveit_configs_utils.launch_utils import (
-    add_debuggable_node,
-    DeclareBooleanLaunchArg,
-)
-
-
-# Reference: https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes/
-# run_move_group/launch/run_move_group.launch.py
-
+from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg
 
 def load_file(package_name, file_path):
     package_path = get_package_share_directory(package_name)
@@ -77,11 +64,23 @@ def generate_launch_description():
 
     ld.add_action(declare_loaded_description)
 
-    # MoveItConfigBuilderによるパラメータ設定
+    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "rviz_config",
+            default_value=get_package_share_directory("crane_plus_moveit_config")
+            + "/config/moveit.rviz",
+            description="Set the path to rviz configuration file.",
+        )
+    )
+
     moveit_config = (
-        # ロボット名の定義
         MoveItConfigsBuilder("crane_plus")
-        # URDFの設定
+        .planning_scene_monitor(
+            publish_robot_description=True,
+            publish_robot_description_semantic=True,
+        )
         .robot_description(
             file_path=os.path.join(
                 get_package_share_directory("crane_plus_description"),
@@ -90,29 +89,15 @@ def generate_launch_description():
             ),
             mappings={},
         )
-        # SRDFの設定
         .robot_description_semantic(
             file_path="config/crane_plus.srdf",
             mappings={"model": "crane_plus"},
         )
         .joint_limits(file_path="config/joint_limits.yaml")
-        # Planning Sceneのトピックの設定
-        .planning_scene_monitor(
-            publish_planning_scene=True,
-            publish_geometry_updates=True,
-            publish_state_updates=True,
-            publish_transforms_updates=True,
-            publish_robot_description=True,
-            publish_robot_description_semantic=True,
-        )
-        # 軌道追従制御ノードの設定
         .trajectory_execution(
-            file_path="config/controllers.yaml",
-            moveit_manage_controllers=True
+            file_path="config/controllers.yaml", moveit_manage_controllers=True
         )
-        # 軌道計画のプラグイン設定
-        .planning_pipelines(pipelines=["ompl"], default_planning_pipeline="ompl")
-        # キネマティクスの設定
+        .planning_pipelines(pipelines=["ompl"])
         .robot_description_kinematics(file_path="config/kinematics.yaml")
         .to_moveit_configs()
     )
@@ -121,33 +106,18 @@ def generate_launch_description():
         "robot_description": LaunchConfiguration("loaded_description")
     }
 
-    moveit_config.move_group_capabilities = {"capabilities": ""}
+    moveit_config.move_group_capabilities = {
+        "capabilities": ""
+    }
 
     # Move group
     ld.add_entity(generate_move_group_launch(moveit_config))
 
     # RViz
     ld.add_entity(generate_moveit_rviz_launch(moveit_config))
-    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
-    ld.add_action(
-        DeclareLaunchArgument(
-            "rviz_config",
-            default_value=get_package_share_directory("crane_plus_moveit_config")
-            + "/config/moveit.rviz",
-            description="Set the path to rviz configuration file.",
-        )
-    )
-
+
     # Static TF
-    ld.add_action(
-        Node(
-            package="tf2_ros",
-            executable="static_transform_publisher",
-            name="static_transform_publisher",
-            output="log",
-            arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
-        )
-    )
+    ld.add_entity(generate_static_virtual_joint_tfs_launch(moveit_config))
 
     # Publish TF
     ld.add_entity(generate_rsp_launch(moveit_config))