bullet3/data/xacro_standalone.py

#! /usr/bin/env python
# Copyright (c) 2013, Willow Garage, Inc.
# Copyright (c) 2014, Open Source Robotics Foundation, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Open Source Robotics Foundation, Inc.
#       nor the names of its contributors may be used to endorse or promote
#       products derived from this software without specific prior
#       written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

# Author: Stuart Glaser
# Maintainer: William Woodall <william@osrfoundation.org>

from __future__ import print_function

import getopt
import glob
import os
import re
import string
import sys
import xml

from xml.dom.minidom import parse

try:
    _basestr = basestring
except NameError:
    _basestr = str

# Dictionary of subtitution args
substitution_args_context = {}


class XacroException(Exception):
    pass


def isnumber(x):
    return hasattr(x, '__int__')


# Better pretty printing of xml
# Taken from http://ronrothman.com/public/leftbraned/xml-dom-minidom-toprettyxml-and-silly-whitespace/
def fixed_writexml(self, writer, indent="", addindent="", newl=""):
    # indent = current indentation
    # addindent = indentation to add to higher levels
    # newl = newline string
    writer.write(indent + "<" + self.tagName)

    attrs = self._get_attributes()
    a_names = list(attrs.keys())
    a_names.sort()

    for a_name in a_names:
        writer.write(" %s=\"" % a_name)
        xml.dom.minidom._write_data(writer, attrs[a_name].value)
        writer.write("\"")
    if self.childNodes:
        if len(self.childNodes) == 1 \
           and self.childNodes[0].nodeType == xml.dom.minidom.Node.TEXT_NODE:
            writer.write(">")
            self.childNodes[0].writexml(writer, "", "", "")
            writer.write("</%s>%s" % (self.tagName, newl))
            return
        writer.write(">%s" % (newl))
        for node in self.childNodes:
            # skip whitespace-only text nodes
            if node.nodeType == xml.dom.minidom.Node.TEXT_NODE and \
               not node.data.strip():
                continue
            node.writexml(writer, indent + addindent, addindent, newl)
        writer.write("%s</%s>%s" % (indent, self.tagName, newl))
    else:
        writer.write("/>%s" % (newl))
# replace minidom's function with ours
xml.dom.minidom.Element.writexml = fixed_writexml


class Table:
    def __init__(self, parent=None):
        self.parent = parent
        self.table = {}

    def __getitem__(self, key):
        if key in self.table:
            return self.table[key]
        elif self.parent:
            return self.parent[key]
        else:
            raise KeyError(key)

    def __setitem__(self, key, value):
        self.table[key] = value

    def __contains__(self, key):
        return \
            key in self.table or \
            (self.parent and key in self.parent)


class QuickLexer(object):
    def __init__(self, **res):
        self.str = ""
        self.top = None
        self.res = []
        for k, v in res.items():
            self.__setattr__(k, len(self.res))
            self.res.append(v)

    def lex(self, str):
        self.str = str
        self.top = None
        self.next()

    def peek(self):
        return self.top

    def next(self):
        result = self.top
        self.top = None
        for i in range(len(self.res)):
            m = re.match(self.res[i], self.str)
            if m:
                self.top = (i, m.group(0))
                self.str = self.str[m.end():]
                break
        return result


def first_child_element(elt):
    c = elt.firstChild
    while c:
        if c.nodeType == xml.dom.Node.ELEMENT_NODE:
            return c
        c = c.nextSibling
    return None


def next_sibling_element(elt):
    c = elt.nextSibling
    while c:
        if c.nodeType == xml.dom.Node.ELEMENT_NODE:
            return c
        c = c.nextSibling
    return None


# Pre-order traversal of the elements
def next_element(elt):
    child = first_child_element(elt)
    if child:
        return child
    while elt and elt.nodeType == xml.dom.Node.ELEMENT_NODE:
        next = next_sibling_element(elt)
        if next:
            return next
        elt = elt.parentNode
    return None


