From 07f5302d688668ac1906ce3841e45bf49599dafa Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Tue, 30 Apr 2019 18:42:28 +0900
Subject: [PATCH] add test to build robot model from python urdf classes

---
 test/test_urdf.py | 72 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 72 insertions(+)

diff --git a/test/test_urdf.py b/test/test_urdf.py
index 7ec81e5..af88953 100644
--- a/test/test_urdf.py
+++ b/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">
@@ -46,6 +52,18 @@ def test_new_transmission(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name = 'test')
+        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">
@@ -65,6 +83,22 @@ def test_new_transmission_multiple_joints(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name = 'test')
+        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">
@@ -81,6 +115,20 @@ def test_new_transmission_multiple_actuators(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name = 'test')
+        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">
@@ -113,6 +161,11 @@ def test_old_transmission(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name = 'test')
+        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">
@@ -127,6 +180,20 @@ def test_link_material_missing_color_and_texture(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name = 'test')
+        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')
+        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">
@@ -136,6 +203,11 @@ def test_robot_material(self):
 </robot>'''
         self.parse_and_compare(xml)
 
+        robot = urdf.Robot(name = 'test')
+        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">