test.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 PKG = 'srdfdom'
3 
4 import sys
5 import rospkg
6 import unittest
7 from srdfdom.srdf import SRDF
8 from xml.dom.minidom import parseString
9 import xml.dom
10 
11 # xml match code from test_xacro.py
12 # by Stuart Glaser and William Woodall
13 
15  c = elt.firstChild
16  while c:
17  if c.nodeType == xml.dom.Node.ELEMENT_NODE:
18  return c
19  c = c.nextSibling
20  return None
21 
23  c = elt.nextSibling
24  while c:
25  if c.nodeType == xml.dom.Node.ELEMENT_NODE:
26  return c
27  c = c.nextSibling
28  return None
29 
31  if len(a.attributes) != len(b.attributes):
32  print("Different number of attributes")
33  return False
34  a_atts = [(a.attributes.item(i).name, a.attributes.item(i).value) for i in range(len(a.attributes))]
35  b_atts = [(b.attributes.item(i).name, b.attributes.item(i).value) for i in range(len(b.attributes))]
36  a_atts.sort()
37  b_atts.sort()
38  for i in range(len(a_atts)):
39  if a_atts[i][0] != b_atts[i][0]:
40  print("Different attribute names: %s and %s" % (a_atts[i][0], b_atts[i][0]))
41  return False
42  try:
43  if abs(float(a_atts[i][1]) - float(b_atts[i][1])) > 1.0e-9:
44  print("Different attribute values: %s and %s" % (a_atts[i][1], b_atts[i][1]))
45  return False
46  except ValueError: # Attribute values aren't numeric
47  if a_atts[i][1] != b_atts[i][1]:
48  print("Different attribute values: %s and %s" % (a_atts[i][1], b_atts[i][1]))
49  return False
50  return True
51 
52 def elements_match(a, b):
53  if not a and not b:
54  return True
55  if not a or not b:
56  return False
57  if a.nodeType != b.nodeType:
58  print("Different node types: %d and %d" % (a.nodeType, b.nodeType))
59  return False
60  if a.nodeName != b.nodeName:
61  print("Different element names: %s and %s" % (a.nodeName, b.nodeName))
62  return False
63  if not all_attributes_match(a, b):
64  return False
66  return False
68  return False
69  return True
70 
71 def xml_matches(a, b):
72  if isinstance(a, str):
73  return xml_matches(parseString(a).documentElement, b)
74  if isinstance(b, str):
75  return xml_matches(a, parseString(b).documentElement)
76  if a.nodeType == xml.dom.Node.DOCUMENT_NODE:
77  return xml_matches(a.documentElement, b)
78  if b.nodeType == xml.dom.Node.DOCUMENT_NODE:
79  return xml_matches(a, b.documentElement)
80  if not elements_match(a, b):
81  print("Match failed:")
82  a.writexml(sys.stdout)
83  print
84  print('=' * 78)
85  b.writexml(sys.stdout)
86  return False
87  return True
88 
89 ## A python unit test for srdf
90 class TestSRDFParser(unittest.TestCase):
91  ## test valid srdf
92 
93  def test_full_srdf(self):
94  srdf_data = '''
95  <robot name="myrobot">
96  <group name="body">
97  <joint name="J1" />
98  <joint name="J2" />
99  <joint name="J3" />
100  <chain base_link="robot_base" tip_link="robot_tip" />
101  <group name="arm" />
102  </group>
103  <group_state name="zero" group="body">
104  <joint name="J1" value="0" />
105  <joint name="J2" value="0" />
106  <joint name="J3" value="0" />
107  </group_state>
108  <end_effector name="tip_ee" parent_link="tip" group="arm" parent_group="body" />
109  <end_effector name="othertip_ee" parent_link="othertip" group="arm" />
110  <virtual_joint name="virtual_joint" type="floating" parent_frame="body_frame" child_link="arm" />
111  <disable_collisions link1="link1" link2="link3" />
112  <disable_collisions reason="Adjacent" link1="link1" link2="link2" />
113  <link_sphere_approximation link="link1" />
114  <link_sphere_approximation link="link2" >
115  <sphere center="1.0 2.0 3.0" radius="1.0" />
116  <sphere center="1.0 2.0 4.0" radius="2.0" />
117  </link_sphere_approximation>
118  </robot>
119  '''
120  expected = '''
121 <robot name="myrobot">
122  <group name="body">
123  <joint name="J1" />
124  <joint name="J2" />
125  <joint name="J3" />
126  <chain base_link="robot_base" tip_link="robot_tip"/>
127  <group name="arm" />
128  </group>
129  <group_state name="zero" group="body">
130  <joint name="J1" value="0" />
131  <joint name="J2" value="0" />
132  <joint name="J3" value="0" />
133  </group_state>
134  <end_effector group="arm" name="tip_ee" parent_group="body" parent_link="tip"/>
135  <end_effector name="othertip_ee" parent_link="othertip" group="arm" />
136  <virtual_joint child_link="arm" name="virtual_joint" parent_frame="body_frame" type="floating" />
137  <disable_collisions link1="link1" link2="link3" />
138  <disable_collisions link1="link1" link2="link2" reason="Adjacent" />
139  <link_sphere_approximation link="link1" />
140  <link_sphere_approximation link="link2" >
141  <sphere center="1.0 2.0 3.0" radius="1.0" />
142  <sphere center="1.0 2.0 4.0" radius="2.0" />
143  </link_sphere_approximation>
144 </robot>
145  '''
146  robot = SRDF.from_xml_string(srdf_data)
147  self.assertTrue( xml_matches(robot.to_xml_string(),expected))
148 
149  def test_simple_srdf(self):
150  datadir=rospkg.RosPack().get_path('srdfdom')+"/test/res/"
151  stream = open(datadir+'pr2_desc.1.srdf', 'r')
152  robot = SRDF.from_xml_string(stream.read())
153  stream.close()
154  self.assertTrue(len(robot.virtual_joints)==0)
155  self.assertTrue(len(robot.groups)==0)
156  self.assertTrue(len(robot.group_states)==0)
157  self.assertTrue(len(robot.disable_collisionss)==0)
158  self.assertTrue(len(robot.end_effectors)==0)
159 
160  stream = open(datadir+'pr2_desc.2.srdf', 'r')
161  robot = SRDF.from_xml_string(stream.read())
162  stream.close()
163  self.assertTrue(len(robot.virtual_joints)==1)
164  self.assertTrue(len(robot.groups)==1)
165  self.assertTrue(len(robot.group_states)==0)
166  self.assertTrue(len(robot.disable_collisionss)==0)
167  self.assertTrue(len(robot.end_effectors)==0)
168 
169  def test_complex_srdf(self):
170  datadir=rospkg.RosPack().get_path('srdfdom')+"/test/res/"
171  stream = open(datadir+'pr2_desc.3.srdf', 'r')
172  robot = SRDF.from_xml_string(stream.read())
173  stream.close()
174  self.assertTrue(len(robot.virtual_joints)==1)
175  self.assertTrue(len(robot.groups)==7)
176  self.assertTrue(len(robot.group_states)==2)
177  self.assertTrue(len(robot.disable_collisionss)==2)
178  self.assertTrue(robot.disable_collisionss[0].reason=="adjacent")
179  self.assertTrue(len(robot.end_effectors)==2)
180 
181  self.assertTrue(robot.virtual_joints[0].name=="world_joint")
182  self.assertTrue(robot.virtual_joints[0].type=="planar")
183 
184  for group in robot.groups:
185  if (group.name == "left_arm" or group.name == "right_arm" ):
186  self.assertTrue(len(group.chains)==1)
187  if group.name == "arms":
188  self.assertTrue(len(group.subgroups)==2)
189  if group.name == "base":
190  self.assertTrue(len(group.joints)==1)
191  if (group.name == "l_end_effector" or group.name == "r_end_effector" ):
192  self.assertTrue(len(group.links)==1)
193  self.assertTrue(len(group.joints)==9)
194  if group.name == "whole_body" :
195  self.assertTrue(len(group.joints)==1)
196  self.assertTrue(len(group.subgroups)==2)
197 
198  index=0
199  if robot.group_states[0].group !="arms":
200  index=1
201 
202  self.assertTrue(robot.group_states[index].group =="arms")
203  self.assertTrue(robot.group_states[index].name =="tuck_arms")
204  self.assertTrue(robot.group_states[1-index].group =="base")
205  self.assertTrue(robot.group_states[1-index].name =="home")
206 
207  v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None)
208  self.assertTrue(len(v) == 1)
209  self.assertTrue(v[0] ==0.2)
210 
211  w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None)
212  self.assertTrue(len(w) == 3)
213  self.assertTrue(w[0] ==0.4)
214  self.assertTrue(w[1] ==0)
215  self.assertTrue(w[2] ==-1)
216 
217  index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1
218  self.assertTrue(robot.end_effectors[index].name == 'r_end_effector')
219  self.assertTrue(robot.end_effectors[index].group == 'r_end_effector')
220  self.assertTrue(robot.end_effectors[index].parent_link == 'r_wrist_roll_link')
221 
222 
223 if __name__ == '__main__':
224  import rostest
225  rostest.rosrun(PKG, 'srdf_python_parser_test', TestSRDFParser)
def first_child_element(elt)
Definition: test.py:14
def xml_matches(a, b)
Definition: test.py:71
def all_attributes_match(a, b)
Definition: test.py:30
def test_full_srdf(self)
test valid srdf
Definition: test.py:93
def next_sibling_element(elt)
Definition: test.py:22
def elements_match(a, b)
Definition: test.py:52
A python unit test for srdf.
Definition: test.py:90
def test_complex_srdf(self)
Definition: test.py:169
def test_simple_srdf(self)
Definition: test.py:149


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Thu Jun 6 2019 19:59:13