├── README.md ├── GazeboMaterial ├── __init__.py ├── GazeboMaterialItem.py └── GazeboMaterialFile.py ├── examples └── example001.sdf └── sdf2urdf.py /README.md: -------------------------------------------------------------------------------- 1 | # sdf2urdf 2 | 3 | Converter gazebo's sdf to urdf. 4 | 5 | ## Dependencies 6 | ```(sh) 7 | sudo pip3 install xacro python-dotenv pyparsing 8 | ``` 9 | 10 | ## Usage 11 | 12 | ```(sh) 13 | export PYTHON_PATH=$(pwd)/GazeboMaterial 14 | ./sdf2urdf [ ] 15 | ``` 16 | 17 | ### Examples 18 | 19 | See `./examples/` 20 | ```(sh) 21 | ./sdf2urdf.py ./examples/example001.sdf 22 | ``` 23 | -------------------------------------------------------------------------------- /GazeboMaterial/__init__.py: -------------------------------------------------------------------------------- 1 | from GazeboMaterial.GazeboMaterialFile import * 2 | from GazeboMaterial.GazeboMaterialItem import * 3 | 4 | 5 | # Example content 6 | def _getTestContent(): 7 | return """ 8 | import * from "grid.material" 9 | 10 | vertex_program Gazebo/DepthMapVS glsl 11 | { 12 | source depth_map.vert 13 | 14 | default_params 15 | { 16 | param_named_auto texelOffsets texel_offsets 17 | } 18 | } 19 | fragment_program Gazebo/DepthMapFS glsl 20 | { 21 | source depth_map.frag 22 | 23 | default_params 24 | { 25 | param_named_auto pNear near_clip_distance 26 | param_named_auto pFar far_clip_distance 27 | } 28 | } 29 | 30 | material Gazebo/DepthMap 31 | { 32 | technique 33 | { 34 | pass 35 | { 36 | vertex_program_ref Gazebo/DepthMapVS { } 37 | fragment_program_ref Gazebo/DepthMapFS { } 38 | } 39 | } 40 | } 41 | 42 | vertex_program Gazebo/XYZPointsVS glsl 43 | { 44 | source depth_points_map.vert 45 | } 46 | 47 | fragment_program Gazebo/XYZPointsFS glsl 48 | { 49 | source depth_points_map.frag 50 | 51 | default_params 52 | { 53 | param_named_auto width viewport_width 54 | param_named_auto height viewport_height 55 | } 56 | } 57 | 58 | material Gazebo/XYZPoints 59 | { 60 | technique 61 | { 62 | pass pcd_tex 63 | { 64 | separate_scene_blend one zero one zero 65 | 66 | vertex_program_ref Gazebo/XYZPointsVS { } 67 | fragment_program_ref Gazebo/XYZPointsFS { } 68 | } 69 | } 70 | } 71 | 72 | material Gazebo/Grey 73 | { 74 | technique 75 | { 76 | pass main 77 | { 78 | ambient .3 .3 .3 1.0 79 | diffuse .7 .7 .7 1.0 80 | specular 0.01 0.01 0.01 1.000000 1.500000 81 | } 82 | } 83 | } 84 | material Gazebo/Gray : Gazebo/Grey 85 | { 86 | } 87 | 88 | """ 89 | 90 | 91 | def load_gazebo_setup(): 92 | from dotenv import load_dotenv 93 | from pathlib import Path 94 | from os.path import join as joinp, dirname, abspath 95 | 96 | dotenv_path = Path('/usr/share/gazebo/setup.sh') 97 | load_dotenv(dotenv_path) 98 | 99 | def gazebo_material_test(): 100 | gazebo_resource_path = os.getenv('GAZEBO_RESOURCE_PATH').split(':') 101 | 102 | for gzpath in gazebo_resource_path: 103 | gazebo_material_path = joinp(str(gazebo_resource_path), "media", "materials", "scripts", "gazebo.material") 104 | if not os.path.exists(gazebo_material_path): 105 | continue 106 | gzMaterial = GazeboMaterialFile(gazebo_material_path); 107 | gzMaterial._parse(_getTestContent()) 108 | GAZEBO_GREY='Gazebo/Grey' 109 | GAZEBO_GRAY='Gazebo/Gray' 110 | 111 | print("Test: ", 112 | gzMaterial.find('material[name={0}].technique.pass.ambient'.format(GAZEBO_GREY)), 113 | ) 114 | grey = gzMaterial.getColor(GAZEBO_GREY) 115 | gray = gzMaterial.getColor(GAZEBO_GRAY) 116 | assert grey 117 | assert gray 118 | c1, c2 = gray[0], grey[0] 119 | print(str(" ".join(c1.args()))) 120 | 121 | def gazebo_tree_test(): 122 | pass 123 | 124 | def parse_args(): 125 | import argparse 126 | 127 | parser = argparse.ArgumentParser() 128 | parser.add_argument('--test-inheritance', action='store_true') 129 | parser.add_argument('--test-dumptree', action='store_true') 130 | return parser.parse_args() 131 | 132 | if __name__ == "__main__": 133 | args = parse_args() 134 | if args.test_inheritance: 135 | gazebo_material_test() 136 | elif args.test_dumptree: 137 | pass 138 | 139 | __all__ = ['GazeboMaterialItem', 'GazeboMaterialFile', 'load_gazebo_setup'] 140 | 141 | # vim: ts=4 sw=4 noet 142 | -------------------------------------------------------------------------------- /GazeboMaterial/GazeboMaterialItem.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | class GazeboMaterialItem(object): 4 | def __init__(self, typeName): 5 | self._type = typeName 6 | self._name = None 7 | self._level = -1 8 | self._parent = None 9 | self._arguments = [] 10 | self._children = [] 11 | self.__has_inheritance = False 12 | self._inherits_link = [] 13 | self._inheritance = [] 14 | 15 | @property 16 | def level(self): 17 | return self._level 18 | 19 | @property 20 | def type(self): 21 | return self._type 22 | 23 | @property 24 | def parent(self): 25 | return self._parent 26 | 27 | def arg(self, idx): 28 | if idx < len(self._arguments): 29 | return self._arguments[idx] 30 | 31 | @property 32 | def args(self): 33 | return self._arguments 34 | 35 | @property 36 | def name(self): 37 | return self._name 38 | 39 | def _setName(self, name): 40 | self._name = name 41 | 42 | def __fixChildLevels(self, child): 43 | if child._parent != self or child._level == self._level+1: 44 | return 45 | child._level = self._level+1 46 | for c2 in child._children: 47 | child.__fixChildLevels(c2) 48 | 49 | def _addChild(self, item): 50 | self._children.append(item) 51 | self.__fixChildLevels(item) 52 | 53 | def addChild(self, item): 54 | #if item not in self._children: 55 | item._parent = self 56 | self._addChild(item) 57 | return item 58 | 59 | def addArgument(self, arg): 60 | self._arguments.append(arg) 61 | 62 | def addInheritance(self, classname): 63 | self.__has_inheritance = True 64 | self._inheritance.append(classname) 65 | 66 | def findAll(self, typeName): 67 | opts = {} 68 | if '[' in typeName: 69 | typeName, vargs = typeName[:typeName.index('[')], typeName[typeName.index('[')+1:typeName.rindex(']')] 70 | buf = {} 71 | for o in vargs.split(','): # multiple opts 72 | varg = o.split('=',1) 73 | if len(varg)==1: 74 | opts[varg[0]] = True 75 | elif len(varg)==2: 76 | opts[varg[0]] = varg[1] 77 | def checkOpts(child): 78 | for k, v in opts.items(): 79 | if not hasattr(child, k): 80 | return False 81 | attr = getattr(child, k) 82 | if v is True: 83 | if attr is None or (callable(attr) and attr() is None): 84 | return False 85 | else: 86 | if attr is None or (not callable(attr) and attr != v) or (callable(attr) and attr() != v): 87 | return False 88 | return True 89 | out = [] 90 | for child in self._children[:]: 91 | if child.type == typeName and checkOpts(child): 92 | out.append(child) 93 | return out 94 | 95 | def __repr__(self): 96 | return 'GazeboMaterialItem<' + str(self) + '>' 97 | 98 | def __str__(self): 99 | srep = "" 100 | if self._level>=0: 101 | prefix = " "*(self._level) 102 | srep = "{0}{1}".format(prefix, self._type) 103 | if self._arguments: 104 | srep += " ".join(['']+self._arguments) 105 | if self._inheritance: 106 | if type(self._inheritance[0]) == str: 107 | srep += self._inheritance[0] 108 | else: 109 | srep += self._inheritance[0].name() 110 | if self._children: 111 | if self._level >= 0: 112 | srep += '\n%s{\n'%prefix 113 | for child in self._children: 114 | srep += child.__str__() 115 | if self._level >= 0: 116 | srep += '%s}'%prefix 117 | else: 118 | srep += '\n' 119 | else: 120 | if self._level == 0 and self.type() != 'import': 121 | srep += ' { }' 122 | return srep+'\n' 123 | 124 | def dumptree(self, filename=None, fwrite=None): 125 | if fwrite is None: 126 | fwrite = sys.stdout 127 | if filename is not None: 128 | fwrite = open(fp, "w") 129 | 130 | 131 | lvl = self._level+1 132 | namedef = '@root' if lvl == 0 else ("<"+self._type+"[{0}]>".format(len(self._arguments))) 133 | name = self._name if self._name is not None else namedef 134 | 135 | fwrite.write("{2}{0}: {1} +({3})\n".format(lvl, name, " "*(lvl), len(self._children))) 136 | for child in self._children: 137 | child.dumptree(fwrite=fwrite) 138 | 139 | # vim: ts=4 sw=4 noet 140 | -------------------------------------------------------------------------------- /examples/example001.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 7 | 8 | 0.166667 9 | 0 10 | 0 11 | 0.166667 12 | 0 13 | 0.166667 14 | 15 | 0 0 0 0 -0 0 16 | 17 | 0 18 | 0 19 | 0 20 | 0.161003 -0.167578 -0 0 -0 0 21 | 22 | 23 | 24 | 1 1 1 25 | 26 | 27 | 28 | 32 | 33 | 34 | 0 0 0 0 -0 0 35 | 0 36 | 1 37 | 38 | 39 | 0 40 | 10 41 | 0 0 0 0 -0 0 42 | 43 | 44 | 1 1 1 45 | 46 | 47 | 48 | 49 | 50 | 1 51 | 1 52 | 0 0 0 53 | 0 54 | 0 55 | 56 | 57 | 1 58 | 0 59 | 0 60 | 1 61 | 62 | 0 63 | 64 | 65 | 66 | 67 | 0 68 | 1e+06 69 | 70 | 71 | 0 72 | 1 73 | 1 74 | 75 | 0 76 | 0.2 77 | 1e+13 78 | 1 79 | 0.01 80 | 0 81 | 82 | 83 | 1 84 | -0.01 85 | 0 86 | 0.2 87 | 1e+13 88 | 1 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 1 97 | 98 | 0.1 99 | 0 100 | 0 101 | 0.1 102 | 0 103 | 0.1 104 | 105 | 106 | 0.161644 -0.057787 0.811911 0 -0 0 107 | 108 | 0 0 0 0 -0 0 109 | 110 | 111 | 0.5 112 | 113 | 114 | 115 | 1 116 | 120 | 121 | 122 | 0 123 | 1 124 | 125 | 126 | 0 127 | 10 128 | 0 0 0 0 -0 0 129 | 130 | 131 | 0.5 132 | 133 | 134 | 135 | 136 | 137 | 1 138 | 1 139 | 0 0 0 140 | 0 141 | 0 142 | 143 | 144 | 1 145 | 0 146 | 0 147 | 1 148 | 149 | 0 150 | 151 | 152 | 153 | 154 | 0 155 | 1e+06 156 | 157 | 158 | 0 159 | 1 160 | 1 161 | 162 | 0 163 | 0.2 164 | 1e+13 165 | 1 166 | 0.01 167 | 0 168 | 169 | 170 | 1 171 | -0.01 172 | 0 173 | 0.2 174 | 1e+13 175 | 1 176 | 177 | 178 | 179 | 180 | 181 | 0 182 | 1 183 | 184 | 185 | -------------------------------------------------------------------------------- /GazeboMaterial/GazeboMaterialFile.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | # gazebo's material syntax parsing 4 | import pyparsing as gz 5 | 6 | from GazeboMaterial.GazeboMaterialItem import * 7 | 8 | def _build_tokenizer(): 9 | gz.ParserElement.setDefaultWhitespaceChars(' \t') 10 | singleline_comment = "//" + gz.restOfLine 11 | multiline_comment = gz.cStyleComment 12 | comments = gz.MatchFirst([singleline_comment, multiline_comment]) 13 | 14 | real = gz.Combine(gz.Optional(gz.oneOf("+ -")) + gz.Optional(gz.Word(gz.nums)) +"." + gz.Word(gz.nums)).setName("real") 15 | integer = gz.Combine(gz.Optional(gz.oneOf("+ -")) + gz.Word(gz.nums)).setName("integer") 16 | nums = real | integer 17 | 18 | words = gz.Word(gz.alphas) 19 | string = gz.dblQuotedString() 20 | item_type = gz.Word(gz.alphas+"_") 21 | 22 | extensions = (gz.oneOf(["frag", "glsl", "jpeg", "jpg", "png", "vert"])) 23 | _filename = gz.ZeroOrMore(gz.Word(gz.alphanums+"_.") + '.') + gz.Word(gz.alphanums+"_") 24 | _filepath = gz.ZeroOrMore(gz.Word(gz.alphanums+"_.") + "/") 25 | filename = gz.Combine(_filename + '.' + extensions) 26 | filepath = gz.Combine(gz.Optional("/")+_filepath) 27 | fileany = gz.Combine(filepath + filename) 28 | 29 | importall = gz.Literal("*") 30 | importheader = gz.Literal("import").setResultsName('itemtype') + \ 31 | (importall | gz.Combine(gz.delimitedList(item_type, delim=",", combine=True))).setResultsName('imports') + \ 32 | gz.Literal("from").suppress() + (string | words).setResultsName('from') 33 | 34 | lineend = gz.OneOrMore(gz.LineEnd()).suppress() 35 | oplineend = gz.Optional(lineend) 36 | 37 | blockstart, blockend = gz.Literal("{").suppress(), gz.Literal("}").suppress() 38 | 39 | blockname = gz.Combine(gz.Optional(gz.Word(gz.alphas+"_") + gz.Literal("/")) + gz.Word(gz.alphanums+"_")) 40 | blockoption = item_type.setResultsName('itemtype') + (gz.OneOrMore(fileany | nums | item_type)).setResultsName('arguments') + lineend 41 | blockinherit = gz.Literal(":").suppress() + blockname 42 | blockheader = item_type.setResultsName('itemtype') + gz.Optional(blockname).setResultsName('blockname') + \ 43 | gz.Group(gz.Optional(gz.OneOrMore(item_type))).setResultsName('arguments') + \ 44 | gz.Group(gz.Optional(blockinherit)).setResultsName('inheritance') 45 | 46 | blockinner = gz.Forward() 47 | blockinner << gz.Group(item_type.setResultsName('itemtype') + gz.Optional(blockname).setResultsName('blockname') + gz.ZeroOrMore(blockname).setResultsName('arguments') + oplineend + \ 48 | blockstart + oplineend + \ 49 | gz.ZeroOrMore(blockinner ^ gz.Group(blockoption)).setResultsName('blockbody') + \ 50 | oplineend + blockend) + oplineend 51 | 52 | block = gz.Group(blockheader + oplineend + \ 53 | blockstart + oplineend + \ 54 | gz.ZeroOrMore(blockinner ^ gz.Group(blockoption)).setResultsName('blockbody') + \ 55 | oplineend + blockend) + oplineend 56 | 57 | allitems = gz.ZeroOrMore(gz.Group(importheader) + lineend) + gz.ZeroOrMore(block) + oplineend 58 | allitems.ignore(comments) 59 | 60 | return allitems 61 | 62 | def _parse_query(query): 63 | # FIXME: parse query with pyparsing aka local `gz` 64 | level = 0 65 | items = [] 66 | idx, idy = 0, 0 67 | has_string = False 68 | for c in query: 69 | if level == 0 and c == '.': 70 | items.append(query[idx:idy]) 71 | idx, idy = idy+1, idy 72 | elif c == '[' or (not has_string and c == '"'): 73 | if (not has_string and c == '"'): 74 | has_string = True 75 | level+=1 76 | elif c == ']' or (has_string and c == '"'): 77 | if (has_string and c == '"'): 78 | has_string = False 79 | level-=1 80 | idy+=1 81 | items.append(query[idx:]) 82 | return items 83 | 84 | 85 | class GazeboMaterialFile: 86 | def __init__(self, filename): 87 | self._filename = filename 88 | self._imports = [] # TODO 89 | self._parsed = False 90 | self._root = GazeboMaterialItem(None) 91 | self._tokenizer = _build_tokenizer() 92 | 93 | def getFilename(self): 94 | return self._filename 95 | 96 | def _parse(self, content): 97 | def makeBlock(token, level=0): 98 | tkeys = list(token.keys()) 99 | if 'itemtype' in tkeys: 100 | item = GazeboMaterialItem(token['itemtype']) 101 | else: 102 | raise Exception(f"Cannot found itemtype in {oken}") 103 | 104 | if 'blockname' in tkeys: 105 | item._setName(token['blockname']) 106 | #item.addArgument(token['blockname']) 107 | if 'arguments' in tkeys: 108 | for xarg in token['arguments']: 109 | item.addArgument(xarg) 110 | if 'inheritance' in tkeys: 111 | for xarg in token['inheritance']: 112 | item.addInheritance(xarg) 113 | 114 | if 'blockbody' in tkeys: 115 | for child in token['blockbody']: 116 | if type(child) != str: 117 | item.addChild(makeBlock(child, level=level+1)) 118 | else: 119 | raise Exception("Failured while parsing blockbody", child) 120 | 121 | return item 122 | 123 | for tokens,start,end in self._tokenizer.scanString(content): 124 | for t in tokens: 125 | self._root.addChild(makeBlock(t)) 126 | self._parsed = True 127 | 128 | def _do_inherit(self): 129 | if not self._parsed: 130 | raise Exception("Inheritance processing should be run after parser complete") 131 | 132 | def makechildmap(node): 133 | childmap = {} 134 | for child in node._children: 135 | if child.name is None: 136 | continue 137 | if child.type not in childmap: 138 | childmap[child.type] = {} 139 | childmap[child.type][child.name] = child 140 | return childmap 141 | childmap = makechildmap(self._root) 142 | 143 | def makeblocks(node): 144 | blocks = {} 145 | for child in node._children: 146 | bid = child.type 147 | if child.name is not None: 148 | bid += ":"+child.name 149 | if bid in blocks: 150 | raise Exception(f"Item is exists {bid}") 151 | blocks[bid] = child 152 | return blocks 153 | 154 | def inherit(nodeto, nodefrom, copy=False): 155 | # TODO: make copy of inherited properties 156 | bt, bf = makeblocks(nodeto), makeblocks(nodefrom) 157 | for bid, fchild in bf.items(): 158 | if bid not in bt: # Item full inheritance 159 | nodeto._addChild(fchild) 160 | else: # Partial interitance 161 | fchildren = nodefrom.findAll(f'{fchild.type}') 162 | tchildren = nodeto.findAll(f'{fchild.type}') 163 | tchk = {} 164 | for tf in tchildren: 165 | tchk[f"{tf.type}:{tf.name}"] = tf 166 | for fc in fchildren: 167 | uid = f"{fc.type}:{fc.name}" 168 | if uid in tchk: 169 | inherit(tchk[uid], fc) 170 | continue 171 | else: 172 | nodeto._addChild(fchild) 173 | 174 | for typeName, cm in childmap.items(): 175 | for name, child in cm.items(): 176 | inheritance = [] 177 | if not child._inheritance: 178 | continue 179 | child._inheritance[:] 180 | for inh in child._inheritance[:]: 181 | if cm.get(inh) is not None: 182 | childinh = cm.get(inh) 183 | inheritance.append(cm.get(inh)) 184 | else: 185 | raise Exception("{name} type {typeName} inherits from undeclared {inh}") 186 | child._inheritance = inheritance 187 | for inh in inheritance: 188 | if child not in inh._inherits_link: 189 | inh._inherits_link.append(child) 190 | inherit(child, inh) 191 | 192 | def parse(self, filename=None): 193 | if filename is None: 194 | return self.parse(self._filename) 195 | else: 196 | if filename == self._filename and self._parsed: 197 | return 198 | self._filename = filename 199 | with open(filename, "r") as f: 200 | content = f.read() 201 | self._parse(content) 202 | self._do_inherit() 203 | 204 | def find(self, query): 205 | self.parse() 206 | path = _parse_query(query) 207 | items = self._root.findAll(path.pop(0)) 208 | while len(path): 209 | buf, p = [], path.pop(0) 210 | for it in items: 211 | buf += it.findAll(p) 212 | items = buf 213 | return items 214 | 215 | def getColor(self, name): 216 | return self.find('material[name={0}].technique.pass.ambient'.format(name)) 217 | 218 | # vim: ts=4 sw=4 noet 219 | -------------------------------------------------------------------------------- /sdf2urdf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | # GazeboMaterial 7 | sys.path.append(os.path.dirname(os.path.realpath(__file__))) 8 | from GazeboMaterial import * 9 | 10 | # xml operations 11 | from xacro import parse 12 | from xacro.xmlutils import * 13 | 14 | 15 | rospack = None 16 | 17 | class Item: 18 | """ 19 | ['ATTRIBUTE_NODE', 'CDATA_SECTION_NODE', 'COMMENT_NODE', 'DOCUMENT_FRAGMENT_NODE', 'DOCUMENT_NODE', 'DOCUMENT_TYPE_NODE', 'ELEMENT_NODE', 'ENTITY_NODE', 'ENTITY_REFERENCE_NODE', 'NOTATION_NODE', 'PROCESSING_INSTRUCTION_NODE', 'TEXT_NODE', '__doc__', '__init__', '__module__', '__nonzero__', '__repr__', '_attrs', '_attrsNS', '_call_user_data_handler', '_child_node_types', '_get_attributes', '_get_childNodes', '_get_firstChild', '_get_lastChild', '_get_localName', '_get_nodeName', '_magic_id_nodes', 'appendChild', 'attributes', 'childNodes', 'cloneNode', 'firstChild', 'getAttribute', 'getAttributeNS', 'getAttributeNode', 'getAttributeNodeNS', 'getElementsByTagName', 'getElementsByTagNameNS', 'getInterface', 'getUserData', 'hasAttribute', 'hasAttributeNS', 'hasAttributes', 'hasChildNodes', 'insertBefore', 'isSameNode', 'isSupported', 'lastChild', 'localName', 'namespaceURI', 'nextSibling', 'nodeName', 'nodeType', 'nodeValue', 'normalize', 'ownerDocument', 'parentNode', 'prefix', 'previousSibling', 'removeAttribute', 'removeAttributeNS', 'removeAttributeNode', 'removeAttributeNodeNS', 'removeChild', 'replaceChild', 'schemaType', 'setAttribute', 'setAttributeNS', 'setAttributeNode', 'setAttributeNodeNS', 'setIdAttribute', 'setIdAttributeNS', 'setIdAttributeNode', 'setUserData', 'nodeName', 'toprettyxml', 'toxml', 'unlink', 'writexml'] 20 | """ 21 | def __init__(self, xmlnode, parent=None, document=None, level=0, root=None): 22 | self._document = xmlnode if xmlnode.nodeName == '#document' else document 23 | self._root = root if root is not None else self 24 | self._node = xmlnode 25 | self._parent = parent 26 | self._level = level 27 | self._children = [] 28 | self._textvalue = None 29 | 30 | if (len(self._node.childNodes) == 1) and (self._node.childNodes[0].nodeName == '#text'): 31 | self._textvalue = Item(self._node.childNodes[0], document=self._document, parent=self, level=level+1, root=self.getRootNode()) 32 | return 33 | for n in self._node.childNodes[:]: 34 | if level == 0: 35 | self._root = self 36 | # Skip pretty tabs 37 | if n.nodeName == '#text' and n.toxml().strip() == '': 38 | self._node.removeChild(n) 39 | continue 40 | self.appendChild(Item(n, parent=self, document=self._document, level=level+1, root=self.getRootNode())) 41 | 42 | def __repr__(self): 43 | return f'Item'; 44 | 45 | def appendChild(self, item): 46 | if item.__class__.__name__ == 'Item': 47 | if item._node.parentNode is not None and item._node.parentNode != self._node: 48 | self._node.appendChild(item._node) 49 | if item in item._parent._children: 50 | del item._parent._children[item._parent._children.index(item)] 51 | if item not in self._children: 52 | self._children.append(item) 53 | else: 54 | item = Item(item, parent=self, document=self._document, level=self._level+1, root=self.getRootNode()) 55 | self._children.append(item) 56 | 57 | def setAttribute(self, name, value): 58 | self._node.setAttribute(name, value) 59 | 60 | def getAttribute(self, name): 61 | return self._node.getAttribute(name) 62 | 63 | def removeAttribute(self, name): 64 | self._node.removeAttribute(name) 65 | 66 | def text(self): 67 | return self._node.childNodes[0].toxml() 68 | 69 | @property 70 | def nodeName(self): 71 | return self._node.nodeName 72 | 73 | def removeChild(self, item): 74 | pn = item._node.parentNode 75 | node = item._node 76 | while pn is not None and pn != self._node.parentNode: 77 | pn.removeChild(node) 78 | node, pn = pn, pn.parentNode 79 | 80 | #self._node.removeChild(item._node) 81 | if item in self._children: 82 | del self._children[self._children.index(item)] 83 | 84 | def replaceChild(self, replace, which): 85 | if which in self._children: 86 | self._children[self._children.index(which)] = replace 87 | self._node.replaceChild(replace._node, which._node) 88 | 89 | def cloneNode(self): 90 | return Item(self._node.cloneNode(True), parent=None, document=self._document, level=self._level, root=self.getRootNode()) 91 | 92 | def createElement(self, *args, **kwargs): 93 | try: 94 | return self._document.createElement(*args, **kwargs) 95 | except Exception as e: 96 | sys.stderr.write("{0}\n".format(self._node)) 97 | raise e 98 | 99 | def isPlugin(self): 100 | return self 101 | 102 | def isSDF(self): 103 | return self.nodeName == 'sdf' and self._level == 1 104 | 105 | def uriToPath(self, uri): 106 | def try_encode(uri, wrap): 107 | try: 108 | return wrap(uri.encode("utf-8")) 109 | except: 110 | return wrap(uri) 111 | 112 | parts = try_encode(uri, lambda x: x.split('/')) 113 | uritype, _, parts = parts[0], None, parts[2:] 114 | 115 | searchpath = [] 116 | typemap = { 117 | "model:": ["GAZEBO_MODEL_PATH"], 118 | "file:": ["GAZEBO_RESOURCE_PATH"], 119 | }.get(uritype) 120 | 121 | if typemap is None: 122 | return None 123 | 124 | for envvar in typemap: 125 | value = os.getenv(envvar) 126 | if value is not None: 127 | searchpath += try_encode(value, lambda x: x.strip(":").split(':')) 128 | 129 | for gmp in searchpath: 130 | path = os.path.join(gmp, *parts) 131 | if os.path.exists(path): 132 | return path 133 | return None 134 | 135 | _gazeboMaterialFiles = {} 136 | def _getGazeboMaterial(self, node): 137 | name, gzMaterial = None, None 138 | 139 | for opt in node._children: 140 | if opt.nodeName == 'name': 141 | name = opt.text() 142 | if not name.startswith('Gazebo/'): 143 | raise Exception("Unsupported color NS: {0}".format(name)) 144 | elif opt.nodeName == 'uri': 145 | materialpath = self.uriToPath(opt.text()) 146 | if materialpath is None: 147 | raise Exception(f"Cannot find material reference in GAZEBO_MODELS_PATH and GAZEBO_RESOURCE_PATH: {opt.text()}") 148 | if self.__class__._gazeboMaterialFiles.get(materialpath) is None: 149 | self.__class__._gazeboMaterialFiles[materialpath] = GazeboMaterialFile(materialpath) 150 | gzMaterial = self.__class__._gazeboMaterialFiles[materialpath] 151 | else: 152 | raise Exception("Unknown material/script tag: {0}".format(opt.nodeName)) 153 | return name, gzMaterial 154 | 155 | def getRootNode(self): 156 | if self._root is None: 157 | return self 158 | return self._root 159 | 160 | _materialNodes = {} 161 | def _getMaterialNode(self, name): 162 | exists = name in self.__class__._materialNodes 163 | if not exists: 164 | root = self.getRootNode() 165 | item = Item(self.createElement('material'), parent=root, document=self._document, level=root._level+1, root=self.getRootNode()) 166 | item.setAttribute('name', name) 167 | root.appendChild( item ) 168 | self.__class__._materialNodes[name] = item 169 | return self.__class__._materialNodes[name], exists 170 | 171 | def convert(self): 172 | if self._level == 0: 173 | for c in self._children[:]: 174 | # Main node replacement 175 | if c.isSDF(): 176 | """ 177 | 178 | 179 | <...> 180 | <...> 181 | 182 | 183 | --- 184 | 185 | <...> 186 | <...> 187 | 188 | """ 189 | node = Item(self.createElement('robot'), parent=self, document=self._document, level=self._level+1) # Root is None because it's a new root node 190 | copy = c.cloneNode() 191 | for ch in self._children: 192 | self.removeChild(ch) 193 | for ch in self._node.childNodes: 194 | self._node.removeChild(ch) 195 | 196 | for name, attr in copy._node.childNodes[0].attributes.items(): 197 | node._node.setAttribute(name, attr) 198 | 199 | for ch in copy._node.childNodes[0].childNodes[:]: 200 | node._node.appendChild(ch) 201 | item = Item(ch, parent=self, document=self._document, level=self._level+1, root=node.getRootNode()) 202 | node.appendChild(item) 203 | #self.replaceChild(node, copy) 204 | self.appendChild(node) 205 | break 206 | else: 207 | raise Exception("Unsupported file type: {0}".format(c.nodeName)) 208 | 209 | _properties = {} 210 | for c in self._children[:]: 211 | if c.nodeName not in _properties: 212 | _properties[c.nodeName] = c 213 | elif type(_properties[c.nodeName]) == list: 214 | _properties[c.nodeName].append(c) 215 | else: 216 | _properties[c.nodeName] = [_properties[c.nodeName], c] 217 | 218 | for c in self._children[:]: 219 | 220 | # Unused elements 221 | if c.nodeName in ['plugin', 'physics', 'gravity', 'velocity_decay', 'self_collide', 'surface', 'static', 'dissipation', 'stiffness', 'sensor', '#comment']: 222 | self.removeChild(c) 223 | 224 | # Inner tags into attributes, nothing else 225 | elif c.nodeName in ['inertia', 'cylinder', 'sphere', 'box', 'limit', 'dynamics'] : 226 | 227 | if c.nodeName == 'limit': 228 | c.convert() 229 | for mc in c._children[:]: 230 | c.setAttribute(mc.nodeName, mc.text().strip()) 231 | c.removeChild(mc) 232 | 233 | elif c.nodeName in ['visual', 'collision']: 234 | if c.nodeName == 'collision' and c.nodeName in _properties: 235 | self.removeChild(c) 236 | else: 237 | for name,attr in c._node.attributes.items(): 238 | c.removeAttribute(name) 239 | 240 | elif c.nodeName == 'material': 241 | """ 242 | 243 | 247 | 248 | --- 249 | 250 | 251 | 252 | """ 253 | unsupported = [] 254 | for child in c._children: 255 | if child.nodeName == 'script' and c._level>1: 256 | name, gzMaterial = self._getGazeboMaterial(child) 257 | c.removeChild(c._children[0]) 258 | if name: 259 | c.setAttribute("name", name) 260 | 261 | materials = gzMaterial.getColor(name) 262 | material, exists = self._getMaterialNode(name) 263 | if materials: 264 | if not exists: 265 | color = Item(self.createElement('color'), parent=c, document=self._document, level=c._level+1, root=self.getRootNode()) 266 | rgba = materials[0].args + ['1.0', '1.0', '1.0', '1.0'] 267 | color.setAttribute("rgba", " ".join(rgba[:4])) # rgba should contains 4 element, some of gazebo's material could contains 3 elements 268 | material.appendChild(color) 269 | else: 270 | query, fn = f'material[name={name}].technique.pass.ambient', gzMaterial.getFilename() 271 | raise Exception(f"Material not found ({query}) at {fn}") 272 | else: 273 | unsupported.append(child) 274 | 275 | if not c._children: 276 | sys.stderr.write(f"WARN: empty material: {c}\n") 277 | if unsupported: 278 | sys.stderr.write(f"WARN: Unsupported one of material subtag: {unsupported}\n") 279 | #raise Exception() 280 | elif c.nodeName == 'pose': 281 | """ 282 | 0 0 0 0 0 -1.0471975512 283 | --- 284 | 285 | """ 286 | if self.nodeName == 'robot': 287 | self.removeChild(c) 288 | else: 289 | # Pose replaced with Origin 290 | origin = Item(self.createElement("origin"), parent=self, document=self._document, level=self._level+1, root=self.getRootNode()) 291 | try: 292 | text = c.text().strip().split(' ') 293 | except: 294 | text = ['0', '0', '0', '0', '0', '0'] 295 | origin.setAttribute("xyz", " ".join(text[:3])) 296 | origin.setAttribute("rpy", " ".join(text[3:])) 297 | self.replaceChild(origin, c) 298 | 299 | elif c.nodeName in ['parent', 'child', 'mass']: 300 | """ 301 | @text 302 | --- 303 | 304 | """ 305 | attr = dict(zip( 306 | ('parent', 'child', 'mass'), 307 | ('link', 'link', 'value'))).get(c.nodeName) 308 | if len(c._node.childNodes): 309 | c.setAttribute(attr, c.text().strip()) 310 | cnodes = c._node.childNodes[:] 311 | for mc in cnodes: 312 | c._node.removeChild(mc) 313 | for mc in c._children: 314 | c.removeChild(mc) 315 | 316 | elif c.nodeName == 'mesh': 317 | mesh = c._node 318 | for mc in c._children[:]: 319 | if mc.nodeName == "scale": 320 | mesh.setAttribute("scale", mc.text()) 321 | elif mc.nodeName == "uri": 322 | model = mc.text().strip() 323 | mesh.setAttribute("filename", "file://"+self.uriToPath(model)) 324 | else: 325 | sys.stderr.write("Ignored tagname {0} at level {1}: {2}".format(c.nodeName, self._level, mc.nodeName)) 326 | c.removeChild(mc) 327 | 328 | elif c.nodeName == 'axis': 329 | """ 330 | 331 | 0 -1 0 332 | 333 | 0 334 | 0 335 | 100 336 | -1 337 | 338 | 339 | 0.1 340 | 341 | 1 342 | 343 | --- 344 | 345 | 346 | """ 347 | c.convert() 348 | for mc in c._children[:]: 349 | if mc.nodeName == 'xyz': 350 | c.setAttribute(mc.nodeName, mc._node.childNodes[0].toxml().strip()) 351 | c.removeChild(mc) 352 | if mc.nodeName == 'limit': 353 | self.appendChild(mc) 354 | if mc.nodeName == 'dynamics': 355 | if mc.getAttribute('damping') or mc.getAttribute('friction'): 356 | self.appendChild(mc) 357 | else: 358 | c.removeChild(mc) 359 | _props = self._node.childNodes[:] 360 | for prop in _props: 361 | if prop.nodeName == 'origin': 362 | break 363 | else: 364 | origin = Item(self.createElement("origin"), parent=self, document=self._document, level=self._level+1, root=self.getRootNode()) 365 | for name,value in c._node.attributes.items(): 366 | origin.setAttribute(name, value) 367 | self.appendChild(origin) 368 | #self.removeChild(c) 369 | 370 | for c in self._children: 371 | # Fix broken tree 372 | if c._node.parentNode != self._node: 373 | self._node.appendChild(c._node) 374 | c._parent = self 375 | if c.nodeName == 'limit': 376 | c.convert() 377 | c.convert() 378 | 379 | def toxml(self): 380 | return self._node.toprettyxml(indent=" ") 381 | 382 | def dumptree(self): 383 | sys.stderr.write("{0}{1}: {2}({3})\n".format(" "*self._level, self._level, self.nodeName, len(self._children))) 384 | for c in self._children: 385 | c.dumptree() 386 | 387 | 388 | def main(): 389 | load_gazebo_setup() 390 | 391 | if len(sys.argv) == 1 or len(sys.argv) > 3 or 'help' in sys.argv or '--help' in sys.argv or '-h' in sys.argv: 392 | sys.stdout.write("""usage: {0} []\n\n By default content is printed to stdout.\n\n""".format(sys.argv[0])) 393 | exit(1) 394 | if len(sys.argv) == 3 and os.path.realpath(sys.argv[1]) == os.path.realpath(sys.argv[2]): 395 | raise Exception("Input and output filenames is the same file") 396 | xml = parse(None, sys.argv[1]) 397 | output = sys.stdout 398 | if len(sys.argv) == 3: 399 | output = open(sys.argv[2], "w") 400 | 401 | doc = Item(xml) 402 | doc.convert() 403 | 404 | output.write(doc.toxml()) 405 | 406 | if __name__ == '__main__': 407 | main() 408 | 409 | # vim: ts=4 sw=4 noet 410 | --------------------------------------------------------------------------------