# Pre-order traversal of all the nodes
def next_node(node):
    if node.firstChild:
        return node.firstChild
    while node:
        if node.nextSibling:
            return node.nextSibling
        node = node.parentNode
    return None


def child_nodes(elt):
    c = elt.firstChild
    while c:
        yield c
        c = c.nextSibling

all_includes = []

# Deprecated message for <include> tags that don't have <xacro:include> prepended:
deprecated_include_msg = """DEPRECATED IN HYDRO:
  The <include> tag should be prepended with 'xacro' if that is the intended use
  of it, such as <xacro:include ...>. Use the following script to fix incorrect
  xacro includes:
     sed -i 's/<include/<xacro:include/g' `find . -iname *.xacro`"""

include_no_matches_msg = """Include tag filename spec \"{}\" matched no files."""


## @throws XacroException if a parsing error occurs with an included document
def process_includes(doc, base_dir):
    namespaces = {}
    previous = doc.documentElement
    elt = next_element(previous)
    while elt:
        # Xacro should not use plain 'include' tags but only namespaced ones. Causes conflicts with
        # other XML elements including Gazebo's <gazebo> extensions
        is_include = False
        if elt.tagName == 'xacro:include' or elt.tagName == 'include':

            is_include = True
            # Temporary fix for ROS Hydro and the xacro include scope problem
            if elt.tagName == 'include':

                # check if there is any element within the <include> tag. mostly we are concerned
                # with Gazebo's <uri> element, but it could be anything. also, make sure the child
                # nodes aren't just a single Text node, which is still considered a deprecated
                # instance
                if elt.childNodes and not (len(elt.childNodes) == 1 and
                                           elt.childNodes[0].nodeType == elt.TEXT_NODE):
                    # this is not intended to be a xacro element, so we can ignore it
                    is_include = False
                else:
                    # throw a deprecated warning
                    print(deprecated_include_msg, file=sys.stderr)

        # Process current element depending on previous conditions
        if is_include:
            filename_spec = eval_text(elt.getAttribute('filename'), {})
            if not os.path.isabs(filename_spec):
                filename_spec = os.path.join(base_dir, filename_spec)

            if re.search('[*[?]+', filename_spec):
                # Globbing behaviour
                filenames = sorted(glob.glob(filename_spec))
                if len(filenames) == 0:
                    print(include_no_matches_msg.format(filename_spec), file=sys.stderr)
            else:
                # Default behaviour
                filenames = [filename_spec]

            for filename in filenames:
                global all_includes
                all_includes.append(filename)
                try:
                    with open(filename) as f:
                        try:
                            included = parse(f)
                        except Exception as e:
                            raise XacroException(
                                "included file \"%s\" generated an error during XML parsing: %s"
                                % (filename, str(e)))
                except IOError as e:
                    raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e)))

                # Replaces the include tag with the elements of the included file
                for c in child_nodes(included.documentElement):
                    elt.parentNode.insertBefore(c.cloneNode(deep=True), elt)

                # Grabs all the declared namespaces of the included document
                for name, value in included.documentElement.attributes.items():
                    if name.startswith('xmlns:'):
                        namespaces[name] = value

            elt.parentNode.removeChild(elt)
            elt = None
        else:
            previous = elt

        elt = next_element(previous)

    # Makes sure the final document declares all the namespaces of the included documents.
    for k, v in namespaces.items():
        doc.documentElement.setAttribute(k, v)


# Returns a dictionary: { macro_name => macro_xml_block }
def grab_macros(doc):
    macros = {}

    previous = doc.documentElement
    elt = next_element(previous)
    while elt:
        if elt.tagName == 'macro' or elt.tagName == 'xacro:macro':
            name = elt.getAttribute('name')

            macros[name] = elt
            macros['xacro:' + name] = elt

            elt.parentNode.removeChild(elt)
            elt = None
        else:
            previous = elt

        elt = next_element(previous)
    return macros


