crane_plushのjazzy対応を参考に設定ファイルをjazzy対応

crane_x7_moveit_config/config/controllers.yaml

@@ -1,23 +1,27 @@
-controller_names:
-  - crane_x7_arm_controller
-  - crane_x7_gripper_controller
+# MoveIt uses this configuration for controller management
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
 
-crane_x7_arm_controller:
-  action_ns: follow_joint_trajectory
-  type: FollowJointTrajectory
-  default: true
-  joints:
-    - crane_x7_shoulder_fixed_part_pan_joint
-    - crane_x7_shoulder_revolute_part_tilt_joint
-    - crane_x7_upper_arm_revolute_part_twist_joint
-    - crane_x7_upper_arm_revolute_part_rotate_joint
-    - crane_x7_lower_arm_fixed_part_joint
-    - crane_x7_lower_arm_revolute_part_joint
-    - crane_x7_wrist_joint
+moveit_simple_controller_manager:
+  controller_names:
+    - crane_x7_arm_controller
+    - crane_x7_gripper_controller
 
-crane_x7_gripper_controller:
-  action_ns: gripper_cmd
-  type: GripperCommand
-  default: true
-  joints:
-    - crane_x7_gripper_finger_a_joint
+  crane_x7_arm_controller:
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - crane_x7_shoulder_fixed_part_pan_joint
+      - crane_x7_shoulder_revolute_part_tilt_joint
+      - crane_x7_upper_arm_revolute_part_twist_joint
+      - crane_x7_upper_arm_revolute_part_rotate_joint
+      - crane_x7_lower_arm_fixed_part_joint
+      - crane_x7_lower_arm_revolute_part_joint
+      - crane_x7_wrist_joint
+
+  crane_x7_gripper_controller:
+    action_ns: gripper_cmd
+    type: GripperCommand
+    default: true
+    joints:
+      - crane_x7_gripper_finger_a_joint

crane_x7_moveit_config/config/kinematics.yaml

@@ -2,3 +2,4 @@ arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3

crane_x7_moveit_config/config/ompl_planning.yaml

@@ -1,4 +1,25 @@
+planning_plugins:
+  - ompl_interface/OMPLPlanner
+# To optionally use Ruckig for jerk-limited smoothing, add this line to the request adapters below
+# default_planning_request_adapters/AddRuckigTrajectorySmoothing
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
+
 planner_configs:
+  APSConfigDefault:
+    type: geometric::AnytimePathShortening
+    shortcut: 1  # Attempt to shortcut all new solution paths
+    hybridize: 1  # Compute hybrid solution trajectories
+    max_hybrid_paths: 36  # Number of hybrid paths generated per iteration
+    num_planners: 8  # The number of default planners to use for planning
+    planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect"  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
   SBLkConfigDefault:
     type: geometric::SBL
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@@ -122,6 +143,7 @@ planner_configs:
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
 arm:
   planner_configs:
+    - APSConfigDefault
     - SBLkConfigDefault
     - ESTkConfigDefault
     - LBKPIECEkConfigDefault
@@ -147,6 +169,7 @@ arm:
     - SPARStwokConfigDefault
 gripper:
   planner_configs:
+    - APSConfigDefault
     - SBLkConfigDefault
     - ESTkConfigDefault
     - LBKPIECEkConfigDefault

crane_x7_moveit_config/launch/run_move_group.launch.py

@@ -1,4 +1,4 @@
-# Copyright 2022 RT Corporation
+# Copyright 2020 RT Corporation
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -15,15 +15,16 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-from crane_x7_description.robot_description_loader import RobotDescriptionLoader
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-import yaml
-
-# Reference: https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/
-# examples/move_group_interface/launch/move_group.launch.py
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg
+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
 
 
 def load_file(package_name, file_path):
@@ -33,7 +34,9 @@ def load_file(package_name, file_path):
     try:
         with open(absolute_file_path, 'r') as file:
             return file.read()
-    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+    except (
+        EnvironmentError
+    ):  # parent of IOError, OSError *and* WindowsError where available
         return None
 
 
@@ -44,114 +47,81 @@ def load_yaml(package_name, file_path):
     try:
         with open(absolute_file_path, 'r') as file:
             return yaml.safe_load(file)
-    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+    except (
+        EnvironmentError
+    ):  # parent of IOError, OSError *and* WindowsError where available
         return None
 
 
 def generate_launch_description():
-    description_loader = RobotDescriptionLoader()
+    ld = LaunchDescription()
 
     declare_loaded_description = DeclareLaunchArgument(
         'loaded_description',
-        default_value=description_loader.load(),
+        default_value='',
         description='Set robot_description text.  \
-                     It is recommended to use RobotDescriptionLoader() in crane_x7_description.'
+                     It is recommended to use RobotDescriptionLoader() in  \
+                        crane_x7_description.',
     )
 
