♻️ Update sim structure
This commit is contained in:
Rune Harlyk
@@ -0,0 +1,120 @@
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
|
||||
from src.robot.kinematics import BodyStateT
|
||||
from src.robot.gait import GaitStateT, GaitType, default_stand_frac, default_offset
|
||||
|
||||
|
||||
class GUI:
|
||||
def __init__(self, bot):
|
||||
self.robot = bot
|
||||
self.c_yaw = 10
|
||||
self.c_pitch = -17
|
||||
self.c_distance = 5
|
||||
|
||||
self.x_slider = p.addUserDebugParameter("x", -50, 50, 0)
|
||||
self.y_slider = p.addUserDebugParameter("y", -50, 50, 0)
|
||||
self.z_slider = p.addUserDebugParameter("z", -50, 50, 0)
|
||||
self.yaw_slider = p.addUserDebugParameter(
|
||||
"yaw", -np.pi / 4, np.pi / 4, 0)
|
||||
self.pitch_slider = p.addUserDebugParameter(
|
||||
"pitch", -np.pi / 4, np.pi / 4, 0)
|
||||
self.roll_slider = p.addUserDebugParameter(
|
||||
"roll", -np.pi / 4, np.pi / 4, 0)
|
||||
|
||||
self.pivot_x_slider = p.addUserDebugParameter("pivot x", -50, 50, 0)
|
||||
self.pivot_y_slider = p.addUserDebugParameter("pivot y", -50, 50, 0)
|
||||
self.pivot_z_slider = p.addUserDebugParameter("pivot z", -50, 50, 0)
|
||||
|
||||
self.step_x_slider = p.addUserDebugParameter("Step x", -50, 50, 0)
|
||||
self.step_z_slider = p.addUserDebugParameter("Step z", -50, 50, 0)
|
||||
self.angle_slider = p.addUserDebugParameter(
|
||||
"Angle", -np.pi / 4, np.pi / 4, 0)
|
||||
self.step_height_slider = p.addUserDebugParameter(
|
||||
"Step height", 0, 50, 15)
|
||||
self.step_depth_slider = p.addUserDebugParameter(
|
||||
"Step depth", 0, 0.01, 0.002)
|
||||
self.speed_slider = p.addUserDebugParameter("Speed", 0, 2, 1)
|
||||
self.stand_frac_slider = p.addUserDebugParameter(
|
||||
"Stand frac", 0, 1, 0.5)
|
||||
|
||||
self.gait_type_slider = p.addUserDebugParameter(
|
||||
"Gait Type", 0, len(GaitType) - 1, 0)
|
||||
|
||||
# self.gait_type_slider = p.addUserDebugParameter("Gait Type", 0, len(GaitType) - 1, 0, paramType=p.GUI_ENUM,
|
||||
# enumNames=[g.value for g in GaitType])
|
||||
self.last_gait_type = GaitType.TROT_GATE
|
||||
|
||||
def update_gait_state(self, gait_state: GaitStateT):
|
||||
gait_state["step_x"] = p.readUserDebugParameter(self.step_x_slider)
|
||||
gait_state["step_z"] = p.readUserDebugParameter(self.step_z_slider)
|
||||
gait_state["step_angle"] = p.readUserDebugParameter(self.angle_slider)
|
||||
gait_state["step_height"] = p.readUserDebugParameter(
|
||||
self.step_height_slider)
|
||||
gait_state["step_depth"] = p.readUserDebugParameter(
|
||||
self.step_depth_slider)
|
||||
gait_state["step_velocity"] = p.readUserDebugParameter(
|
||||
self.speed_slider)
|
||||
gait_state["stand_frac"] = p.readUserDebugParameter(
|
||||
self.stand_frac_slider)
|
||||
gait_state["offset"] = default_offset[self.last_gait_type]
|
||||
|
||||
def update_body_state(self, body_state: BodyStateT):
|
||||
body_state["xm"] = p.readUserDebugParameter(self.x_slider)
|
||||
body_state["ym"] = p.readUserDebugParameter(self.y_slider)
|
||||
body_state["zm"] = p.readUserDebugParameter(self.z_slider)
|
||||
body_state["omega"] = p.readUserDebugParameter(self.roll_slider)
|
||||
body_state["phi"] = p.readUserDebugParameter(self.pitch_slider)
|
||||
body_state["psi"] = p.readUserDebugParameter(self.yaw_slider)
|
||||
body_state["px"] = p.readUserDebugParameter(self.pivot_x_slider)
|
||||
body_state["py"] = p.readUserDebugParameter(self.pivot_y_slider)
|
||||
body_state["pz"] = p.readUserDebugParameter(self.pivot_z_slider)
|
||||
|
||||
def update(self):
|
||||
gait_type = GaitType(
|
||||
int(p.readUserDebugParameter(self.gait_type_slider)))
|
||||
if gait_type != self.last_gait_type:
|
||||
self.last_gait_type = gait_type
|
||||
p.removeUserDebugItem(self.stand_frac_slider)
|
||||
self.stand_frac_slider = p.addUserDebugParameter(
|
||||
"Stand frac", 0, 1, default_stand_frac[gait_type])
|
||||
|
||||
quadruped_pos, _ = p.getBasePositionAndOrientation(self.robot)
|
||||
p.resetDebugVisualizerCamera(
|
||||
cameraDistance=self.c_distance,
|
||||
cameraYaw=self.c_yaw,
|
||||
cameraPitch=self.c_pitch,
|
||||
cameraTargetPosition=quadruped_pos,
|
||||
)
|
||||
|
||||
keys = p.getKeyboardEvents()
|
||||
if keys.get(ord("j")):
|
||||
self.c_yaw += 0.1
|
||||
if keys.get(ord("k")):
|
||||
self.c_yaw -= 0.1
|
||||
if keys.get(ord("m")):
|
||||
self.c_pitch += 0.1
|
||||
if keys.get(ord("i")):
|
||||
self.c_pitch -= 0.1
|
||||
|
||||
if keys.get(ord("q")) or keys.get(27):
|
||||
p.disconnect()
|
||||
exit()
|
||||
|
||||
self.position = np.array(
|
||||
[
|
||||
p.readUserDebugParameter(self.x_slider),
|
||||
p.readUserDebugParameter(self.y_slider),
|
||||
p.readUserDebugParameter(self.z_slider),
|
||||
]
|
||||
)
|
||||
|
||||
self.orientation = np.array(
|
||||
[
|
||||
p.readUserDebugParameter(self.roll_slider),
|
||||
p.readUserDebugParameter(self.pitch_slider),
|
||||
p.readUserDebugParameter(self.yaw_slider),
|
||||
]
|
||||
)
|
||||
|
||||
return self.position, self.orientation
|
||||
@@ -0,0 +1,638 @@
|
||||
#! /usr/bin/env python
|
||||
# Copyright (c) 2008, Willow Garage, 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 Willow Garage, 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
|
||||
# Modified by Saul Reynolds-Haertle Oct 14 2012 to remove ROS dependencies.
|
||||
|
||||
|
||||
import os.path
|
||||
import sys
|
||||
import os
|
||||
import getopt
|
||||
import subprocess
|
||||
from xml.dom.minidom import parse, parseString
|
||||
import xml.dom
|
||||
import re
|
||||
import string
|
||||
|
||||
|
||||
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 = attrs.keys()
|
||||
# a_names.sort()
|
||||
sorted(a_names)
|
||||
|
||||
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:
|
||||
if node.nodeType is not xml.dom.minidom.Node.TEXT_NODE: # 3:
|
||||
node.writexml(writer, indent+addindent, addindent, newl)
|
||||
# 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_elements(elt):
|
||||
c = elt.firstChild
|
||||
while c:
|
||||
if c.nodeType == xml.dom.Node.ELEMENT_NODE:
|
||||
yield c
|
||||
c = c.nextSibling
|
||||
|
||||
|
||||
all_includes = []
|
||||
# @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:
|
||||
if elt.tagName == 'include' or elt.tagName == 'xacro:include':
|
||||
# print("elt.getAttribute('filename'):", elt.getAttribute('filename'))
|
||||
filename = eval_text(elt.getAttribute('filename'), {})
|
||||
# print("filename:",filename)
|
||||
if not os.path.isabs(filename):
|
||||
filename = os.path.join(base_dir, filename)
|
||||
f = None
|
||||
try:
|
||||
try:
|
||||
f = open(filename)
|
||||
except IOError as e:
|
||||
print(elt)
|
||||
raise XacroException(
|
||||
"included file \"%s\" could not be opened: %s" % (filename, str(e)))
|
||||
try:
|
||||
global all_includes
|
||||
all_includes.append(filename)
|
||||
included = parse(f)
|
||||
except Exception as e:
|
||||
raise XacroException(
|
||||
"included file [%s] generated an error during XML parsing: %s" % (filename, str(e)))
|
||||
finally:
|
||||
if f:
|
||||
f.close()
|
||||
|
||||
# Replaces the include tag with the elements of the included file
|
||||
for c in child_elements(included.documentElement):
|
||||
elt.parentNode.insertBefore(c.cloneNode(1), elt)
|
||||
elt.parentNode.removeChild(elt)
|
||||
elt = None
|
||||
|
||||
# Grabs all the declared namespaces of the included document
|
||||
for name, value in included.documentElement.attributes.items():
|
||||
if name.startswith('xmlns:'):
|
||||
namespaces[name] = value
|
||||
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:
|
||||
value = symbols[lex.next()[1]]
|
||||
except KeyError as ex:
|
||||
# sys.stderr.write("Could not find symbol: %s\n" % str(ex))
|
||||
raise XacroException("Property wasn't defined: %s" % str(ex))
|
||||
if not (isnumber(value) or isinstance(value, (str, str))):
|
||||
print([value], isinstance(value, str), type(value))
|
||||
raise XacroException("WTF2")
|
||||
try:
|
||||
return int(value)
|
||||
except:
|
||||
try:
|
||||
return float(value)
|
||||
except:
|
||||
return value
|
||||
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_extension(s):
|
||||
# if s == '$(cwd)':
|
||||
return os.getcwd()
|
||||
# try:
|
||||
# return substitution_args.resolve_args(s, context=substitution_args_context, resolve_anon=False)
|
||||
# except substitution_args.ArgException as e:
|
||||
# raise XacroException("Undefined substitution argument", exc=e)
|
||||
# except ResourceNotFound as e:
|
||||
# raise XacroException("resource not found:", exc=e)
|
||||
|
||||
|
||||
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):
|
||||
# print("handle_extension", s)
|
||||
return eval_extension("$(%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]))
|
||||
# print("1:", results)
|
||||
elif lex.peek()[0] == lex.EXTENSION:
|
||||
results.append(handle_extension(lex.next()[1][2:-1]))
|
||||
# print("2:",results)
|
||||
elif lex.peek()[0] == lex.TEXT:
|
||||
results.append(lex.next()[1])
|
||||
# print("3:",results)
|
||||
elif lex.peek()[0] == lex.DOLLAR_DOLLAR_BRACE:
|
||||
results.append(lex.next()[1][1:])
|
||||
# print("4:",results)
|
||||
# print(results)
|
||||
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()
|
||||
|
||||
# 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
|
||||
|
||||
if params:
|
||||
raise XacroException("Some parameters were not set for macro %s" %
|
||||
str(node.tagName))
|
||||
eval_all(body, macros, scoped)
|
||||
|
||||
# Replaces the macro node with the expansion
|
||||
for e in list(child_elements(body)): # Ew
|
||||
node.parentNode.insertBefore(e, node)
|
||||
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_elements(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
|
||||
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 main():
|
||||
# print("dir:",os.path.dirname(sys.argv[2]))
|
||||
# sys.exit(0)
|
||||
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 = sys.stdout
|
||||
for o, a in opts:
|
||||
if o == '-h':
|
||||
print_usage(0)
|
||||
elif o == '-o':
|
||||
output = open(a, 'w')
|
||||
elif o == '--deps':
|
||||
just_deps = True
|
||||
elif o == '--includes':
|
||||
just_includes = True
|
||||
|
||||
if len(args) < 1:
|
||||
print("No input given")
|
||||
print_usage(2)
|
||||
|
||||
f = open(args[0])
|
||||
# print(args[0])
|
||||
# sys.exit(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()
|
||||
|
||||
# print(type(doc))
|
||||
# print(opts, args)
|
||||
# print("dir:",os.path.dirname(sys.argv[2]))
|
||||
# sys.exit(0)
|
||||
process_includes(doc, os.path.dirname(sys.argv[2]))
|
||||
if just_deps:
|
||||
for inc in all_includes:
|
||||
sys.stdout.write(inc + " ")
|
||||
sys.stdout.write("\n")
|
||||
elif just_includes:
|
||||
doc.writexml(output)
|
||||
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)
|
||||
|
||||
output.write(doc.toprettyxml(indent=' '))
|
||||
# doc.writexml(output, newl = "\n")
|
||||
print()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user