# Returns a Table of the properties
def grab_properties(doc):
    table = Table()

    previous = doc.documentElement
    elt = next_element(previous)
    while elt:
        if elt.tagName == 'property' or elt.tagName == 'xacro:property':
            name = elt.getAttribute('name')
            value = None

            if elt.hasAttribute('value'):
                value = elt.getAttribute('value')
            else:
                name = '**' + name
                value = elt  # debug

            bad = string.whitespace + "${}"
            has_bad = False
            for b in bad:
                if b in name:
                    has_bad = True
                    break

            if has_bad:
                sys.stderr.write('Property names may not have whitespace, ' +
                                 '"{", "}", or "$" : "' + name + '"')
            else:
                table[name] = value

            elt.parentNode.removeChild(elt)
            elt = None
        else:
            previous = elt

        elt = next_element(previous)
    return table


def eat_ignore(lex):
    while lex.peek() and lex.peek()[0] == lex.IGNORE:
        lex.next()


def eval_lit(lex, symbols):
    eat_ignore(lex)
    if lex.peek()[0] == lex.NUMBER:
        return float(lex.next()[1])
    if lex.peek()[0] == lex.SYMBOL:
        try:
            key = lex.next()[1]
            value = symbols[key]
        except KeyError as ex:
            raise XacroException("Property wasn't defined: %s" % str(ex))
        if not (isnumber(value) or isinstance(value, _basestr)):
            if value is None:
                raise XacroException("Property %s recursively used" % key)
            raise XacroException("WTF2")
        try:
            return int(value)
        except:
            try:
                return float(value)
            except:
                # prevent infinite recursion
                symbols[key] = None
                result = eval_text(value, symbols)
                # restore old entry
                symbols[key] = value
                return result
    raise XacroException("Bad literal")


def eval_factor(lex, symbols):
    eat_ignore(lex)

    neg = 1
    if lex.peek()[1] == '-':
        lex.next()
        neg = -1

    if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL]:
        return neg * eval_lit(lex, symbols)
    if lex.peek()[0] == lex.LPAREN:
        lex.next()
        eat_ignore(lex)
        result = eval_expr(lex, symbols)
        eat_ignore(lex)
        if lex.next()[0] != lex.RPAREN:
            raise XacroException("Unmatched left paren")
        eat_ignore(lex)
        return neg * result

    raise XacroException("Misplaced operator")


def eval_term(lex, symbols):
    eat_ignore(lex)

    result = 0
    if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL, lex.LPAREN] \
            or lex.peek()[1] == '-':
        result = eval_factor(lex, symbols)

    eat_ignore(lex)
    while lex.peek() and lex.peek()[1] in ['*', '/']:
        op = lex.next()[1]
        n = eval_factor(lex, symbols)

        if op == '*':
            result = float(result) * float(n)
        elif op == '/':
            result = float(result) / float(n)
        else:
            raise XacroException("WTF")
        eat_ignore(lex)
    return result


def eval_expr(lex, symbols):
    eat_ignore(lex)

    op = None
    if lex.peek()[0] == lex.OP:
        op = lex.next()[1]
        if not op in ['+', '-']:
            raise XacroException("Invalid operation. Must be '+' or '-'")

    result = eval_term(lex, symbols)
    if op == '-':
        result = -float(result)

    eat_ignore(lex)
    while lex.peek() and lex.peek()[1] in ['+', '-']:
        op = lex.next()[1]
        n = eval_term(lex, symbols)

        if op == '+':
            result = float(result) + float(n)
        if op == '-':
            result = float(result) - float(n)
        eat_ignore(lex)
    return result