-    declare_rviz_config_file = DeclareLaunchArgument(
-        'rviz_config_file',
-        default_value=get_package_share_directory(
-            'crane_x7_moveit_config') + '/launch/run_move_group.rviz',
-        description='Set the path to rviz configuration file.'
+    ld.add_action(declare_loaded_description)
+
+    ld.add_action(DeclareBooleanLaunchArg('debug', default_value=False))
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            'rviz_config',
+            default_value=get_package_share_directory('crane_x7_moveit_config')
+            + '/config/moveit.rviz',
+            description='Set the path to rviz configuration file.',
+        )
     )
 
-    robot_description = {'robot_description': LaunchConfiguration('loaded_description')}
+    moveit_config = (
+        MoveItConfigsBuilder('crane_x7')
+        .planning_scene_monitor(
+            publish_robot_description=True,
+            publish_robot_description_semantic=True,
+        )
+        .robot_description(
+            file_path=os.path.join(
+                get_package_share_directory('crane_x7_description'),
+                'urdf',
+                'crane_x7.urdf.xacro',
+            ),
+            mappings={},
+        )
+        .robot_description_semantic(
+            file_path='config/crane_x7.srdf',
+            mappings={'model': 'crane_x7'},
+        )
+        .joint_limits(file_path='config/joint_limits.yaml')
+        .trajectory_execution(
+            file_path='config/controllers.yaml', moveit_manage_controllers=True
+        )
+        .planning_pipelines(pipelines=['ompl'])
+        .robot_description_kinematics(file_path='config/kinematics.yaml')
+        .to_moveit_configs()
+    )
 
-    robot_description_semantic_config = load_file(
-        'crane_x7_moveit_config', 'config/crane_x7.srdf')
-    robot_description_semantic = {
-        'robot_description_semantic': robot_description_semantic_config}
+    moveit_config.robot_description = {
+        'robot_description': LaunchConfiguration('loaded_description')
+    }
 
-    joint_limits_yaml = load_yaml(
-        "crane_x7_moveit_config", "config/joint_limits.yaml"
-    )
-    robot_description_planning = {
-        "robot_description_planning": joint_limits_yaml}
-
-    kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml')
-
-    # Planning Functionality
-    ompl_planning_pipeline_config = {'move_group': {
-        'planning_plugin': 'ompl_interface/OMPLPlanner',
-        'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization \
-                               default_planner_request_adapters/FixWorkspaceBounds \
-                               default_planner_request_adapters/FixStartStateBounds \
-                               default_planner_request_adapters/FixStartStateCollision \
-                               default_planner_request_adapters/FixStartStatePathConstraints',
-        'start_state_max_bounds_error': 0.1}}
-    ompl_planning_yaml = load_yaml('crane_x7_moveit_config', 'config/ompl_planning.yaml')
-    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
-
-    # Trajectory Execution Functionality
-    controllers_yaml = load_yaml('crane_x7_moveit_config', 'config/controllers.yaml')
-    moveit_controllers = {
-        'moveit_simple_controller_manager': controllers_yaml,
-        'moveit_controller_manager':
-            'moveit_simple_controller_manager/MoveItSimpleControllerManager'}
-
-    trajectory_execution = {'moveit_manage_controllers': True,
-                            'trajectory_execution.allowed_execution_duration_scaling': 1.2,
-                            'trajectory_execution.allowed_goal_duration_margin': 0.5,
-                            'trajectory_execution.allowed_start_tolerance': 0.1}
-
-    planning_scene_monitor_parameters = {'publish_planning_scene': True,
-                                         'publish_geometry_updates': True,
-                                         'publish_state_updates': True,
-                                         'publish_transforms_updates': True}
-
-    # Start the actual move_group node/action server
-    run_move_group_node = Node(package='moveit_ros_move_group',
-                               executable='move_group',
-                               output='screen',
-                               parameters=[robot_description,
-                                           robot_description_semantic,
-                                           robot_description_planning,
-                                           kinematics_yaml,
-                                           ompl_planning_pipeline_config,
-                                           trajectory_execution,
-                                           moveit_controllers,
-                                           planning_scene_monitor_parameters])
+    moveit_config.move_group_capabilities = {
+        'capabilities': ''
+    }
+
+    # Move group
+    ld.add_entity(generate_move_group_launch(moveit_config))
 
     # RViz
-    rviz_config_file = LaunchConfiguration('rviz_config_file')
-    rviz_node = Node(package='rviz2',
-                     executable='rviz2',
-                     name='rviz2',
-                     output='log',
-                     arguments=['-d', rviz_config_file],
-                     parameters=[robot_description,
-                                 robot_description_semantic,
-                                 ompl_planning_pipeline_config,
-                                 kinematics_yaml])
+    ld.add_entity(generate_moveit_rviz_launch(moveit_config))
 
     # Static TF
-    static_tf = 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
-    robot_state_publisher = Node(package='robot_state_publisher',
-                                 executable='robot_state_publisher',
-                                 name='robot_state_publisher',
-                                 output='both',
-                                 parameters=[robot_description])
-
-    return LaunchDescription([declare_loaded_description,
-                              declare_rviz_config_file,
-                              run_move_group_node,
-                              rviz_node,
-                              static_tf,
-                              robot_state_publisher,
-                              ])
+    ld.add_entity(generate_rsp_launch(moveit_config))
+
+    return ld