From b16cedeabdc2bf2767358f2d545de0d1a2ef8008 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 23 Dec 2024 11:46:59 +0900 Subject: [PATCH] =?UTF-8?q?=E6=8C=87=E6=91=98=E4=BA=8B=E9=A0=85=E3=82=92?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/example.launch.py | 28 +++++++++++-------- .../pick_and_place_left_arm.py | 13 ++++----- .../pick_and_place_right_arm_waist.py | 13 ++++----- 3 files changed, 29 insertions(+), 25 deletions(-) diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index c5bd907..f459f57 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -20,22 +20,24 @@ from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from launch.conditions import IfCondition def generate_launch_description(): - + description_loader = RobotDescriptionLoader() - declare_loaded_description = DeclareLaunchArgument('loaded_description', - default_value=description_loader.load(), - description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() \ - in sciurus17_description.') + declare_loaded_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in sciurus17_description.') moveit_config = (MoveItConfigsBuilder('sciurus17').planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, - ).moveit_cpp(file_path=get_package_share_directory('sciurus17_examples_py') + - '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs()) + ).moveit_cpp(file_path=get_package_share_directory('sciurus17_examples_py') + + '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs()) moveit_config.robot_description = { 'robot_description': LaunchConfiguration('loaded_description') @@ -54,19 +56,23 @@ def generate_launch_description(): 'use_sim_time', default_value='false', description=('Set true when using the gazebo simulator.') ) - + + # 下記Issue対応のためここでパラメータを設定する + # https://github.com/moveit/moveit2/issues/2940#issuecomment-2401302214 + config_dict = moveit_config.to_dict() + config_dict.update({'use_sim_time':LaunchConfiguration('use_sim_time')}) + example_node = Node( name=[LaunchConfiguration('example'), '_node'], package='sciurus17_examples_py', executable=LaunchConfiguration('example'), output='screen', - parameters=[moveit_config.to_dict()], + parameters=[config_dict], ) return LaunchDescription([ declare_loaded_description, declare_use_sim_time, - SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), declare_example_name, example_node ]) diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py index 5c58b26..f136799 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py @@ -23,7 +23,6 @@ import rclpy from rclpy.logging import get_logger from sciurus17_examples_py.utils import plan_and_execute -from std_msgs.msg import Header def main(args=None): @@ -95,7 +94,7 @@ def main(args=None): # 物体の上に腕を伸ばす l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -108,7 +107,7 @@ def main(args=None): # 掴みに行く l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = GRASP_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -133,7 +132,7 @@ def main(args=None): # 持ち上げる l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -146,7 +145,7 @@ def main(args=None): # 移動する l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -159,7 +158,7 @@ def main(args=None): # 下ろす l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = RELEASE_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( @@ -184,7 +183,7 @@ def main(args=None): # ハンドを持ち上げる l_arm.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') plan_and_execute( diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py index 5cff84e..bba7737 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py @@ -24,7 +24,6 @@ import rclpy from rclpy.logging import get_logger from sciurus17_examples_py.utils import plan_and_execute -from std_msgs.msg import Header def main(args=None): @@ -108,7 +107,7 @@ def main(args=None): r_arm_waist.set_path_constraints(path_constraints=constraints) r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -121,7 +120,7 @@ def main(args=None): # 掴みに行く r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = GRASP_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -146,7 +145,7 @@ def main(args=None): # 持ち上げる r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_GRASP_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -159,7 +158,7 @@ def main(args=None): # 移動する r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -172,7 +171,7 @@ def main(args=None): # 下ろす r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = RELEASE_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute( @@ -197,7 +196,7 @@ def main(args=None): # ハンドを持ち上げる r_arm_waist.set_start_state_to_current_state() goal_pose = PoseStamped() - goal_pose.header.frame_id='base_link' + goal_pose.header.frame_id = 'base_link' goal_pose.pose = PRE_AND_POST_RELEASE_POSE r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') plan_and_execute(