def eval_text(text, symbols):
    def handle_expr(s):
        lex = QuickLexer(IGNORE=r"\s+",
                         NUMBER=r"(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?",
                         SYMBOL=r"[a-zA-Z_]\w*",
                         OP=r"[\+\-\*/^]",
                         LPAREN=r"\(",
                         RPAREN=r"\)")
        lex.lex(s)
        return eval_expr(lex, symbols)

    def handle_extension(s):
        return ("$(%s)" % s)

    results = []
    lex = QuickLexer(DOLLAR_DOLLAR_BRACE=r"\$\$+\{",
                     EXPR=r"\$\{[^\}]*\}",
                     EXTENSION=r"\$\([^\)]*\)",
                     TEXT=r"([^\$]|\$[^{(]|\$$)+")
    lex.lex(text)
    while lex.peek():
        if lex.peek()[0] == lex.EXPR:
            results.append(handle_expr(lex.next()[1][2:-1]))
        elif lex.peek()[0] == lex.EXTENSION:
            results.append(handle_extension(lex.next()[1][2:-1]))
        elif lex.peek()[0] == lex.TEXT:
            results.append(lex.next()[1])
        elif lex.peek()[0] == lex.DOLLAR_DOLLAR_BRACE:
            results.append(lex.next()[1][1:])
    return ''.join(map(str, results))


# Expands macros, replaces properties, and evaluates expressions
def eval_all(root, macros, symbols):
    # Evaluates the attributes for the root node
    for at in root.attributes.items():
        result = eval_text(at[1], symbols)
        root.setAttribute(at[0], result)

    previous = root
    node = next_node(previous)
    while node:
        if node.nodeType == xml.dom.Node.ELEMENT_NODE:
            if node.tagName in macros:
                body = macros[node.tagName].cloneNode(deep=True)
                params = body.getAttribute('params').split()

                # Parse default values for any parameters
                defaultmap = {}
                for param in params[:]:
                    splitParam = param.split(':=')

                    if len(splitParam) == 2:
                        defaultmap[splitParam[0]] = splitParam[1]
                        params.remove(param)
                        params.append(splitParam[0])
                        
                    elif len(splitParam) != 1:
                        raise XacroException("Invalid parameter definition")

                # Expands the macro
                scoped = Table(symbols)
                for name, value in node.attributes.items():
                    if not name in params:
                        raise XacroException("Invalid parameter \"%s\" while expanding macro \"%s\"" %
                                             (str(name), str(node.tagName)))
                    params.remove(name)
                    scoped[name] = eval_text(value, symbols)

                # Pulls out the block arguments, in order
                cloned = node.cloneNode(deep=True)
                eval_all(cloned, macros, symbols)
                block = cloned.firstChild
                for param in params[:]:
                    if param[0] == '*':
                        while block and block.nodeType != xml.dom.Node.ELEMENT_NODE:
                            block = block.nextSibling
                        if not block:
                            raise XacroException("Not enough blocks while evaluating macro %s" % str(node.tagName))
                        params.remove(param)
                        scoped[param] = block
                        block = block.nextSibling

                # Try to load defaults for any remaining non-block parameters
                for param in params[:]:
                    if param[0] != '*' and param in defaultmap:
                        scoped[param] = defaultmap[param]
                        params.remove(param)

                if params:
                    raise XacroException("Parameters [%s] were not set for macro %s" %
                                         (",".join(params), str(node.tagName)))
                eval_all(body, macros, scoped)

                # Replaces the macro node with the expansion
                for e in list(child_nodes(body)):  # Ew
                    node.parentNode.insertBefore(e, node)
                node.parentNode.removeChild(node)

                node = None
            elif node.tagName == 'arg' or node.tagName == 'xacro:arg':
                name = node.getAttribute('name')
                if not name:
                    raise XacroException("Argument name missing")
                default = node.getAttribute('default')
                if default and name not in substitution_args_context['arg']:
                    substitution_args_context['arg'][name] = default
                
                node.parentNode.removeChild(node)
                node = None

            elif node.tagName == 'insert_block' or node.tagName == 'xacro:insert_block':
                name = node.getAttribute('name')

                if ("**" + name) in symbols:
                    # Multi-block
                    block = symbols['**' + name]

                    for e in list(child_nodes(block)):
                        node.parentNode.insertBefore(e.cloneNode(deep=True), node)
                    node.parentNode.removeChild(node)
                elif ("*" + name) in symbols:
                    # Single block
                    block = symbols['*' + name]

                    node.parentNode.insertBefore(block.cloneNode(deep=True), node)
                    node.parentNode.removeChild(node)
                else:
                    raise XacroException("Block \"%s\" was never declared" % name)

                node = None
            elif node.tagName in ['if', 'xacro:if', 'unless', 'xacro:unless']:
                value = eval_text(node.getAttribute('value'), symbols)
                try: 
                    if value == 'true': keep = True
                    elif value == 'false': keep = False
                    else: keep = float(value)
                except ValueError:
                    raise XacroException("Xacro conditional evaluated to \"%s\". Acceptable evaluations are one of [\"1\",\"true\",\"0\",\"false\"]" % value)
                if node.tagName in ['unless', 'xacro:unless']: keep = not keep
                if keep:
                    for e in list(child_nodes(node)):
                        node.parentNode.insertBefore(e.cloneNode(deep=True), node)

                node.parentNode.removeChild(node)
            else:
                # Evals the attributes
                for at in node.attributes.items():
                    result = eval_text(at[1], symbols)
                    node.setAttribute(at[0], result)
                previous = node
        elif node.nodeType == xml.dom.Node.TEXT_NODE:
            node.data = eval_text(node.data, symbols)
            previous = node
        else:
            previous = node

        node = next_node(previous)
    return macros


