00001
00002
00003
00004
00005
00006 import lxml.etree
00007 import lxml.builder
00008 import os
00009 import rospkg
00010 import subprocess
00011 import sys
00012
00013 class InvalidURDFException(Exception):
00014 pass
00015
00016 def printf(*args):
00017 print "[urdf_to_xacro.py] " + ' '.join(args)
00018
00019 class GazeboModelPathResolver(object):
00020 def __init__(self):
00021 self.rospack = rospkg.RosPack()
00022 self.paths = []
00023 self.rospkgs = {}
00024 self.cached_path = {}
00025 self.load_path_from_env()
00026 self.load_path_from_plugin()
00027
00028 def add_path(self, path):
00029 self.paths.insert(0, path)
00030
00031 def add_ros_pkg(self, pkg, path):
00032 if pkg in self.rospkgs:
00033 self.rospkgs[pkg].insert(0, path)
00034 else:
00035 self.rospkgs[pkg] = [path]
00036
00037 def load_path_from_env(self):
00038 env_str = os.environ.get("GAZEBO_MODEL_PATH", None)
00039 if env_str is None:
00040 return
00041 for path in env_str.split(':'):
00042 if os.path.isdir(path):
00043 self.add_path(path)
00044
00045 def load_path_from_plugin(self):
00046 cmd = "rospack plugins --attrib=gazebo_model_path gazebo_ros"
00047 lines = subprocess.check_output(cmd, shell=True)
00048 if lines:
00049 for line in lines.split(os.linesep):
00050 if not line:
00051 continue
00052 pkg, path = line.split()
00053 if os.path.isdir(path):
00054 self.add_path(path)
00055 if pkg not in self.rospkgs:
00056 self.rospkgs[pkg] = self.rospack.get_path(pkg)
00057
00058 def parse_model_path(self, path):
00059 key = None
00060 if path.startswith("model://"):
00061 spath = path[len("model://"):].split('/')
00062 key = spath[0]
00063 path = '/'.join(spath[1:])
00064 return key, path
00065
00066 def search_path(self, key):
00067 for base_path in self.paths:
00068 path = os.path.join(base_path, key)
00069 if os.path.isdir(path):
00070 self.cached_path[key] = path
00071
00072 def pack_ros_path(self, path):
00073 for pkg, pkg_path in self.rospkgs.items():
00074 if path.startswith(pkg_path):
00075 return "package://" + pkg + path[len(pkg_path):]
00076 return path
00077
00078 def resolve_path(self, path):
00079 key, path = self.parse_model_path(path)
00080 if not key:
00081 return path
00082 if key not in self.cached_path:
00083 self.search_path(key)
00084 if key in self.cached_path:
00085 path = os.path.join(self.cached_path[key], path)
00086 else:
00087 raise Exception("path %s is not found" % path)
00088 return self.pack_ros_path(path)
00089
00090 _gazebo = GazeboModelPathResolver()
00091
00092 def resolve_gazebo_model_path(path):
00093 return _gazebo.resolve_path(path)
00094
00095 class URDF2XACRO(object):
00096 def __init__(self, urdf_path, xacro_path, force_rename=None):
00097 printf("loading", urdf_path)
00098 self.root = lxml.etree.parse(urdf_path,
00099 parser=lxml.etree.XMLParser(remove_blank_text=True))
00100 self.xacro_path = xacro_path
00101 self.force_rename = None
00102
00103 def find_root_link(self):
00104 links = self.root.xpath("//robot/link")
00105 if len(links) == 0:
00106 raise InvalidURDFException("No link found in model")
00107 elif len(links) == 1:
00108 return links[0]
00109 else:
00110 link_names = [l.get("name") for l in links]
00111 joints = self.root.xpath("//robot/joint")
00112 for j in joints:
00113 child_link_name = j.find("child").get("link")
00114 if child_link_name in link_names:
00115 link_names.remove(child_link_name)
00116 if len(link_names) != 1:
00117 raise InvalidURDFException("Links are not connected with joints: %s" % link_names)
00118 return self.root.xpath("//robot/link[@name='%s']" % link_names[0])[0]
00119
00120 def add_namespace(self, ns):
00121 ns_str = "${%s}_" % ns
00122
00123 links = self.root.xpath("//robot/link")
00124 for l in links:
00125 l.set("name", ns_str + l.get("name"))
00126
00127 gazebo = self.root.xpath("//robot/gazebo")
00128 for g in gazebo:
00129 if g.get("reference") is not None:
00130 g.set("reference", ns_str + g.get("reference"))
00131
00132 joints = self.root.xpath("//robot/joint")
00133 for j in joints:
00134 j.set("name", ns_str + j.get("name"))
00135 j_parent = j.find("parent")
00136 j_parent.set("link", ns_str + j_parent.get("link"))
00137 j_child = j.find("child")
00138 j_child.set("link", ns_str + j_child.get("link"))
00139
00140 transmissions = self.root.xpath("//robot/transmission")
00141 for t in transmissions:
00142 t.set("name", ns_str + t.get("name"))
00143 t_joint = t.find("joint")
00144 t_joint.set("name", ns_str + t_joint.get("name"))
00145 t_actuator = t.find("actuator")
00146 t_actuator.set("name", ns_str + t_actuator.get("name"))
00147
00148 def replace_model_path(self):
00149 try:
00150 for param in ["visual", "collision", "inertial"]:
00151 for m in self.root.xpath("//robot/link/%s/geometry/mesh" % param):
00152 if "filename" in m.attrib:
00153 modelpath = m.attrib["filename"]
00154 if self.force_rename and modelpath.startswith("model://"):
00155 pkgpath = self.force_rename + modelpath[len("model://"):]
00156 else:
00157 pkgpath = resolve_gazebo_model_path(modelpath)
00158 printf(modelpath, "->", pkgpath)
00159 m.attrib["filename"] = pkgpath
00160 except:
00161 pass
00162
00163 def inject_macro(self):
00164 self.add_namespace("name")
00165 root_link = self.find_root_link()
00166 robot = self.root.getroot()
00167 robot_name = robot.get("name")
00168 first_joint = robot.find("joint")
00169 virtual_joint = lxml.etree.Element("joint",
00170 attrib={"name": "${name}_root_parent_joint", "type": "fixed"})
00171 virtual_joint.append(lxml.etree.Element("parent", attrib={"link": "${parent}"}))
00172 virtual_joint.append(lxml.etree.Element("child", attrib={"link": root_link.get("name")}))
00173 virtual_joint.append(lxml.etree.Element("insert_block", attrib={"name": "origin"}))
00174 if first_joint is not None:
00175 first_joint.addprevious(virtual_joint)
00176 else:
00177 robot.append(virtual_joint)
00178 macro = lxml.etree.Element("macro",
00179 attrib={"name": robot_name, "params": "name parent *origin"})
00180 for e in robot.getchildren():
00181 macro.append(e)
00182 for e in robot.getchildren():
00183 robot.remove(e)
00184 robot.append(macro)
00185
00186 def save(self):
00187 out_path = os.path.abspath(self.xacro_path)
00188 if not os.path.exists(os.path.dirname(out_path)):
00189 os.makedirs(os.path.dirname(out_path))
00190 xmlstring = lxml.etree.tostring(self.root,
00191 encoding="utf-8",
00192 xml_declaration=True,
00193 pretty_print=True,
00194 with_comments=True)
00195 with open(out_path, "w") as f:
00196 f.write(xmlstring)
00197 printf("saved to", out_path)
00198 def convert(self):
00199 self.replace_model_path()
00200 self.inject_macro()
00201 self.save()
00202
00203 if __name__ == '__main__':
00204 import argparse
00205 p = argparse.ArgumentParser(description="xacrify urdf file")
00206 p.add_argument("urdf", type=str, help="path to input urdf file")
00207 p.add_argument("xacro", type=str, help="path to output xacro file")
00208 p.add_argument("-f", "--force-rename", type=str,
00209 default=None, help="force replace model:// tag with specified name")
00210 args = p.parse_args()
00211
00212 c = URDF2XACRO(args.urdf, args.xacro, args.force_rename)
00213 c.convert()