Go to the documentation of this file.00001
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
00012
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:
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
00090 class TestSRDFParser(unittest.TestCase):
00091
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)