# Expands everything except includes
def eval_self_contained(doc):
    macros = grab_macros(doc)
    symbols = grab_properties(doc)
    eval_all(doc.documentElement, macros, symbols)


def print_usage(exit_code=0):
    print("Usage: %s [-o <output>] <input>" % 'xacro.py')
    print("       %s --deps       Prints dependencies" % 'xacro.py')
    print("       %s --includes   Only evalutes includes" % 'xacro.py')
    sys.exit(exit_code)


def set_substitution_args_context(context={}):
    substitution_args_context['arg'] = context

def open_output(output_filename):
    if output_filename is None:
        return sys.stdout
    else:
        return open(output_filename, 'w') 

def main():
    try:
        opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes'])
    except getopt.GetoptError as err:
        print(str(err))
        print_usage(2)

    just_deps = False
    just_includes = False

    output_filename = None
    for o, a in opts:
        if o == '-h':
            print_usage(0)
        elif o == '-o':
            output_filename = a 
        elif o == '--deps':
            just_deps = True
        elif o == '--includes':
            just_includes = True

    if len(args) < 1:
        print("No input given")
        print_usage(2)

    # Process substitution args
    # set_substitution_args_context(load_mappings(sys.argv))
    set_substitution_args_context((sys.argv))

    f = open(args[0])
    doc = None
    try:
        doc = parse(f)
    except xml.parsers.expat.ExpatError:
        sys.stderr.write("Expat parsing error.  Check that:\n")
        sys.stderr.write(" - Your XML is correctly formed\n")
        sys.stderr.write(" - You have the xacro xmlns declaration: " +
                         "xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n")
        sys.stderr.write("\n")
        raise
    finally:
        f.close()

    process_includes(doc, os.path.dirname(args[0]))
    if just_deps:
        for inc in all_includes:
            sys.stdout.write(inc + " ")
        sys.stdout.write("\n")
    elif just_includes:
        doc.writexml(open_output(output_filename))
        print()
    else:
        eval_self_contained(doc)
        banner = [xml.dom.minidom.Comment(c) for c in
                  [" %s " % ('=' * 83),
                   " |    This document was autogenerated by xacro from %-30s | " % args[0],
                   " |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED  %-30s | " % "",
                   " %s " % ('=' * 83)]]
        first = doc.firstChild
        for comment in banner:
            doc.insertBefore(comment, first)

        open_output(output_filename).write(doc.toprettyxml(indent='  '))
        print()

if __name__ == '__main__':
    main()