test.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 PKG = 'srdfdom'
00003 
00004 import sys
00005 import rospkg
00006 import unittest
00007 from srdfdom.srdf import SRDF
00008 from xml.dom.minidom import parseString
00009 import xml.dom
00010 
00011 # xml match code from test_xacro.py  
00012 # by Stuart Glaser and William Woodall
00013 
00014 def first_child_element(elt):
00015   c = elt.firstChild
00016   while c:
00017     if c.nodeType == xml.dom.Node.ELEMENT_NODE:
00018       return c
00019     c = c.nextSibling
00020   return None
00021   
00022 def next_sibling_element(elt):
00023   c = elt.nextSibling
00024   while c:
00025     if c.nodeType == xml.dom.Node.ELEMENT_NODE:
00026       return c
00027     c = c.nextSibling
00028   return None
00029 
00030 def all_attributes_match(a, b):
00031   if len(a.attributes) != len(b.attributes):
00032     print("Different number of attributes")
00033     return False
00034   a_atts = [(a.attributes.item(i).name, a.attributes.item(i).value) for i in range(len(a.attributes))]
00035   b_atts = [(b.attributes.item(i).name, b.attributes.item(i).value) for i in range(len(b.attributes))]
00036   a_atts.sort()
00037   b_atts.sort()
00038   for i in range(len(a_atts)):
00039     if a_atts[i][0] != b_atts[i][0]:
00040       print("Different attribute names: %s and %s" % (a_atts[i][0], b_atts[i][0]))
00041       return False
00042     try:
00043       if abs(float(a_atts[i][1]) - float(b_atts[i][1])) > 1.0e-9:
00044         print("Different attribute values: %s and %s" % (a_atts[i][1], b_atts[i][1]))
00045         return False
00046     except ValueError: # Attribute values aren't numeric
00047       if a_atts[i][1] != b_atts[i][1]:
00048         print("Different attribute values: %s and %s" % (a_atts[i][1], b_atts[i][1]))
00049         return False
00050   return True
00051 
00052 def elements_match(a, b):
00053   if not a and not b:
00054     return True
00055   if not a or not b:
00056     return False
00057   if a.nodeType != b.nodeType:
00058     print("Different node types: %d and %d" % (a.nodeType, b.nodeType))
00059     return False
00060   if a.nodeName != b.nodeName:
00061     print("Different element names: %s and %s" % (a.nodeName, b.nodeName))
00062     return False
00063   if not all_attributes_match(a, b):
00064     return False
00065   if not elements_match(first_child_element(a), first_child_element(b)):
00066     return False
00067   if not elements_match(next_sibling_element(a), next_sibling_element(b)):
00068     return False
00069   return True
00070 
00071 def xml_matches(a, b):
00072   if isinstance(a, str):
00073     return xml_matches(parseString(a).documentElement, b)
00074   if isinstance(b, str):
00075     return xml_matches(a, parseString(b).documentElement)
00076   if a.nodeType == xml.dom.Node.DOCUMENT_NODE:
00077     return xml_matches(a.documentElement, b)
00078   if b.nodeType == xml.dom.Node.DOCUMENT_NODE:
00079     return xml_matches(a, b.documentElement)
00080   if not elements_match(a, b):
00081     print("Match failed:")
00082     a.writexml(sys.stdout)
00083     print
00084     print('=' * 78)
00085     b.writexml(sys.stdout)
00086     return False
00087   return True
00088   
00089 ## A python unit test for srdf
00090 class TestSRDFParser(unittest.TestCase):
00091     ## test valid srdf
00092 
00093   def test_full_srdf(self):
00094         srdf_data = '''
00095         <robot name="myrobot">
00096         <group name="body">
00097           <joint name="J1" />
00098           <joint name="J2" />
00099           <joint name="J3" />
00100           <chain base_link="robot_base" tip_link="robot_tip" />
00101           <group name="arm" />
00102         </group>
00103         <group_state name="zero" group="body">
00104         <joint name="J1" value="0" />
00105         <joint name="J2" value="0" />
00106         <joint name="J3" value="0" />
00107         </group_state>
00108         <end_effector name="tip_ee" parent_link="tip" group="arm" parent_group="body" />
00109         <end_effector name="othertip_ee" parent_link="othertip" group="arm" />
00110         <virtual_joint name="virtual_joint" type="floating" parent_frame="body_frame" child_link="arm" />
00111         <disable_collisions link1="link1" link2="link3" />
00112         <disable_collisions reason="Adjacent"  link1="link1" link2="link2" />
00113         <link_sphere_approximation link="link1" />
00114         <link_sphere_approximation link="link2" >
00115             <sphere center="1.0 2.0 3.0" radius="1.0" />
00116             <sphere center="1.0 2.0 4.0" radius="2.0" />
00117         </link_sphere_approximation>
00118         </robot>
00119         '''
00120         expected = '''
00121 <robot name="myrobot">
00122   <group name="body">
00123     <joint name="J1" />
00124     <joint name="J2" />
00125     <joint name="J3" />
00126     <chain base_link="robot_base" tip_link="robot_tip"/>
00127     <group name="arm" />
00128   </group>
00129   <group_state name="zero" group="body">
00130     <joint name="J1" value="0" />
00131     <joint name="J2" value="0" />
00132     <joint name="J3" value="0" />
00133   </group_state>
00134   <end_effector group="arm" name="tip_ee" parent_group="body" parent_link="tip"/>
00135   <end_effector name="othertip_ee" parent_link="othertip" group="arm" />
00136   <virtual_joint child_link="arm" name="virtual_joint" parent_frame="body_frame" type="floating"  />
00137   <disable_collisions link1="link1" link2="link3" />
00138   <disable_collisions link1="link1" link2="link2" reason="Adjacent" />
00139   <link_sphere_approximation link="link1" />
00140   <link_sphere_approximation link="link2" >
00141     <sphere center="1.0 2.0 3.0" radius="1.0" />
00142     <sphere center="1.0 2.0 4.0" radius="2.0" />
00143   </link_sphere_approximation>
00144 </robot>
00145         '''
00146         robot = SRDF.from_xml_string(srdf_data)
00147         self.assertTrue( xml_matches(robot.to_xml_string(),expected))
00148         
00149   def test_simple_srdf(self):
00150         datadir=rospkg.RosPack().get_path('srdfdom')+"/test/res/"
00151         stream = open(datadir+'pr2_desc.1.srdf', 'r')
00152         robot = SRDF.from_xml_string(stream.read())
00153         stream.close()
00154         self.assertTrue(len(robot.virtual_joints)==0)
00155         self.assertTrue(len(robot.groups)==0)
00156         self.assertTrue(len(robot.group_states)==0)
00157         self.assertTrue(len(robot.disable_collisionss)==0)
00158         self.assertTrue(len(robot.end_effectors)==0)
00159         
00160         stream = open(datadir+'pr2_desc.2.srdf', 'r')
00161         robot = SRDF.from_xml_string(stream.read())
00162         stream.close()
00163         self.assertTrue(len(robot.virtual_joints)==1)
00164         self.assertTrue(len(robot.groups)==1)
00165         self.assertTrue(len(robot.group_states)==0)
00166         self.assertTrue(len(robot.disable_collisionss)==0)
00167         self.assertTrue(len(robot.end_effectors)==0)
00168         
00169   def test_complex_srdf(self):
00170         datadir=rospkg.RosPack().get_path('srdfdom')+"/test/res/"
00171         stream = open(datadir+'pr2_desc.3.srdf', 'r')
00172         robot = SRDF.from_xml_string(stream.read())
00173         stream.close()
00174         self.assertTrue(len(robot.virtual_joints)==1)
00175         self.assertTrue(len(robot.groups)==7)
00176         self.assertTrue(len(robot.group_states)==2)
00177         self.assertTrue(len(robot.disable_collisionss)==2)
00178         self.assertTrue(robot.disable_collisionss[0].reason=="adjacent")
00179         self.assertTrue(len(robot.end_effectors)==2)
00180         
00181         self.assertTrue(robot.virtual_joints[0].name=="world_joint")
00182         self.assertTrue(robot.virtual_joints[0].type=="planar")
00183         
00184         for group in robot.groups:
00185           if (group.name == "left_arm" or group.name == "right_arm" ):
00186             self.assertTrue(len(group.chains)==1)
00187           if group.name == "arms":
00188             self.assertTrue(len(group.subgroups)==2)
00189           if group.name == "base":
00190             self.assertTrue(len(group.joints)==1)
00191           if (group.name == "l_end_effector" or group.name == "r_end_effector" ):
00192             self.assertTrue(len(group.links)==1)
00193             self.assertTrue(len(group.joints)==9)
00194           if group.name == "whole_body" :
00195             self.assertTrue(len(group.joints)==1)
00196             self.assertTrue(len(group.subgroups)==2)
00197     
00198         index=0
00199         if robot.group_states[0].group !="arms":
00200           index=1
00201           
00202         self.assertTrue(robot.group_states[index].group =="arms")
00203         self.assertTrue(robot.group_states[index].name =="tuck_arms")
00204         self.assertTrue(robot.group_states[1-index].group =="base")
00205         self.assertTrue(robot.group_states[1-index].name =="home")
00206             
00207         v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None)  
00208         self.assertTrue(len(v) == 1)
00209         self.assertTrue(v[0] ==0.2)
00210         
00211         w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None)  
00212         self.assertTrue(len(w) == 3)
00213         self.assertTrue(w[0] ==0.4)
00214         self.assertTrue(w[1] ==0)
00215         self.assertTrue(w[2] ==-1)
00216         
00217         index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1
00218         self.assertTrue(robot.end_effectors[index].name == 'r_end_effector')
00219         self.assertTrue(robot.end_effectors[index].group == 'r_end_effector')
00220         self.assertTrue(robot.end_effectors[index].parent_link == 'r_wrist_roll_link')
00221       
00222 
00223 if __name__ == '__main__':
00224     import rostest
00225     rostest.rosrun(PKG, 'srdf_python_parser_test', TestSRDFParser)


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Thu Feb 9 2017 03:50:57