2626
2727
2828import os
29- import yaml
30- import xacro
3129
30+ from ament_index_python .packages import get_package_share_directory
3231from launch import LaunchDescription
3332from launch .actions import DeclareLaunchArgument
3433from launch .substitutions import LaunchConfiguration
3534from launch_ros .actions import Node
36- from ament_index_python .packages import get_package_share_directory
35+ import xacro
36+ import yaml
3737
3838
3939def generate_launch_description ():
4040 # Robot description
4141 robot_description_config = xacro .process_file (
4242 os .path .join (
43- get_package_share_directory (" turtlebot3_manipulation_description" ),
44- " urdf" ,
45- " turtlebot3_manipulation.urdf.xacro" ,
43+ get_package_share_directory (' turtlebot3_manipulation_description' ),
44+ ' urdf' ,
45+ ' turtlebot3_manipulation.urdf.xacro' ,
4646 )
4747 )
48- robot_description = {" robot_description" : robot_description_config .toxml ()}
48+ robot_description = {' robot_description' : robot_description_config .toxml ()}
4949
5050 # Robot description Semantic config
5151 robot_description_semantic_path = os .path .join (
52- get_package_share_directory (" turtlebot3_manipulation_moveit_config" ),
53- " config" ,
54- " turtlebot3_manipulation.srdf" ,
52+ get_package_share_directory (' turtlebot3_manipulation_moveit_config' ),
53+ ' config' ,
54+ ' turtlebot3_manipulation.srdf' ,
5555 )
56- with open (robot_description_semantic_path , "r" ) as file :
56+ with open (robot_description_semantic_path , 'r' ) as file :
5757 robot_description_semantic_config = file .read ()
5858
5959 robot_description_semantic = {
60- " robot_description_semantic" : robot_description_semantic_config
60+ ' robot_description_semantic' : robot_description_semantic_config
6161 }
6262
6363 # kinematics yaml
6464 kinematics_yaml_path = os .path .join (
65- get_package_share_directory (" turtlebot3_manipulation_moveit_config" ),
66- " config" ,
67- " kinematics.yaml" ,
65+ get_package_share_directory (' turtlebot3_manipulation_moveit_config' ),
66+ ' config' ,
67+ ' kinematics.yaml' ,
6868 )
69- with open (kinematics_yaml_path , "r" ) as file :
69+ with open (kinematics_yaml_path , 'r' ) as file :
7070 kinematics_yaml = yaml .safe_load (file )
7171
7272 # Planning Functionality
7373 # Planning Functionality
7474 ompl_planning_pipeline_config = {
75- "move_group" : {
76- "planning_plugin" : "ompl_interface/OMPLPlanner" ,
77- "request_adapters" : """default_planner_request_adapters/AddTimeOptimalParameterization \
78- default_planner_request_adapters/FixWorkspaceBounds \
79- default_planner_request_adapters/FixStartStateBounds \
80- default_planner_request_adapters/FixStartStateCollision \
81- default_planner_request_adapters/FixStartStatePathConstraints""" ,
82- "start_state_max_bounds_error" : 0.1 ,
75+ 'move_group' : {
76+ 'planning_plugin' : 'ompl_interface/OMPLPlanner' ,
77+ 'request_adapters' : (
78+ 'default_planner_request_adapters/AddTimeOptimalParameterization '
79+ 'default_planner_request_adapters/FixWorkspaceBounds '
80+ 'default_planner_request_adapters/FixStartStateBounds '
81+ 'default_planner_request_adapters/FixStartStateCollision '
82+ 'default_planner_request_adapters/FixStartStatePathConstraints'
83+ ),
84+ 'start_state_max_bounds_error' : 0.1 ,
8385 }
8486 }
8587 ompl_planning_yaml_path = os .path .join (
86- get_package_share_directory (" turtlebot3_manipulation_moveit_config" ),
87- " config" ,
88- " ompl_planning.yaml" ,
88+ get_package_share_directory (' turtlebot3_manipulation_moveit_config' ),
89+ ' config' ,
90+ ' ompl_planning.yaml' ,
8991 )
90- with open (ompl_planning_yaml_path , "r" ) as file :
92+ with open (ompl_planning_yaml_path , 'r' ) as file :
9193 ompl_planning_yaml = yaml .safe_load (file )
92- ompl_planning_pipeline_config [" move_group" ].update (ompl_planning_yaml )
94+ ompl_planning_pipeline_config [' move_group' ].update (ompl_planning_yaml )
9395
9496 # Trajectory Execution
9597 trajectory_execution = {
96- " moveit_manage_controllers" : True ,
97- " trajectory_execution.allowed_execution_duration_scaling" : 1.2 ,
98- " trajectory_execution.allowed_goal_duration_margin" : 0.5 ,
99- " trajectory_execution.allowed_start_tolerance" : 0.05 ,
98+ ' moveit_manage_controllers' : True ,
99+ ' trajectory_execution.allowed_execution_duration_scaling' : 1.2 ,
100+ ' trajectory_execution.allowed_goal_duration_margin' : 0.5 ,
101+ ' trajectory_execution.allowed_start_tolerance' : 0.05 ,
100102 }
101103
102104 # Moveit Controllers
103105 moveit_simple_controllers_yaml_path = os .path .join (
104- get_package_share_directory (" turtlebot3_manipulation_moveit_config" ),
105- " config" ,
106- " moveit_controllers.yaml" ,
106+ get_package_share_directory (' turtlebot3_manipulation_moveit_config' ),
107+ ' config' ,
108+ ' moveit_controllers.yaml' ,
107109 )
108- with open (moveit_simple_controllers_yaml_path , "r" ) as file :
110+ with open (moveit_simple_controllers_yaml_path , 'r' ) as file :
109111 moveit_simple_controllers_yaml = yaml .safe_load (file )
110112
111113 moveit_controllers = {
112- " moveit_simple_controller_manager" : moveit_simple_controllers_yaml ,
113- " moveit_controller_manager" :
114- " moveit_simple_controller_manager/MoveItSimpleControllerManager" ,
114+ ' moveit_simple_controller_manager' : moveit_simple_controllers_yaml ,
115+ ' moveit_controller_manager' :
116+ ' moveit_simple_controller_manager/MoveItSimpleControllerManager' ,
115117 }
116118
117119 # Planning Scene Monitor Parameters
118120 planning_scene_monitor_parameters = {
119- " publish_planning_scene" : True ,
120- " publish_geometry_updates" : True ,
121- " publish_state_updates" : True ,
122- " publish_transforms_updates" : True ,
123- " publish_robot_description" : True ,
124- " publish_robot_description_semantic" : True
121+ ' publish_planning_scene' : True ,
122+ ' publish_geometry_updates' : True ,
123+ ' publish_state_updates' : True ,
124+ ' publish_transforms_updates' : True ,
125+ ' publish_robot_description' : True ,
126+ ' publish_robot_description_semantic' : True
125127 }
126128
127129 ld = LaunchDescription ()
@@ -133,9 +135,9 @@ def generate_launch_description():
133135 ld .add_action (declare_use_sim )
134136
135137 move_group_node = Node (
136- package = " moveit_ros_move_group" ,
137- executable = " move_group" ,
138- output = " screen" ,
138+ package = ' moveit_ros_move_group' ,
139+ executable = ' move_group' ,
140+ output = ' screen' ,
139141 parameters = [
140142 robot_description ,
141143 robot_description_semantic ,
0 commit comments