00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 from __future__ import print_function
00034
00035 import getopt
00036 import glob
00037 import os
00038 import re
00039 import string
00040 import sys
00041 import xml
00042
00043 from xml.dom.minidom import parse
00044
00045 from roslaunch import substitution_args
00046 from rosgraph.names import load_mappings
00047
00048 try:
00049 _basestr = basestring
00050 except NameError:
00051 _basestr = str
00052
00053
00054 substitution_args_context = {}
00055
00056
00057 class XacroException(Exception):
00058 pass
00059
00060
00061 def isnumber(x):
00062 return hasattr(x, '__int__')
00063
00064
00065 def eval_extension(str):
00066 return substitution_args.resolve_args(str, context=substitution_args_context, resolve_anon=False)
00067
00068
00069
00070
00071 def fixed_writexml(self, writer, indent="", addindent="", newl=""):
00072
00073
00074
00075 writer.write(indent + "<" + self.tagName)
00076
00077 attrs = self._get_attributes()
00078 a_names = list(attrs.keys())
00079 a_names.sort()
00080
00081 for a_name in a_names:
00082 writer.write(" %s=\"" % a_name)
00083 xml.dom.minidom._write_data(writer, attrs[a_name].value)
00084 writer.write("\"")
00085 if self.childNodes:
00086 if len(self.childNodes) == 1 \
00087 and self.childNodes[0].nodeType == xml.dom.minidom.Node.TEXT_NODE:
00088 writer.write(">")
00089 self.childNodes[0].writexml(writer, "", "", "")
00090 writer.write("</%s>%s" % (self.tagName, newl))
00091 return
00092 writer.write(">%s" % (newl))
00093 for node in self.childNodes:
00094
00095 if node.nodeType == xml.dom.minidom.Node.TEXT_NODE and \
00096 not node.data.strip():
00097 continue
00098 node.writexml(writer, indent + addindent, addindent, newl)
00099 writer.write("%s</%s>%s" % (indent, self.tagName, newl))
00100 else:
00101 writer.write("/>%s" % (newl))
00102
00103 xml.dom.minidom.Element.writexml = fixed_writexml
00104
00105
00106 class Table:
00107 def __init__(self, parent=None):
00108 self.parent = parent
00109 self.table = {}
00110
00111 def __getitem__(self, key):
00112 if key in self.table:
00113 return self.table[key]
00114 elif self.parent:
00115 return self.parent[key]
00116 else:
00117 raise KeyError(key)
00118
00119 def __setitem__(self, key, value):
00120 self.table[key] = value
00121
00122 def __contains__(self, key):
00123 return \
00124 key in self.table or \
00125 (self.parent and key in self.parent)
00126
00127
00128 class QuickLexer(object):
00129 def __init__(self, **res):
00130 self.str = ""
00131 self.top = None
00132 self.res = []
00133 for k, v in res.items():
00134 self.__setattr__(k, len(self.res))
00135 self.res.append(v)
00136
00137 def lex(self, str):
00138 self.str = str
00139 self.top = None
00140 self.next()
00141
00142 def peek(self):
00143 return self.top
00144
00145 def next(self):
00146 result = self.top
00147 self.top = None
00148 for i in range(len(self.res)):
00149 m = re.match(self.res[i], self.str)
00150 if m:
00151 self.top = (i, m.group(0))
00152 self.str = self.str[m.end():]
00153 break
00154 return result
00155
00156
00157 def first_child_element(elt):
00158 c = elt.firstChild
00159 while c:
00160 if c.nodeType == xml.dom.Node.ELEMENT_NODE:
00161 return c
00162 c = c.nextSibling
00163 return None
00164
00165
00166 def next_sibling_element(elt):
00167 c = elt.nextSibling
00168 while c:
00169 if c.nodeType == xml.dom.Node.ELEMENT_NODE:
00170 return c
00171 c = c.nextSibling
00172 return None
00173
00174
00175
00176 def next_element(elt):
00177 child = first_child_element(elt)
00178 if child:
00179 return child
00180 while elt and elt.nodeType == xml.dom.Node.ELEMENT_NODE:
00181 next = next_sibling_element(elt)
00182 if next:
00183 return next
00184 elt = elt.parentNode
00185 return None
00186
00187
00188
00189 def next_node(node):
00190 if node.firstChild:
00191 return node.firstChild
00192 while node:
00193 if node.nextSibling:
00194 return node.nextSibling
00195 node = node.parentNode
00196 return None
00197
00198
00199 def child_nodes(elt):
00200 c = elt.firstChild
00201 while c:
00202 yield c
00203 c = c.nextSibling
00204
00205 all_includes = []
00206
00207
00208 deprecated_include_msg = """DEPRECATED IN HYDRO:
00209 The <include> tag should be prepended with 'xacro' if that is the intended use
00210 of it, such as <xacro:include ...>. Use the following script to fix incorrect
00211 xacro includes:
00212 sed -i 's/<include/<xacro:include/g' `find . -iname *.xacro`"""
00213
00214 include_no_matches_msg = """Include tag filename spec \"{}\" matched no files."""
00215
00216
00217
00218 def process_includes(doc, base_dir):
00219 namespaces = {}
00220 previous = doc.documentElement
00221 elt = next_element(previous)
00222 while elt:
00223
00224
00225 is_include = False
00226 if elt.tagName == 'xacro:include' or elt.tagName == 'include':
00227
00228 is_include = True
00229
00230 if elt.tagName == 'include':
00231
00232
00233
00234
00235
00236 if elt.childNodes and not (len(elt.childNodes) == 1 and
00237 elt.childNodes[0].nodeType == elt.TEXT_NODE):
00238
00239 is_include = False
00240 else:
00241
00242 print(deprecated_include_msg, file=sys.stderr)
00243
00244
00245 if is_include:
00246 filename_spec = eval_text(elt.getAttribute('filename'), {})
00247 if not os.path.isabs(filename_spec):
00248 filename_spec = os.path.join(base_dir, filename_spec)
00249
00250 if re.search('[*[?]+', filename_spec):
00251
00252 filenames = sorted(glob.glob(filename_spec))
00253 if len(filenames) == 0:
00254 print(include_no_matches_msg.format(filename_spec), file=sys.stderr)
00255 else:
00256
00257 filenames = [filename_spec]
00258
00259 for filename in filenames:
00260 global all_includes
00261 all_includes.append(filename)
00262 try:
00263 with open(filename) as f:
00264 try:
00265 included = parse(f)
00266 except Exception as e:
00267 raise XacroException(
00268 "included file \"%s\" generated an error during XML parsing: %s"
00269 % (filename, str(e)))
00270 except IOError as e:
00271 raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e)))
00272
00273
00274 for c in child_nodes(included.documentElement):
00275 elt.parentNode.insertBefore(c.cloneNode(deep=True), elt)
00276
00277
00278 for name, value in included.documentElement.attributes.items():
00279 if name.startswith('xmlns:'):
00280 namespaces[name] = value
00281
00282 elt.parentNode.removeChild(elt)
00283 elt = None
00284 else:
00285 previous = elt
00286
00287 elt = next_element(previous)
00288
00289
00290 for k, v in namespaces.items():
00291 doc.documentElement.setAttribute(k, v)
00292
00293
00294
00295 def grab_macros(doc):
00296 macros = {}
00297
00298 previous = doc.documentElement
00299 elt = next_element(previous)
00300 while elt:
00301 if elt.tagName == 'macro' or elt.tagName == 'xacro:macro':
00302 name = elt.getAttribute('name')
00303
00304 macros[name] = elt
00305 macros['xacro:' + name] = elt
00306
00307 elt.parentNode.removeChild(elt)
00308 elt = None
00309 else:
00310 previous = elt
00311
00312 elt = next_element(previous)
00313 return macros
00314
00315
00316
00317 def grab_properties(doc):
00318 table = Table()
00319
00320 previous = doc.documentElement
00321 elt = next_element(previous)
00322 while elt:
00323 if elt.tagName == 'property' or elt.tagName == 'xacro:property':
00324 name = elt.getAttribute('name')
00325 value = None
00326
00327 if elt.hasAttribute('value'):
00328 value = elt.getAttribute('value')
00329 else:
00330 name = '**' + name
00331 value = elt
00332
00333 bad = string.whitespace + "${}"
00334 has_bad = False
00335 for b in bad:
00336 if b in name:
00337 has_bad = True
00338 break
00339
00340 if has_bad:
00341 sys.stderr.write('Property names may not have whitespace, ' +
00342 '"{", "}", or "$" : "' + name + '"')
00343 else:
00344 table[name] = value
00345
00346 elt.parentNode.removeChild(elt)
00347 elt = None
00348 else:
00349 previous = elt
00350
00351 elt = next_element(previous)
00352 return table
00353
00354
00355 def eat_ignore(lex):
00356 while lex.peek() and lex.peek()[0] == lex.IGNORE:
00357 lex.next()
00358
00359
00360 def eval_lit(lex, symbols):
00361 eat_ignore(lex)
00362 if lex.peek()[0] == lex.NUMBER:
00363 return float(lex.next()[1])
00364 if lex.peek()[0] == lex.SYMBOL:
00365 try:
00366 key = lex.next()[1]
00367 value = symbols[key]
00368 except KeyError as ex:
00369 raise XacroException("Property wasn't defined: %s" % str(ex))
00370 if not (isnumber(value) or isinstance(value, _basestr)):
00371 if value is None:
00372 raise XacroException("Property %s recursively used" % key)
00373 raise XacroException("WTF2")
00374 try:
00375 return int(value)
00376 except:
00377 try:
00378 return float(value)
00379 except:
00380
00381 symbols[key] = None
00382 result = eval_text(value, symbols)
00383
00384 symbols[key] = value
00385 return result
00386 raise XacroException("Bad literal")
00387
00388
00389 def eval_factor(lex, symbols):
00390 eat_ignore(lex)
00391
00392 neg = 1
00393 if lex.peek()[1] == '-':
00394 lex.next()
00395 neg = -1
00396
00397 if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL]:
00398 return neg * eval_lit(lex, symbols)
00399 if lex.peek()[0] == lex.LPAREN:
00400 lex.next()
00401 eat_ignore(lex)
00402 result = eval_expr(lex, symbols)
00403 eat_ignore(lex)
00404 if lex.next()[0] != lex.RPAREN:
00405 raise XacroException("Unmatched left paren")
00406 eat_ignore(lex)
00407 return neg * result
00408
00409 raise XacroException("Misplaced operator")
00410
00411
00412 def eval_term(lex, symbols):
00413 eat_ignore(lex)
00414
00415 result = 0
00416 if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL, lex.LPAREN] \
00417 or lex.peek()[1] == '-':
00418 result = eval_factor(lex, symbols)
00419
00420 eat_ignore(lex)
00421 while lex.peek() and lex.peek()[1] in ['*', '/']:
00422 op = lex.next()[1]
00423 n = eval_factor(lex, symbols)
00424
00425 if op == '*':
00426 result = float(result) * float(n)
00427 elif op == '/':
00428 result = float(result) / float(n)
00429 else:
00430 raise XacroException("WTF")
00431 eat_ignore(lex)
00432 return result
00433
00434
00435 def eval_expr(lex, symbols):
00436 eat_ignore(lex)
00437
00438 op = None
00439 if lex.peek()[0] == lex.OP:
00440 op = lex.next()[1]
00441 if not op in ['+', '-']:
00442 raise XacroException("Invalid operation. Must be '+' or '-'")
00443
00444 result = eval_term(lex, symbols)
00445 if op == '-':
00446 result = -float(result)
00447
00448 eat_ignore(lex)
00449 while lex.peek() and lex.peek()[1] in ['+', '-']:
00450 op = lex.next()[1]
00451 n = eval_term(lex, symbols)
00452
00453 if op == '+':
00454 result = float(result) + float(n)
00455 if op == '-':
00456 result = float(result) - float(n)
00457 eat_ignore(lex)
00458 return result
00459
00460
00461 def eval_text(text, symbols):
00462 def handle_expr(s):
00463 lex = QuickLexer(IGNORE=r"\s+",
00464 NUMBER=r"(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?",
00465 SYMBOL=r"[a-zA-Z_]\w*",
00466 OP=r"[\+\-\*/^]",
00467 LPAREN=r"\(",
00468 RPAREN=r"\)")
00469 lex.lex(s)
00470 return eval_expr(lex, symbols)
00471
00472 def handle_extension(s):
00473 return eval_extension("$(%s)" % s)
00474
00475 results = []
00476 lex = QuickLexer(DOLLAR_DOLLAR_BRACE=r"\$\$+\{",
00477 EXPR=r"\$\{[^\}]*\}",
00478 EXTENSION=r"\$\([^\)]*\)",
00479 TEXT=r"([^\$]|\$[^{(]|\$$)+")
00480 lex.lex(text)
00481 while lex.peek():
00482 if lex.peek()[0] == lex.EXPR:
00483 results.append(handle_expr(lex.next()[1][2:-1]))
00484 elif lex.peek()[0] == lex.EXTENSION:
00485 results.append(handle_extension(lex.next()[1][2:-1]))
00486 elif lex.peek()[0] == lex.TEXT:
00487 results.append(lex.next()[1])
00488 elif lex.peek()[0] == lex.DOLLAR_DOLLAR_BRACE:
00489 results.append(lex.next()[1][1:])
00490 return ''.join(map(str, results))
00491
00492
00493
00494 def eval_all(root, macros, symbols):
00495
00496 for at in root.attributes.items():
00497 result = eval_text(at[1], symbols)
00498 root.setAttribute(at[0], result)
00499
00500 previous = root
00501 node = next_node(previous)
00502 while node:
00503 if node.nodeType == xml.dom.Node.ELEMENT_NODE:
00504 if node.tagName in macros:
00505 body = macros[node.tagName].cloneNode(deep=True)
00506 params = body.getAttribute('params').split()
00507
00508
00509 defaultmap = {}
00510 for param in params[:]:
00511 splitParam = param.split(':=')
00512
00513 if len(splitParam) == 2:
00514 defaultmap[splitParam[0]] = splitParam[1]
00515 params.remove(param)
00516 params.append(splitParam[0])
00517
00518 elif len(splitParam) != 1:
00519 raise XacroException("Invalid parameter definition")
00520
00521
00522 scoped = Table(symbols)
00523 for name, value in node.attributes.items():
00524 if not name in params:
00525 raise XacroException("Invalid parameter \"%s\" while expanding macro \"%s\"" %
00526 (str(name), str(node.tagName)))
00527 params.remove(name)
00528 scoped[name] = eval_text(value, symbols)
00529
00530
00531 cloned = node.cloneNode(deep=True)
00532 eval_all(cloned, macros, symbols)
00533 block = cloned.firstChild
00534 for param in params[:]:
00535 if param[0] == '*':
00536 while block and block.nodeType != xml.dom.Node.ELEMENT_NODE:
00537 block = block.nextSibling
00538 if not block:
00539 raise XacroException("Not enough blocks while evaluating macro %s" % str(node.tagName))
00540 params.remove(param)
00541 scoped[param] = block
00542 block = block.nextSibling
00543
00544
00545 for param in params[:]:
00546 if param[0] != '*' and param in defaultmap:
00547 scoped[param] = defaultmap[param]
00548 params.remove(param)
00549
00550 if params:
00551 raise XacroException("Parameters [%s] were not set for macro %s" %
00552 (",".join(params), str(node.tagName)))
00553 eval_all(body, macros, scoped)
00554
00555
00556 for e in list(child_nodes(body)):
00557 node.parentNode.insertBefore(e, node)
00558 node.parentNode.removeChild(node)
00559
00560 node = None
00561 elif node.tagName == 'xacro:arg':
00562 name = node.getAttribute('name')
00563 if not name:
00564 raise XacroException("Argument name missing")
00565 if node.hasAttribute('default'):
00566 default = node.getAttribute('default')
00567 if name not in substitution_args_context['arg']:
00568 substitution_args_context['arg'][name] = default
00569
00570 node.parentNode.removeChild(node)
00571 node = None
00572
00573 elif node.tagName == 'insert_block' or node.tagName == 'xacro:insert_block':
00574 name = node.getAttribute('name')
00575
00576 if ("**" + name) in symbols:
00577
00578 block = symbols['**' + name]
00579
00580 for e in list(child_nodes(block)):
00581 node.parentNode.insertBefore(e.cloneNode(deep=True), node)
00582 node.parentNode.removeChild(node)
00583 elif ("*" + name) in symbols:
00584
00585 block = symbols['*' + name]
00586
00587 node.parentNode.insertBefore(block.cloneNode(deep=True), node)
00588 node.parentNode.removeChild(node)
00589 else:
00590 raise XacroException("Block \"%s\" was never declared" % name)
00591
00592 node = None
00593 elif node.tagName in ['if', 'xacro:if', 'unless', 'xacro:unless']:
00594 value = eval_text(node.getAttribute('value'), symbols)
00595 try:
00596 if value == 'true': keep = True
00597 elif value == 'false': keep = False
00598 else: keep = float(value)
00599 except ValueError:
00600 raise XacroException("Xacro conditional evaluated to \"%s\". Acceptable evaluations are one of [\"1\",\"true\",\"0\",\"false\"]" % value)
00601 if node.tagName in ['unless', 'xacro:unless']: keep = not keep
00602 if keep:
00603 for e in list(child_nodes(node)):
00604 node.parentNode.insertBefore(e.cloneNode(deep=True), node)
00605
00606 node.parentNode.removeChild(node)
00607 else:
00608
00609 for at in node.attributes.items():
00610 result = eval_text(at[1], symbols)
00611 node.setAttribute(at[0], result)
00612 previous = node
00613 elif node.nodeType == xml.dom.Node.TEXT_NODE:
00614 node.data = eval_text(node.data, symbols)
00615 previous = node
00616 else:
00617 previous = node
00618
00619 node = next_node(previous)
00620 return macros
00621
00622
00623
00624 def eval_self_contained(doc):
00625 macros = grab_macros(doc)
00626 symbols = grab_properties(doc)
00627 eval_all(doc.documentElement, macros, symbols)
00628
00629
00630 def print_usage(exit_code=0):
00631 print("Usage: %s [-o <output>] <input>" % 'xacro.py')
00632 print(" %s --deps Prints dependencies" % 'xacro.py')
00633 print(" %s --includes Only evalutes includes" % 'xacro.py')
00634 sys.exit(exit_code)
00635
00636
00637 def set_substitution_args_context(context={}):
00638 substitution_args_context['arg'] = context
00639
00640 def open_output(output_filename):
00641 if output_filename is None:
00642 return sys.stdout
00643 else:
00644 return open(output_filename, 'w')
00645
00646 def main():
00647 try:
00648 opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes'])
00649 except getopt.GetoptError as err:
00650 print(str(err))
00651 print_usage(2)
00652
00653 just_deps = False
00654 just_includes = False
00655
00656 output_filename = None
00657 for o, a in opts:
00658 if o == '-h':
00659 print_usage(0)
00660 elif o == '-o':
00661 output_filename = a
00662 elif o == '--deps':
00663 just_deps = True
00664 elif o == '--includes':
00665 just_includes = True
00666
00667 if len(args) < 1:
00668 print("No input given")
00669 print_usage(2)
00670
00671
00672 set_substitution_args_context(load_mappings(sys.argv))
00673
00674 f = open(args[0])
00675 doc = None
00676 try:
00677 doc = parse(f)
00678 except xml.parsers.expat.ExpatError:
00679 sys.stderr.write("Expat parsing error. Check that:\n")
00680 sys.stderr.write(" - Your XML is correctly formed\n")
00681 sys.stderr.write(" - You have the xacro xmlns declaration: " +
00682 "xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n")
00683 sys.stderr.write("\n")
00684 raise
00685 finally:
00686 f.close()
00687
00688 process_includes(doc, os.path.dirname(args[0]))
00689 if just_deps:
00690 for inc in all_includes:
00691 sys.stdout.write(inc + " ")
00692 sys.stdout.write("\n")
00693 elif just_includes:
00694 doc.writexml(open_output(output_filename))
00695 print()
00696 else:
00697 eval_self_contained(doc)
00698 banner = [xml.dom.minidom.Comment(c) for c in
00699 [" %s " % ('=' * 83),
00700 " | This document was autogenerated by xacro from %-30s | " % args[0],
00701 " | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | " % "",
00702 " %s " % ('=' * 83)]]
00703 first = doc.firstChild
00704 for comment in banner:
00705 doc.insertBefore(comment, first)
00706
00707 open_output(output_filename).write(doc.toprettyxml(indent=' '))
00708 print()