diff --git a/crane_plus_moveit_config/launch/run_move_group.launch.py b/crane_plus_moveit_config/launch/run_move_group.launch.py index fb4e9a8..e20593c 100755 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/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))