Go to the documentation of this file.00001
00002
00003 import sys
00004 import yaml
00005 import roslib
00006 import re
00007 import os
00008 from lxml import etree
00009
00010 if __name__ == '__main__':
00011
00012 if(len(sys.argv) != 2):
00013 print("Usage: create_ik_fast_plugin.py <your_robot_name>")
00014 sys.exit(-1)
00015
00016 robot_name = sys.argv[1]
00017 directory_name = roslib.packages.get_pkg_dir(robot_name + "_arm_navigation")
00018
00019 if(not roslib.packages.is_pkg_dir(directory_name)):
00020 print(robot_name + " package does not exist ")
00021 sys.exit(-2)
00022
00023 yaml_file_name = directory_name + '/config/' + robot_name + '_planning_description.yaml'
00024
00025
00026 data = yaml.safe_load(open(yaml_file_name, 'r'))
00027
00028
00029 groups = data['groups']
00030
00031 if(len(groups) < 1) :
00032 print("Need at least 1 planning group in the robot to continue!")
00033 sys.exit(-1)
00034
00035
00036 arm_kinematics_directory = roslib.packages.get_pkg_dir("arm_kinematics_constraint_aware")
00037
00038 template_file_name = arm_kinematics_directory+"/templates/ik_fast_plugin_template.cxx"
00039
00040 for i in groups:
00041 if 'tip_link' in i:
00042 template_file_data = open(template_file_name, 'r')
00043 group_name = i['name']
00044 template_text = template_file_data.read()
00045 template_text = re.sub('_ROBOT_NAME_', robot_name, template_text)
00046 template_text = re.sub('_GROUP_NAME_', group_name, template_text)
00047
00048 if not os.path.exists(directory_name+'/src'):
00049 os.makedirs(directory_name+'/src')
00050
00051 output_template = open(directory_name+'/src/'+robot_name+"_"+group_name+"_ikfast_plugin.cpp",'w')
00052 output_template.write(template_text)
00053
00054
00055
00056 plugin_file = open(directory_name+"/kinematics_plugins.xml",'w')
00057
00058 root_one = etree.Element("library", path="lib/lib"+robot_name+"_kinematics_lib")
00059
00060 for i in groups:
00061 if 'tip_link' in i:
00062 group_name = i['name']
00063 cl = etree.SubElement(root_one, "class")
00064 cl.set("name", robot_name+'_'+group_name+'_kinematics/IKFastKinematicsPlugin')
00065 cl.set("type", robot_name+'_'+group_name+'_kinematics::IKFastKinematicsPlugin')
00066 cl.set("base_class_type", "kinematics::KinematicsBase")
00067 desc = etree.SubElement(cl, "description")
00068 desc.text = "A plugin created by using OpenRAVE's IK Fast component"
00069
00070 etree.ElementTree(root_one).write(plugin_file, xml_declaration=True, pretty_print=True)
00071
00072
00073
00074 cmake_file = open(directory_name+"/CMakeLists.txt", 'a+')
00075 cmake_file_text = cmake_file.read()
00076 if re.search(robot_name+'_kinematics_lib', cmake_file_text) == None:
00077 extra_text = '\n#Library containing ikfast plugins\nrosbuild_add_library('+robot_name+'_kinematics_lib\n'
00078 for i in groups:
00079 if 'tip_link' in i:
00080 group_name = i['name']
00081 extra_text += ' src/'+robot_name+"_"+group_name+'_ikfast_plugin.cpp\n'
00082
00083 extra_text += " )"
00084
00085 cmake_file.write(extra_text)
00086
00087 cmake_file.close()
00088
00089 parser = etree.XMLParser(remove_blank_text=True)
00090
00091
00092 manifest_xml = etree.parse(directory_name+"/manifest.xml", parser)
00093
00094
00095 found = False
00096 for depend_entry in manifest_xml.getroot().findall("depend"):
00097 if depend_entry.get("package") == "kinematics_base":
00098 found = True
00099 break
00100
00101 if not found:
00102 for idx, entry in enumerate(manifest_xml.getroot().getchildren()):
00103 if entry.tag == "depend":
00104 manifest_xml.getroot().insert(idx, etree.Element("depend", package="kinematics_base"))
00105 break
00106
00107 export_element = manifest_xml.getroot().find("export")
00108 if export_element == None:
00109 export_element = etree.SubElement(manifest_xml.getroot(), "export")
00110
00111 kinematics_element = export_element.find("kinematics_base")
00112 if kinematics_element == None:
00113 kinematics_element = etree.SubElement(export_element, "kinematics_base", plugin="${prefix}/kinematics_plugins.xml")
00114
00115 manifest_xml_file = open(directory_name+"/manifest.xml", "w")
00116 manifest_xml.write(manifest_xml_file, xml_declaration=True, pretty_print=True)
00117
00118
00119
00120 constraint_aware_xml = etree.parse(directory_name+"/launch/constraint_aware_kinematics.launch", parser)
00121
00122 for node_entry in constraint_aware_xml.getroot().findall("node"):
00123 for param_entry in node_entry.getchildren():
00124 if param_entry.get("name") == "group":
00125 group_name = param_entry.get("value")
00126 for param_entry in node_entry.getchildren():
00127 if param_entry.get("name") == "kinematics_solver":
00128 param_entry.set("value", robot_name+"_"+group_name+"_kinematics/IKFastKinematicsPlugin")
00129
00130 constraint_aware_xml_file = open(directory_name+"/launch/constraint_aware_kinematics.launch", "w")
00131 constraint_aware_xml.write(constraint_aware_xml_file, xml_declaration=True, pretty_print=True)