From 97bc2f326a83d06e00f015d1d06e18543909d8e9 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Tue, 20 Nov 2018 02:31:09 -0500 Subject: [PATCH] urdf, sdf: Use sealed XML reflection bits Clean up tests. --- src/urdf_parser_py/sdf.py | 82 +++++----- src/urdf_parser_py/urdf.py | 322 ++++++++++++++++++------------------- test/test_base.py | 17 ++ test/test_urdf.py | 54 +++++-- test/test_urdf_error.py | 23 ++- 5 files changed, 277 insertions(+), 221 deletions(-) create mode 100644 test/test_base.py diff --git a/src/urdf_parser_py/sdf.py b/src/urdf_parser_py/sdf.py index 67d25bd..9c9b5e9 100644 --- a/src/urdf_parser_py/sdf.py +++ b/src/urdf_parser_py/sdf.py @@ -1,12 +1,10 @@ -from urdf_parser_py.xml_reflection.basics import * -import urdf_parser_py.xml_reflection as xmlr +from urdf_parser_py import _now_private_property +import urdf_parser_py._xml_reflection as _xmlr -# What is the scope of plugins? Model, World, Sensor? +_xmlr.start_namespace('sdf') -xmlr.start_namespace('sdf') - -class Pose(xmlr.Object): +class Pose(_xmlr.Object): def __init__(self, vec=None, extra=None): self.xyz = None self.rpy = None @@ -32,37 +30,37 @@ def as_vec(self): rpy = self.rpy if self.rpy else [0, 0, 0] return xyz + rpy - def read_xml(self, node): + def _read_xml(self, node): # Better way to do this? Define type? - vec = get_type('vector6').read_xml(node) - self.load_vec(vec) + vec = _xmlr.get_type('vector6').read_xml_value(node) + self.from_vec(vec) - def write_xml(self, node): + def _write_xml(self, node): vec = self.as_vec() - get_type('vector6').write_xml(node, vec) + _xmlr.get_type('vector6').write_xml_value(node, vec) - def check_valid(self): + def _check_valid(self): assert self.xyz is not None or self.rpy is not None -name_attribute = xmlr.Attribute('name', str) -pose_element = xmlr.Element('pose', Pose, False) +_name_attribute = _xmlr.Attribute('name', str) +_pose_element = _xmlr.Element('pose', Pose, required=False) -class Entity(xmlr.Object): +class Entity(_xmlr.Object): def __init__(self, name=None, pose=None): self.name = name self.pose = pose -xmlr.reflect(Entity, params=[ - name_attribute, - pose_element +_xmlr.reflect(Entity, params=[ + _name_attribute, + _pose_element ]) -class Inertia(xmlr.Object): - KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'] +class Inertia(_xmlr.Object): + _KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'] def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0): self.ixx = ixx @@ -79,24 +77,21 @@ def to_matrix(self): [self.ixz, self.iyz, self.izz]] -xmlr.reflect(Inertia, - params=[xmlr.Element(key, float) for key in Inertia.KEYS]) - -# Pretty much copy-paste... Better method? -# Use multiple inheritance to separate the objects out so they are unique? +_xmlr.reflect(Inertia, tag='inertia', + params=[_xmlr.Element(key, float) for key in Inertia._KEYS]) -class Inertial(xmlr.Object): +class Inertial(_xmlr.Object): def __init__(self, mass=0.0, inertia=None, pose=None): self.mass = mass self.inertia = inertia self.pose = pose -xmlr.reflect(Inertial, params=[ - xmlr.Element('mass', float), - xmlr.Element('inertia', Inertia), - pose_element +_xmlr.reflect(Inertial, tag='inertial', params=[ + _xmlr.Element('mass', float), + _xmlr.Element('inertia', Inertia), + _pose_element ]) @@ -107,14 +102,21 @@ def __init__(self, name=None, pose=None, inertial=None, kinematic=False): self.kinematic = kinematic -xmlr.reflect(Link, parent_cls=Entity, params=[ - xmlr.Element('inertial', Inertial), - xmlr.Attribute('kinematic', bool, False), - xmlr.AggregateElement('visual', Visual, var='visuals'), - xmlr.AggregateElement('collision', Collision, var='collisions') +_xmlr.reflect(Link, tag='link', parent_cls=Entity, params=[ + _xmlr.Element('inertial', Inertial), + _xmlr.Attribute('kinematic', bool, False), + _xmlr.AggregateElement('visual', Visual, var='visuals'), + _xmlr.AggregateElement('collision', Collision, var='collisions') ]) +class Joint(Entity): + pass + + +_xmlr.reflect(Joint, tag='joint', parent_cls=Entity, params=[]) + + class Model(Entity): def __init__(self, name=None, pose=None): Entity.__init__(self, name, pose) @@ -123,10 +125,10 @@ def __init__(self, name=None, pose=None): self.plugins = [] -xmlr.reflect(Model, parent_cls=Entity, params=[ - xmlr.AggregateElement('link', Link, var='links'), - xmlr.AggregateElement('joint', Joint, var='joints'), - xmlr.AggregateElement('plugin', Plugin, var='plugins') +_xmlr.reflect(Model, parent_cls=Entity, params=[ + _xmlr.AggregateElement('link', Link, var='links'), + _xmlr.AggregateElement('joint', Joint, var='joints'), + _xmlr.AggregateElement('plugin', Plugin, var='plugins') ]) -xmlr.end_namespace('sdf') +_xmlr.end_namespace('sdf') diff --git a/src/urdf_parser_py/urdf.py b/src/urdf_parser_py/urdf.py index 2b08437..642bc61 100644 --- a/src/urdf_parser_py/urdf.py +++ b/src/urdf_parser_py/urdf.py @@ -1,26 +1,20 @@ -from urdf_parser_py.xml_reflection.basics import * -import urdf_parser_py.xml_reflection as xmlr +from urdf_parser_py import _now_private_property +import urdf_parser_py._xml_reflection as _xmlr -# Add a 'namespace' for names to avoid a conflict between URDF and SDF? -# A type registry? How to scope that? Just make a 'global' type pointer? -# Or just qualify names? urdf.geometric, sdf.geometric +_xmlr.start_namespace('urdf') -xmlr.start_namespace('urdf') +_xmlr.add_type('element_link', _xmlr.SimpleElementType('link', str)) +_xmlr.add_type('element_xyz', _xmlr.SimpleElementType('xyz', 'vector3')) -xmlr.add_type('element_link', xmlr.SimpleElementType('link', str)) -xmlr.add_type('element_xyz', xmlr.SimpleElementType('xyz', 'vector3')) -verbose = True - - -class Pose(xmlr.Object): +class Pose(_xmlr.Object): def __init__(self, xyz=None, rpy=None): self.xyz = xyz self.rpy = rpy - def check_valid(self): - assert (self.xyz is None or len(self.xyz) == 3) and \ - (self.rpy is None or len(self.rpy) == 3) + def _check_valid(self): + assert (self.xyz is None or len(self.xyz) == 3 and + self.rpy is None or len(self.rpy) == 3) # Aliases for backwards compatibility @property @@ -36,18 +30,18 @@ def position(self): return self.xyz def position(self, value): self.xyz = value -xmlr.reflect(Pose, tag='origin', params=[ - xmlr.Attribute('xyz', 'vector3', False, default=[0, 0, 0]), - xmlr.Attribute('rpy', 'vector3', False, default=[0, 0, 0]) +_xmlr.reflect(Pose, tag='origin', params=[ + _xmlr.Attribute('xyz', 'vector3', required=False, default=[0, 0, 0]), + _xmlr.Attribute('rpy', 'vector3', required=False, default=[0, 0, 0]) ]) # Common stuff -name_attribute = xmlr.Attribute('name', str) -origin_element = xmlr.Element('origin', Pose, False) +_name_attribute = _xmlr.Attribute('name', str) +_origin_element = _xmlr.Element('origin', Pose, required=False) -class Color(xmlr.Object): +class Color(_xmlr.Object): def __init__(self, *args): # What about named colors? count = len(args) @@ -64,150 +58,154 @@ def __init__(self, *args): raise Exception('Invalid color argument count') -xmlr.reflect(Color, tag='color', params=[ - xmlr.Attribute('rgba', 'vector4') +_xmlr.reflect(Color, tag='color', params=[ + _xmlr.Attribute('rgba', 'vector4') ]) -class JointDynamics(xmlr.Object): +class JointDynamics(_xmlr.Object): def __init__(self, damping=None, friction=None): self.damping = damping self.friction = friction -xmlr.reflect(JointDynamics, tag='dynamics', params=[ - xmlr.Attribute('damping', float, False), - xmlr.Attribute('friction', float, False) +_xmlr.reflect(JointDynamics, tag='dynamics', params=[ + _xmlr.Attribute('damping', float, required=False), + _xmlr.Attribute('friction', float, required=False) ]) -class Box(xmlr.Object): +class Box(_xmlr.Object): def __init__(self, size=None): self.size = size -xmlr.reflect(Box, tag='box', params=[ - xmlr.Attribute('size', 'vector3') +_xmlr.reflect(Box, tag='box', params=[ + _xmlr.Attribute('size', 'vector3') ]) -class Cylinder(xmlr.Object): +class Cylinder(_xmlr.Object): def __init__(self, radius=0.0, length=0.0): self.radius = radius self.length = length -xmlr.reflect(Cylinder, tag='cylinder', params=[ - xmlr.Attribute('radius', float), - xmlr.Attribute('length', float) +_xmlr.reflect(Cylinder, tag='cylinder', params=[ + _xmlr.Attribute('radius', float), + _xmlr.Attribute('length', float) ]) -class Sphere(xmlr.Object): +class Sphere(_xmlr.Object): def __init__(self, radius=0.0): self.radius = radius -xmlr.reflect(Sphere, tag='sphere', params=[ - xmlr.Attribute('radius', float) +_xmlr.reflect(Sphere, tag='sphere', params=[ + _xmlr.Attribute('radius', float) ]) -class Mesh(xmlr.Object): +class Mesh(_xmlr.Object): def __init__(self, filename=None, scale=None): self.filename = filename self.scale = scale -xmlr.reflect(Mesh, tag='mesh', params=[ - xmlr.Attribute('filename', str), - xmlr.Attribute('scale', 'vector3', required=False) +_xmlr.reflect(Mesh, tag='mesh', params=[ + _xmlr.Attribute('filename', str), + _xmlr.Attribute('scale', 'vector3', required=False) ]) -class GeometricType(xmlr.ValueType): +class _GeometricType(_xmlr.ValueType): def __init__(self): - self.factory = xmlr.FactoryType('geometric', { + self.factory = _xmlr.FactoryType('geometric', { 'box': Box, 'cylinder': Cylinder, 'sphere': Sphere, 'mesh': Mesh }) - def from_xml(self, node, path): - children = xml_children(node) + def read_xml_value(self, node, path): + children = _xmlr.xml_children(node) assert len(children) == 1, 'One element only for geometric' - return self.factory.from_xml(children[0], path=path) + return self.factory.read_xml_value(children[0], path=path) - def write_xml(self, node, obj): + def write_xml_value(self, node, obj): name = self.factory.get_name(obj) - child = node_add(node, name) - obj.write_xml(child) + child = _xmlr.node_add(node, name) + obj._write_xml(child) + + +# TODO(eacousineau): Deprecate public access. +GeometricType = _GeometricType -xmlr.add_type('geometric', GeometricType()) +_xmlr.add_type('geometric', _GeometricType()) -class Collision(xmlr.Object): +class Collision(_xmlr.Object): def __init__(self, geometry=None, origin=None): self.geometry = geometry self.origin = origin -xmlr.reflect(Collision, tag='collision', params=[ - origin_element, - xmlr.Element('geometry', 'geometric') +_xmlr.reflect(Collision, tag='collision', params=[ + _origin_element, + _xmlr.Element('geometry', 'geometric') ]) -class Texture(xmlr.Object): +class Texture(_xmlr.Object): def __init__(self, filename=None): self.filename = filename -xmlr.reflect(Texture, tag='texture', params=[ - xmlr.Attribute('filename', str) +_xmlr.reflect(Texture, tag='texture', params=[ + _xmlr.Attribute('filename', str) ]) -class Material(xmlr.Object): +class Material(_xmlr.Object): def __init__(self, name=None, color=None, texture=None): self.name = name self.color = color self.texture = texture - def check_valid(self): + def _check_valid(self): if self.color is None and self.texture is None: - xmlr.on_error("Material has neither a color nor texture.") + _xmlr.on_error("Material has neither a color nor texture.") -xmlr.reflect(Material, tag='material', params=[ - name_attribute, - xmlr.Element('color', Color, False), - xmlr.Element('texture', Texture, False) +_xmlr.reflect(Material, tag='material', params=[ + _name_attribute, + _xmlr.Element('color', Color, False), + _xmlr.Element('texture', Texture, False) ]) class LinkMaterial(Material): - def check_valid(self): + def _check_valid(self): pass -class Visual(xmlr.Object): +class Visual(_xmlr.Object): def __init__(self, geometry=None, material=None, origin=None): self.geometry = geometry self.material = material self.origin = origin -xmlr.reflect(Visual, tag='visual', params=[ - origin_element, - xmlr.Element('geometry', 'geometric'), - xmlr.Element('material', LinkMaterial, False) +_xmlr.reflect(Visual, tag='visual', params=[ + _origin_element, + _xmlr.Element('geometry', 'geometric'), + _xmlr.Element('material', LinkMaterial, False) ]) -class Inertia(xmlr.Object): +class Inertia(_xmlr.Object): KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'] def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0): @@ -225,38 +223,38 @@ def to_matrix(self): [self.ixz, self.iyz, self.izz]] -xmlr.reflect(Inertia, tag='inertia', - params=[xmlr.Attribute(key, float) for key in Inertia.KEYS]) +_xmlr.reflect(Inertia, tag='inertia', + params=[_xmlr.Attribute(key, float) for key in Inertia.KEYS]) -class Inertial(xmlr.Object): +class Inertial(_xmlr.Object): def __init__(self, mass=0.0, inertia=None, origin=None): self.mass = mass self.inertia = inertia self.origin = origin -xmlr.reflect(Inertial, tag='inertial', params=[ - origin_element, - xmlr.Element('mass', 'element_value'), - xmlr.Element('inertia', Inertia, False) +_xmlr.reflect(Inertial, tag='inertial', params=[ + _origin_element, + _xmlr.Element('mass', 'element_value'), + _xmlr.Element('inertia', Inertia, required=False) ]) # FIXME: we are missing the reference position here. -class JointCalibration(xmlr.Object): +class JointCalibration(_xmlr.Object): def __init__(self, rising=None, falling=None): self.rising = rising self.falling = falling -xmlr.reflect(JointCalibration, tag='calibration', params=[ - xmlr.Attribute('rising', float, False, 0), - xmlr.Attribute('falling', float, False, 0) +_xmlr.reflect(JointCalibration, tag='calibration', params=[ + _xmlr.Attribute('rising', float, required=False, default=0), + _xmlr.Attribute('falling', float, required=False, default=0) ]) -class JointLimit(xmlr.Object): +class JointLimit(_xmlr.Object): def __init__(self, effort=None, velocity=None, lower=None, upper=None): self.effort = effort self.velocity = velocity @@ -264,31 +262,31 @@ def __init__(self, effort=None, velocity=None, lower=None, upper=None): self.upper = upper -xmlr.reflect(JointLimit, tag='limit', params=[ - xmlr.Attribute('effort', float), - xmlr.Attribute('lower', float, False, 0), - xmlr.Attribute('upper', float, False, 0), - xmlr.Attribute('velocity', float) +_xmlr.reflect(JointLimit, tag='limit', params=[ + _xmlr.Attribute('effort', float), + _xmlr.Attribute('lower', float, required=False, default=0), + _xmlr.Attribute('upper', float, required=False, default=0), + _xmlr.Attribute('velocity', float) ]) # FIXME: we are missing __str__ here. -class JointMimic(xmlr.Object): +class JointMimic(_xmlr.Object): def __init__(self, joint_name=None, multiplier=None, offset=None): self.joint = joint_name self.multiplier = multiplier self.offset = offset -xmlr.reflect(JointMimic, tag='mimic', params=[ - xmlr.Attribute('joint', str), - xmlr.Attribute('multiplier', float, False), - xmlr.Attribute('offset', float, False) +_xmlr.reflect(JointMimic, tag='mimic', params=[ + _xmlr.Attribute('joint', str), + _xmlr.Attribute('multiplier', float, required=False), + _xmlr.Attribute('offset', float, required=False) ]) -class SafetyController(xmlr.Object): +class SafetyController(_xmlr.Object): def __init__(self, velocity=None, position=None, lower=None, upper=None): self.k_velocity = velocity self.k_position = position @@ -296,15 +294,15 @@ def __init__(self, velocity=None, position=None, lower=None, upper=None): self.soft_upper_limit = upper -xmlr.reflect(SafetyController, tag='safety_controller', params=[ - xmlr.Attribute('k_velocity', float), - xmlr.Attribute('k_position', float, False, 0), - xmlr.Attribute('soft_lower_limit', float, False, 0), - xmlr.Attribute('soft_upper_limit', float, False, 0) +_xmlr.reflect(SafetyController, tag='safety_controller', params=[ + _xmlr.Attribute('k_velocity', float), + _xmlr.Attribute('k_position', float, required=False, default=0), + _xmlr.Attribute('soft_lower_limit', float, required=False, default=0), + _xmlr.Attribute('soft_upper_limit', float, required=False, default=0) ]) -class Joint(xmlr.Object): +class Joint(_xmlr.Object): TYPES = ['unknown', 'revolute', 'continuous', 'prismatic', 'floating', 'planar', 'fixed'] @@ -324,7 +322,7 @@ def __init__(self, name=None, parent=None, child=None, joint_type=None, self.calibration = calibration self.mimic = mimic - def check_valid(self): + def _check_valid(self): assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type) # noqa # Aliases @@ -334,25 +332,25 @@ def joint_type(self): return self.type @joint_type.setter def joint_type(self, value): self.type = value -xmlr.reflect(Joint, tag='joint', params=[ - name_attribute, - xmlr.Attribute('type', str), - origin_element, - xmlr.Element('axis', 'element_xyz', False), - xmlr.Element('parent', 'element_link'), - xmlr.Element('child', 'element_link'), - xmlr.Element('limit', JointLimit, False), - xmlr.Element('dynamics', JointDynamics, False), - xmlr.Element('safety_controller', SafetyController, False), - xmlr.Element('calibration', JointCalibration, False), - xmlr.Element('mimic', JointMimic, False), +_xmlr.reflect(Joint, tag='joint', params=[ + _name_attribute, + _xmlr.Attribute('type', str), + _origin_element, + _xmlr.Element('axis', 'element_xyz', required=False), + _xmlr.Element('parent', 'element_link'), + _xmlr.Element('child', 'element_link'), + _xmlr.Element('limit', JointLimit, required=False), + _xmlr.Element('dynamics', JointDynamics, required=False), + _xmlr.Element('safety_controller', SafetyController, required=False), + _xmlr.Element('calibration', JointCalibration, required=False), + _xmlr.Element('mimic', JointMimic, required=False), ]) -class Link(xmlr.Object): +class Link(_xmlr.Object): def __init__(self, name=None, visual=None, inertial=None, collision=None, origin=None): - self.aggregate_init() + self._aggregate_init() self.name = name self.visuals = [] self.inertial = inertial @@ -388,16 +386,16 @@ def __set_collision(self, collision): collision = property(__get_collision, __set_collision) -xmlr.reflect(Link, tag='link', params=[ - name_attribute, - origin_element, - xmlr.AggregateElement('visual', Visual), - xmlr.AggregateElement('collision', Collision), - xmlr.Element('inertial', Inertial, False), +_xmlr.reflect(Link, tag='link', params=[ + _name_attribute, + _origin_element, + _xmlr.AggregateElement('visual', Visual), + _xmlr.AggregateElement('collision', Collision), + _xmlr.Element('inertial', Inertial, required=False), ]) -class PR2Transmission(xmlr.Object): +class PR2Transmission(_xmlr.Object): def __init__(self, name=None, joint=None, actuator=None, type=None, mechanicalReduction=1): self.name = name @@ -407,72 +405,72 @@ def __init__(self, name=None, joint=None, actuator=None, type=None, self.mechanicalReduction = mechanicalReduction -xmlr.reflect(PR2Transmission, tag='pr2_transmission', params=[ - name_attribute, - xmlr.Attribute('type', str), - xmlr.Element('joint', 'element_name'), - xmlr.Element('actuator', 'element_name'), - xmlr.Element('mechanicalReduction', float) +_xmlr.reflect(PR2Transmission, tag='pr2_transmission', params=[ + _name_attribute, + _xmlr.Attribute('type', str), + _xmlr.Element('joint', 'element_name'), + _xmlr.Element('actuator', 'element_name'), + _xmlr.Element('mechanicalReduction', float) ]) -class Actuator(xmlr.Object): +class Actuator(_xmlr.Object): def __init__(self, name=None, mechanicalReduction=1): self.name = name self.mechanicalReduction = None -xmlr.reflect(Actuator, tag='actuator', params=[ - name_attribute, - xmlr.Element('mechanicalReduction', float, required=False) +_xmlr.reflect(Actuator, tag='actuator', params=[ + _name_attribute, + _xmlr.Element('mechanicalReduction', float, required=False) ]) -class TransmissionJoint(xmlr.Object): +class TransmissionJoint(_xmlr.Object): def __init__(self, name=None): - self.aggregate_init() + self._aggregate_init() self.name = name self.hardwareInterfaces = [] - def check_valid(self): + def _check_valid(self): assert len(self.hardwareInterfaces) > 0, "no hardwareInterface defined" -xmlr.reflect(TransmissionJoint, tag='joint', params=[ - name_attribute, - xmlr.AggregateElement('hardwareInterface', str), +_xmlr.reflect(TransmissionJoint, tag='joint', params=[ + _name_attribute, + _xmlr.AggregateElement('hardwareInterface', str), ]) -class Transmission(xmlr.Object): +class Transmission(_xmlr.Object): """ New format: http://wiki.ros.org/urdf/XML/Transmission """ def __init__(self, name=None): - self.aggregate_init() + self._aggregate_init() self.name = name self.joints = [] self.actuators = [] - def check_valid(self): + def _check_valid(self): assert len(self.joints) > 0, "no joint defined" assert len(self.actuators) > 0, "no actuator defined" -xmlr.reflect(Transmission, tag='new_transmission', params=[ - name_attribute, - xmlr.Element('type', str), - xmlr.AggregateElement('joint', TransmissionJoint), - xmlr.AggregateElement('actuator', Actuator) +_xmlr.reflect(Transmission, tag='new_transmission', params=[ + _name_attribute, + _xmlr.Element('type', str), + _xmlr.AggregateElement('joint', TransmissionJoint), + _xmlr.AggregateElement('actuator', Actuator) ]) -xmlr.add_type('transmission', - xmlr.DuckTypedFactory('transmission', +_xmlr.add_type('transmission', + _xmlr.DuckTypedFactory('transmission', [Transmission, PR2Transmission])) -class Robot(xmlr.Object): +class Robot(_xmlr.Object): def __init__(self, name=None): - self.aggregate_init() + self._aggregate_init() self.name = name self.joints = [] @@ -487,8 +485,8 @@ def __init__(self, name=None): self.parent_map = {} self.child_map = {} - def add_aggregate(self, typeName, elem): - xmlr.Object.add_aggregate(self, typeName, elem) + def _add_aggregate(self, typeName, elem): + _xmlr.Object._add_aggregate(self, typeName, elem) if typeName == 'joint': joint = elem @@ -503,10 +501,10 @@ def add_aggregate(self, typeName, elem): self.link_map[link.name] = link def add_link(self, link): - self.add_aggregate('link', link) + self._add_aggregate('link', link) def add_joint(self, joint): - self.add_aggregate('joint', joint) + self._add_aggregate('joint', joint) def get_chain(self, root, tip, joints=True, links=True, fixed=True): chain = [] @@ -546,16 +544,16 @@ def from_parameter_server(cls, key='robot_description'): return cls.from_xml_string(rospy.get_param(key)) -xmlr.reflect(Robot, tag='robot', params=[ - xmlr.Attribute('name', str, False), # Is 'name' a required attribute? - xmlr.AggregateElement('link', Link), - xmlr.AggregateElement('joint', Joint), - xmlr.AggregateElement('gazebo', xmlr.RawType()), - xmlr.AggregateElement('transmission', 'transmission'), - xmlr.AggregateElement('material', Material) +_xmlr.reflect(Robot, tag='robot', params=[ + _xmlr.Attribute('name', str, required=False), # Is 'name' a required attribute? + _xmlr.AggregateElement('link', Link), + _xmlr.AggregateElement('joint', Joint), + _xmlr.AggregateElement('gazebo', _xmlr.RawType()), + _xmlr.AggregateElement('transmission', 'transmission'), + _xmlr.AggregateElement('material', Material) ]) # Make an alias URDF = Robot -xmlr.end_namespace() +_xmlr.end_namespace() diff --git a/test/test_base.py b/test/test_base.py new file mode 100644 index 0000000..9a092f3 --- /dev/null +++ b/test/test_base.py @@ -0,0 +1,17 @@ +from contextlib import contextmanager +import unittest +import warnings + + +class TestBase(unittest.TestCase): + def setUp(self): + # Ensure that all deprecations are seen as errors. + warnings.simplefilter('error', DeprecationWarning) + + @contextmanager + def catch_warnings(self): + """Wraps nominal catch warnings to reset filters.""" + with warnings.catch_warnings(record=True) as w: + # Do not error; instead, only warn once. + warnings.simplefilter('once', DeprecationWarning) + yield w diff --git a/test/test_urdf.py b/test/test_urdf.py index 8dce198..8dc5b41 100644 --- a/test/test_urdf.py +++ b/test/test_urdf.py @@ -1,27 +1,32 @@ from __future__ import print_function +from os.path import abspath, dirname, join import unittest -import mock -import os import sys +import warnings +from xml.dom import minidom # noqa + +import mock # Add path to import xml_matching -sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '.'))) -sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), - '../src'))) +# TODO(eacousineau): Can CTest somehow provide this? +TEST_DIR = dirname(abspath(__file__)) +sys.path.append(TEST_DIR) +sys.path.append(join(dirname(TEST_DIR), 'src')) -from xml.dom import minidom # noqa -from xml_matching import xml_matches # noqa from urdf_parser_py import urdf # noqa -import urdf_parser_py.xml_reflection as xmlr +import urdf_parser_py._xml_reflection as _xmlr +from xml_matching import xml_matches # noqa +from test_base import TestBase -class ParseException(xmlr.core.ParseError): + +class ParseException(_xmlr.ParseError): def __init__(self, e = "", path = ""): super(ParseException, self).__init__(e, path) -class TestURDFParser(unittest.TestCase): - @mock.patch('urdf_parser_py.xml_reflection.on_error', +class TestURDFParser(TestBase): + @mock.patch('urdf_parser_py._xml_reflection.on_error', mock.Mock(side_effect=ParseException)) def parse(self, xml): return urdf.Robot.from_xml_string(xml) @@ -183,8 +188,8 @@ def test_link_multiple_collision(self): self.parse_and_compare(xml) -class LinkOriginTestCase(unittest.TestCase): - @mock.patch('urdf_parser_py.xml_reflection.on_error', +class LinkOriginTestCase(TestBase): + @mock.patch('urdf_parser_py._xml_reflection.on_error', mock.Mock(side_effect=ParseException)) def parse(self, xml): return urdf.Robot.from_xml_string(xml) @@ -220,7 +225,7 @@ def test_robot_link_defaults_xyz_set(self): self.assertEquals(origin.rpy, [0, 0, 0]) -class LinkMultiVisualsAndCollisionsTest(unittest.TestCase): +class LinkMultiVisualsAndCollisionsTest(TestBase): xml = ''' @@ -276,5 +281,26 @@ def test_multi_collision_access(self): self.assertEquals(id(dummyObject), id(robot.links[0].collisions[0])) +class TestDeprecation(TestBase): + """Tests deprecated interfaces.""" + def test_deprecated_properties(self): + with self.catch_warnings() as w: + urdf.Robot.XML_REFL + urdf.Pose().check_valid() + self.assertEqual(len(w), 2) + self.assertIn("'XML_REFL'", str(w[0].message)) + self.assertIn("'check_valid'", str(w[1].message)) + + +class TestExampleRobots(TestBase): + """Tests that some samples files can be parsed without error.""" + @unittest.skip("Badly formatted transmissions") + def test_calvin_urdf(self): + urdf.Robot.from_xml_file(join(TEST_DIR, 'calvin/calvin.urdf')) + + def test_romeo_urdf(self): + urdf.Robot.from_xml_file(join(TEST_DIR, 'romeo/romeo.urdf')) + + if __name__ == '__main__': unittest.main() diff --git a/test/test_urdf_error.py b/test/test_urdf_error.py index 5cd61ee..06da8c0 100644 --- a/test/test_urdf_error.py +++ b/test/test_urdf_error.py @@ -1,21 +1,33 @@ from __future__ import print_function +from os.path import abspath, dirname, join +import sys import unittest +import warnings + +# TODO(eacousineau): Can CTest somehow provide this? +TEST_DIR = dirname(abspath(__file__)) +sys.path.append(TEST_DIR) +sys.path.append(join(dirname(TEST_DIR), 'src')) + from urdf_parser_py import urdf -import urdf_parser_py.xml_reflection as xmlr +import urdf_parser_py._xml_reflection as _xmlr +from test_base import TestBase -ParseError = xmlr.core.ParseError +ParseError = _xmlr.ParseError -class TestURDFParserError(unittest.TestCase): + +class TestURDFParserError(TestBase): def setUp(self): + TestBase.setUp(self) # Manually patch "on_error" to capture errors self.errors = [] def add_error(message): self.errors.append(message) - xmlr.core.on_error = add_error + _xmlr.core.on_error = add_error def tearDown(self): - xmlr.core.on_error = xmlr.core.on_error_stderr + _xmlr.core.on_error = _xmlr.core.on_error_stderr def assertLoggedErrors(self, errors, func, *args, **kwds): func(*args, **kwds) @@ -146,5 +158,6 @@ def test_bad_ducktype(self): for func in funcs: self.assertParseErrorPath("/robot[@name='test']/transmission[@name='simple_trans_bad']", func) + if __name__ == '__main__': unittest.main()