From 3f82cd396fa2b5595933bb1b22177019fbeb662b Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 14:52:25 +0900 Subject: [PATCH] =?UTF-8?q?crane=5Fx7=5Fexamples=5Fpy=E3=81=AE=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E8=B5=B7=E5=8B=95=E7=94=A8launch?= =?UTF-8?q?=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB=E5=BE=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_x7_examples_py/launch/example.launch.py | 77 +++++++------------ 1 file changed, 26 insertions(+), 51 deletions(-) diff --git a/crane_x7_examples_py/launch/example.launch.py b/crane_x7_examples_py/launch/example.launch.py index 463f851..6a22c14 100644 --- a/crane_x7_examples_py/launch/example.launch.py +++ b/crane_x7_examples_py/launch/example.launch.py @@ -15,105 +15,80 @@ import os from ament_index_python.packages import get_package_share_directory -from crane_x7_description.robot_description_loader import RobotDescriptionLoader +from crane_x7_description.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 -from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # declare_loaded_description = DeclareLaunchArgument( - # "loaded_description", - # default_value="", - # description="Set robot_description text. \ - # It is recommended to use RobotDescriptionLoader() in crane_x7_description.", - # ) ld = LaunchDescription() description_loader = RobotDescriptionLoader() ld.add_action( DeclareLaunchArgument( - "loaded_description", + 'loaded_description', default_value=description_loader.load(), - description="Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_x7_description.", + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in crane_x7_description.', ) ) moveit_config = ( - MoveItConfigsBuilder("crane_x7") + 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", + get_package_share_directory('crane_x7_description'), + 'urdf', + 'crane_x7.urdf.xacro', ), mappings={}, ) .robot_description_kinematics( - file_path=get_package_share_directory("crane_x7_moveit_config") - + "/config/kinematics.yaml" + file_path=get_package_share_directory('crane_x7_moveit_config') + + '/config/kinematics.yaml' ) - # .robot_description_semantic( - # file_path="config/crsane_plus.srdf", - # mappings={"model": "crane_x7"}, - # ) - # .joint_limits(file_path="config/joint_limits.yaml") .trajectory_execution( - file_path=get_package_share_directory("crane_x7_moveit_config") - + "/config/controllers.yaml" + file_path=get_package_share_directory('crane_x7_moveit_config') + + '/config/controllers.yaml' ) - # .planning_pipelines(pipelines=["ompl"]) .moveit_cpp( - file_path=get_package_share_directory("crane_x7_examples_py") - + "/config/crane_x7_moveit_py_examples.yaml" + file_path=get_package_share_directory('crane_x7_examples_py') + + '/config/crane_x7_moveit_py_examples.yaml' ) .to_moveit_configs() ) moveit_config.robot_description = { - "robot_description": LaunchConfiguration("loaded_description") + 'robot_description': LaunchConfiguration('loaded_description') } - moveit_config.move_group_capabilities = {"capabilities": ""} + moveit_config.move_group_capabilities = {'capabilities': ''} declare_example_name = DeclareLaunchArgument( - "example", - default_value="gripper_control", + 'example', + default_value='gripper_control', description=( - "Set an example executable name: " - "[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]" + 'Set an example executable name: ' + '[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]' ), ) - # declare_use_sim_time = DeclareLaunchArgument( - # "use_sim_time", - # default_value="false", - # description=("Set true when using the gazebo simulator."), - # ) - - # use_sim_time_name = SetParameter( - # name="use_sim_time", value=LaunchConfiguration("use_sim_time") - # ) - example_node = Node( - name=[LaunchConfiguration("example"), "_node"], - package="crane_x7_examples_py", - executable=LaunchConfiguration("example"), - output="screen", + name=[LaunchConfiguration('example'), '_node'], + package='crane_x7_examples_py', + executable=LaunchConfiguration('example'), + output='screen', parameters=[moveit_config.to_dict()], ) - # ld = LaunchDescription([SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time'))]) - # ld.add_action(declare_use_sim_time) - # ld.add_action(use_sim_time_name) - # ld.add_action(declare_loaded_description) ld.add_action(declare_example_name) ld.add_action(example_node)