update backward compatibility on visual and collisions (#47)

* add test to build robot model from python urdf classes

* add more tests on python urdf build

* update backward compatibility

Link(visual=...) does not set <visual> on <link> since 0.4.0, breaks backward compatibility

* Robot accepts version kwarg

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>
Co-authored-by: Shane Loretz <sloretz@osrfoundation.org>

Author: 2 people

src/urdf_parser_py/urdf.py

@@ -355,8 +355,12 @@ def __init__(self, name=None, visual=None, inertial=None, collision=None,
         self.aggregate_init()
         self.name = name
         self.visuals = []
+        if visual:
+            self.visual = visual
         self.inertial = inertial
         self.collisions = []
+        if collision:
+            self.collision = collision
         self.origin = origin
 
     def __get_visual(self):
@@ -370,6 +374,8 @@ def __set_visual(self, visual):
             self.visuals[0] = visual
         else:
             self.visuals.append(visual)
+        if visual:
+            self.add_aggregate('visual', visual)
 
     def __get_collision(self):
         """Return the first collision or None."""
@@ -382,6 +388,8 @@ def __set_collision(self, collision):
             self.collisions[0] = collision
         else:
             self.collisions.append(collision)
+        if collision:
+            self.add_aggregate('collision', collision)
 
     # Properties for backwards compatibility
     visual = property(__get_visual, __set_visual)

test/test_urdf.py

@@ -31,6 +31,12 @@ def parse_and_compare(self, orig):
         rewritten = minidom.parseString(robot.to_xml_string())
         self.assertTrue(xml_matches(xml, rewritten))
 
+    def xml_and_compare(self, robot, xml):
+        robot_xml_string = robot.to_xml_string()
+        robot_xml = minidom.parseString(robot_xml_string)
+        orig_xml = minidom.parseString(xml)
+        self.assertTrue(xml_matches(robot_xml, orig_xml))
+
     def test_new_transmission(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -46,6 +52,18 @@ def test_new_transmission(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        trans = urdf.Transmission(name='simple_trans')
+        trans.type = 'transmission_interface/SimpleTransmission'
+        joint = urdf.TransmissionJoint(name='foo_joint')
+        joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
+        trans.add_aggregate('joint', joint)
+        actuator = urdf.Actuator(name='foo_motor')
+        actuator.mechanicalReduction = 50.0
+        trans.add_aggregate('actuator', actuator)
+        robot.add_aggregate('transmission', trans)
+        self.xml_and_compare(robot, xml)
+
     def test_new_transmission_multiple_joints(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -65,6 +83,22 @@ def test_new_transmission_multiple_joints(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        trans = urdf.Transmission(name='simple_trans')
+        trans.type = 'transmission_interface/SimpleTransmission'
+        joint = urdf.TransmissionJoint(name='foo_joint')
+        joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
+        trans.add_aggregate('joint', joint)
+        joint = urdf.TransmissionJoint(name='bar_joint')
+        joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
+        joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
+        trans.add_aggregate('joint', joint)
+        actuator = urdf.Actuator(name='foo_motor')
+        actuator.mechanicalReduction = 50.0
+        trans.add_aggregate('actuator', actuator)
+        robot.add_aggregate('transmission', trans)
+        self.xml_and_compare(robot, xml)
+
     def test_new_transmission_multiple_actuators(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -81,6 +115,20 @@ def test_new_transmission_multiple_actuators(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        trans = urdf.Transmission(name='simple_trans')
+        trans.type = 'transmission_interface/SimpleTransmission'
+        joint = urdf.TransmissionJoint(name='foo_joint')
+        joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
+        trans.add_aggregate('joint', joint)
+        actuator = urdf.Actuator(name='foo_motor')
+        actuator.mechanicalReduction = 50.0
+        trans.add_aggregate('actuator', actuator)
+        actuator = urdf.Actuator(name='bar_motor')
+        trans.add_aggregate('actuator', actuator)
+        robot.add_aggregate('transmission', trans)
+        self.xml_and_compare(robot, xml)
+
     def test_new_transmission_missing_joint(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -113,6 +161,11 @@ def test_old_transmission(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        trans = urdf.PR2Transmission(name='PR2_trans', joint='foo_joint', actuator='foo_motor', type='SimpleTransmission', mechanicalReduction=1.0)
+        robot.add_aggregate('transmission', trans)
+        self.xml_and_compare(robot, xml)
+
     def test_link_material_missing_color_and_texture(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -127,6 +180,20 @@ def test_link_material_missing_color_and_texture(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        link = urdf.Link(name='link',
+                         visual=urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
+                                              material=urdf.Material(name='mat')))
+        robot.add_link(link)
+        self.xml_and_compare(robot, xml)
+
+        robot = urdf.Robot(name='test', version='1.0')
+        link = urdf.Link(name='link')
+        link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
+                                  material=urdf.Material(name='mat'))
+        robot.add_link(link)
+        self.xml_and_compare(robot, xml)
+
     def test_robot_material(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -136,6 +203,11 @@ def test_robot_material(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        material = urdf.Material(name='mat', color=urdf.Color([0.0, 0.0, 0.0, 1.0]))
+        robot.add_aggregate('material', material)
+        self.xml_and_compare(robot, xml)
+
     def test_robot_material_missing_color_and_texture(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -163,6 +235,29 @@ def test_link_multiple_visual(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        link = urdf.Link(name='link')
+        link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
+                                  material=urdf.Material(name='mat'))
+        link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
+                                  material=urdf.Material(name='mat2'))
+        robot.add_link(link)
+        self.xml_and_compare(robot, xml)
+
+    def test_visual_with_name(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="test" version="1.0">
+  <link name="link">
+    <visual name="alice">
+      <geometry>
+        <cylinder length="1" radius="1"/>
+      </geometry>
+      <material name="mat"/>
+    </visual>
+  </link>
+</robot>'''
+        self.parse_and_compare(xml)
+
     def test_link_multiple_collision(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1.0">
@@ -181,6 +276,13 @@ def test_link_multiple_collision(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name='test', version='1.0')
+        link = urdf.Link(name='link')
+        link.collision = urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1))
+        link.collision = urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5))
+        robot.add_link(link)
+        self.xml_and_compare(robot, xml)
+
     def test_version_attribute_not_enough_dots(self):
         xml = '''<?xml version="1.0"?>
 <robot name="test" version="1">
@@ -293,7 +395,7 @@ def test_robot_link_defaults_xyz_set(self):
 class LinkMultiVisualsAndCollisionsTest(unittest.TestCase):
 
     xml = '''<?xml version="1.0"?>
-<robot name="test">
+<robot name="test" version="1.0">
   <link name="link">
     <visual>
       <geometry>
@@ -345,6 +447,41 @@ def test_multi_collision_access(self):
         robot.links[0].collision = dummyObject
         self.assertEqual(id(dummyObject), id(robot.links[0].collisions[0]))
 
+    def test_xml_and_urdfdom_robot_compatible_with_kinetic(self):
+        robot = urdf.Robot(name='test', version='1.0')
+        link = urdf.Link(name='link')
+        link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
+                                  material=urdf.Material(name='mat'))
+        link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
+                                  material=urdf.Material(name='mat2'))
+        link.collision = urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1))
+        link.collision = urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5))
+        robot.add_link(link)
+        link = urdf.Link(name='link2')
+        robot.add_link(link)
+        #
+        robot_xml_string = robot.to_xml_string()
+        robot_xml = minidom.parseString(robot_xml_string)
+        orig_xml = minidom.parseString(self.xml)
+        self.assertTrue(xml_matches(robot_xml, orig_xml))
+
+    def test_xml_and_urdfdom_robot_only_supported_since_melodic(self):
+        robot = urdf.Robot(name='test', version='1.0')
+        link = urdf.Link(name='link')
+        link.add_aggregate('visual', urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
+                                                 material=urdf.Material(name='mat')))
+        link.add_aggregate('visual', urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
+                                                 material=urdf.Material(name='mat2')))
+        link.add_aggregate('collision', urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1)))
+        link.add_aggregate('collision', urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5)))
+        robot.add_link(link)
+        link = urdf.Link(name='link2')
+        robot.add_link(link)
+        #
+        robot_xml_string = robot.to_xml_string()
+        robot_xml = minidom.parseString(robot_xml_string)
+        orig_xml = minidom.parseString(self.xml)
+        self.assertTrue(xml_matches(robot_xml, orig_xml))
 
 class TestCreateNew(unittest.TestCase):
     def test_new_urdf(self):