create_launch_files.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import sys
00004 import yaml
00005 import roslib
00006 import re
00007 
00008 # Expect robot name as argument
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 # Plnning description yaml file
00017 yamlFileName = directoryName + '/config/' + robotName + '_planning_description.yaml'
00018 
00019 # Get dictionary of yaml defs
00020 data = yaml.safe_load(open(yamlFileName, 'r'))
00021 
00022 #getting world frame for rviz
00023 multi_dof = data['multi_dof_joints']
00024 world_joint = multi_dof[0]
00025 world_frame = world_joint['parent_frame_id']
00026 
00027 # List of groups
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 # Substitution strings
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 # Switch left and right arms if we find any of the following strings in the left arm
00063 # this is a terrible hack, but by convention we assume one of these strings is in the right arm
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 #interpolated ik will only work with two arms for now
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 #Now we do the rviz template 
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)


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Fri Dec 6 2013 21:12:33