Go to the documentation of this file.00001
00002
00003 import sys
00004 import yaml
00005 import roslib
00006 import re
00007
00008
00009 if(len(sys.argv) != 2):
00010 print("Usage: create_launch_files <your_robot_name>")
00011 sys.exit(-1)
00012
00013 robotName = sys.argv[1]
00014 directoryName = roslib.packages.get_pkg_dir(robotName + "_arm_navigation")
00015
00016
00017 yamlFileName = directoryName + '/config/' + robotName + '_planning_description.yaml'
00018
00019
00020 data = yaml.safe_load(open(yamlFileName, 'r'))
00021
00022
00023 multi_dof = data['multi_dof_joints']
00024 world_joint = multi_dof[0]
00025 world_frame = world_joint['parent_frame_id']
00026
00027
00028 groups = data['groups']
00029
00030 if(len(groups) < 1) :
00031 print("Need at least 1 planning group in the robot to continue!")
00032 sys.exit(-1)
00033
00034
00035 left_arm = None
00036 right_arm = None
00037 left_arm_name = 'none'
00038 right_arm_name = 'none'
00039 left_arm_ik = 'none'
00040 right_arm_ik = 'none'
00041
00042 done_left = False
00043 done_right = False
00044
00045 for i in groups:
00046 if 'tip_link' in i:
00047 if not done_left:
00048 left_arm = i
00049 done_left = True
00050 else:
00051 done_right = True
00052 right_arm = i
00053 break
00054
00055 left_arm_name = left_arm['name']
00056 left_arm_ik = left_arm['tip_link']
00057
00058 if done_right:
00059 right_arm_name = right_arm['name']
00060 right_arm_ik = right_arm['tip_link']
00061
00062
00063
00064 if(re.search("right | Right | RIGHT | ^r_ | _r$ | ^R_ | _R$", left_arm_name)) :
00065 temp = left_arm_name
00066 left_arm_name = right_arm_name
00067 right_arm_name = temp
00068 temp = left_arm_ik
00069 left_arm_ik = right_arm_ik
00070 right_arm_ik = temp
00071
00072 template = open(roslib.packages.get_pkg_dir('move_arm_warehouse')+'/scripts/planning_scene_warehouse_viewer_template.launch', 'r')
00073 text = template.read()
00074
00075 text = re.sub('__ROBOT_NAME__', sys.argv[1], text)
00076 text = re.sub('__LEFT_GROUP__', left_arm_name, text)
00077 text = re.sub('__RIGHT_GROUP__', right_arm_name, text)
00078 text = re.sub('__LEFT_IK__', left_arm_ik, text)
00079 text = re.sub('__RIGHT_IK__', right_arm_ik, text)
00080
00081
00082 if right_arm_name == 'none':
00083 text = re.sub('/l_interpolated_ik_motion_plan', 'none', text)
00084 text = re.sub('/r_interpolated_ik_motion_plan', 'none', text)
00085
00086 launch = open(directoryName+'/launch/planning_scene_warehouse_viewer_'+sys.argv[1]+'.launch', 'w')
00087 launch.write(text)
00088
00089 launch.close()
00090 template.close()
00091
00092
00093
00094 rviz_template = open(roslib.packages.get_pkg_dir('move_arm_warehouse')+'/scripts/planning_scene_warehouse_viewer_template.vcg', 'r')
00095 rviz_text = rviz_template.read()
00096 rviz_text = re.sub('__WORLD_FRAME__', world_frame, rviz_text)
00097 out_vcg = open(directoryName+'/config/planning_scene_warehouse_viewer.vcg', 'w')
00098 out_vcg.write(rviz_text)
00099 out_vcg.close()
00100 rviz_template.close()
00101
00102 exit(0)