test_scan.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of the Willow Garage nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 #
34 
35 ## Gazebo test cameras validation
36 
37 PKG = 'pr2_gazebo'
38 NAME = 'test_scan'
39 
40 import math
41 import roslib
42 roslib.load_manifest(PKG)
43 
44 
45 import sys, unittest
46 import os, os.path, threading, time
47 import rospy, rostest
48 from sensor_msgs.msg import LaserScan
49 
50 TEST_DURATION = 25
51 ERROR_TOL = 0.05
52 FAIL_COUNT_TOL = 10
53 
54 inf = float('inf')
55 TARGET_RANGES = [
56 inf, inf, inf, inf, inf, inf, inf,
57 inf, inf, inf, inf, inf, inf, inf,
58 inf, inf, inf, inf, inf, inf, inf,
59 inf, inf, inf, inf, inf, inf, inf,
60 inf, inf, inf, inf, inf, inf, inf,
61 inf, inf, inf, inf, inf, inf, inf,
62 inf, inf, inf, inf, inf, inf, inf,
63 inf, inf, inf, inf, inf, inf, inf,
64 inf, inf, inf, inf, inf, inf, inf,
65 inf, inf, inf, inf, inf, inf, inf,
66 inf, inf, inf, inf, inf, inf, inf,
67 inf, inf, inf, inf, inf, inf, inf,
68 inf, inf, inf, inf, inf, inf, inf,
69 inf, inf, inf, inf, inf, inf, inf,
70 inf, inf, inf, inf, inf, inf, inf,
71 inf, inf, inf, inf, inf, inf, inf,
72 inf, inf, inf, inf, inf, inf, inf,
73 inf, inf, inf, inf, inf, inf, inf,
74 inf, inf, inf, inf, inf, inf, inf,
75 inf, inf, inf, inf, inf, inf, inf,
76 inf, inf, inf, inf, inf, inf, inf,
77 inf, inf, inf, inf, inf, inf, inf,
78 inf, inf, inf, inf, inf, inf, inf,
79 inf, inf, inf, inf, inf, inf, inf,
80 inf, inf, inf, inf, inf, inf, inf,
81 inf, inf, inf, inf, inf, inf, inf,
82 inf, inf, inf, inf, inf, inf, inf,
83 inf, inf, inf, inf, inf, inf, inf,
84 inf, inf, inf, inf, inf, inf, inf,
85 inf, inf, inf, inf, inf, inf, inf,
86 inf, inf, inf, inf, inf, inf, inf,
87 inf, inf, inf, inf, inf, inf, inf,
88 inf, inf, inf, inf, inf, inf, inf,
89 inf, inf, inf, inf, inf,
90 2.04421663284, 2.03210663795, 2.0194671154, 2.0155646801, 1.99427628517, 2.00035619736, 1.98695075512,
91 1.97852289677, 1.96805429459, 1.9588958025, 1.95648872852, 1.94692718983, 1.93678343296, 1.91842794418,
92 1.92827701569, 1.91704964638, 1.91358006001, 1.92673146725, 1.94037222862, 1.9256892204284668, inf, inf,
93 inf, inf, inf, 2.14445352554, 2.08714962006, 2.05095148087, 2.02395749092, 2.00428080559,
94 1.97976839542, 1.95980763435, 1.9413497448, 1.93820095062, 1.91665804386, 1.92057430744, 1.9003251791,
95 1.88783442974, 1.8934186697, 1.8741286993, 1.86705768108, 1.85493171215, 1.85695803165, 1.85333991051,
96 1.83796668053, 1.84245407581, 1.84312713146, 1.84570860863, 1.82822322845, 1.83273553848, 1.83729255199,
97 1.83289527893, 1.8334633112, 1.83232152462, 1.82959234715, 1.8262963295, 1.83373129368, 1.83350098133,
98 1.83656823635, 1.83848130703, 1.83709740639, 1.85071253777, 1.84394586086, 1.86468243599, 1.85489320755,
99 1.85955429077, 1.86517083645, 1.88544261456, 1.89780557156, 1.89888262749, 1.92238008976, 1.922524333,
100 1.93953430653, 1.948564291, 1.96180999279, 1.98394215107, 1.99410378933, 2.02381777763, 2.05232095718,
101 2.09428310394, 2.14106678963, 2.51629209518, 2.51145839691, 2.50856781006, 2.49926424026, 2.51138210297,
102 2.50430202484, 2.50846076012, 2.52427816391, 2.52740097046, 2.5371067524, 2.54831123352, 2.56848597527,
103 2.59808444977, 2.62111496925, 2.68968343735, inf, inf, inf, inf,
104 inf, inf, inf, inf, inf, 2.72182154655, 2.7262597084,
105 inf, inf, inf, inf, inf, inf, inf,
106 inf, inf, inf, inf, inf, inf, inf,
107 inf, inf, 1.80504393578, 1.76711213589, 1.73867917061, 1.7188462019, 1.706569314,
108 1.69170439243, 1.67706441879, 1.66418457031, 1.6565977335, 1.64732301235, 1.64360272884, 1.63184809685,
109 1.63481676579, 1.62855827808, 1.62778234482, 1.61982572079, 1.6105659008, 1.6124740839, 1.60954415798,
110 1.6100025177, 1.60651493073, 1.59700119495, 1.60250425339, 1.60432767868, 1.602414608, 1.60677278042,
111 1.61422848701, 1.61278867722, 1.61149096489, 1.61712217331, 1.61782002449, 1.63084983826, 1.63016283512,
112 1.64222669601, 1.64433014393, 1.65826809406, 1.66858124733, 1.67725527287, 1.68793737888, 1.70181775093,
113 1.71679854393, 1.74195897579, 1.76237237453, 1.80084371567, inf, inf, inf,
114 inf, inf, inf, inf, inf, inf, inf,
115 inf, inf, inf, inf, inf, inf, inf,
116 inf, inf, inf, inf, inf, inf, inf,
117 inf, inf, inf, inf, inf, inf, inf,
118 inf, inf, inf, inf, inf, inf, inf,
119 inf, inf, inf, inf, inf, inf, inf,
120 inf, inf, inf, inf, inf, inf, inf,
121 inf, inf, inf, inf, inf, inf, inf,
122 inf, inf, inf, inf, inf, inf, inf,
123 inf, inf, inf, inf, inf, inf, inf,
124 inf, inf, inf, inf, inf, inf, inf,
125 inf, inf, inf, inf, inf, inf, inf,
126 inf, inf, inf, inf, inf, inf, inf,
127 inf, inf, inf, inf, inf, inf, inf,
128 inf, inf, inf, inf, inf, inf, inf,
129 inf, inf, inf, inf, inf, inf, inf,
130 inf, inf, inf, inf, inf, inf, inf,
131 inf, inf, inf, inf, inf, inf, inf,
132 inf, inf, inf, inf, inf, inf, inf,
133 inf, inf, inf, inf, inf, inf, inf,
134 inf, inf, inf, inf, inf, inf, inf,
135 inf, inf, inf, inf, inf, inf, inf,
136 inf, inf, inf, inf, inf, inf, inf,
137 inf, inf, inf, inf, inf, inf, inf,
138 inf, inf, inf, inf, inf, inf, inf,
139 inf, inf, inf, inf, inf, inf, inf,
140 inf, inf, inf, inf, inf, inf, inf,
141 inf, inf, inf, inf, inf, inf, inf,
142 inf, inf, inf, inf, inf, inf, inf,
143 inf, inf, inf, inf, inf, inf, inf,
144 inf, inf, inf, inf, inf, inf, inf,
145 inf, inf, inf, inf, inf, inf, inf,
146 inf, inf, inf, inf, inf, inf, inf,
147 inf, inf, inf]
148 
149 
150 
151 
152 
153 
154 TARGET_INTENSITIES = [
155 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
156 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
157 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
158 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
159 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
160 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
161 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
162 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
163 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
164 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
165 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
166 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
167 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
168 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
169 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
170 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
171 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
172 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
173 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
174 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
175 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
176 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
177 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
178 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
179 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
180 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
181 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
182 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
183 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
184 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
185 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
186 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
187 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
188 0.0, 0.0, 0.0, 0.0, 0.0, 250.002059937, 500.003509521,
189 500.005279541, 499.994689941, 500.009216309, 499.99597168, 499.988525391, 499.999633789, 499.992553711,
190 499.995391846, 499.997558594, 500.000244141, 500.001342773, 499.990570068, 499.999420166, 499.998443604,
191 500.003082275, 500.000579834, 500.011169434, 499.998931885, 249.997711182, 0.0, 0.0,
192 0.0, 1500.00146484, 3000.0, 3000.00292969, 3000.00268555, 3000.00878906, 3000.00415039,
193 3000.00097656, 2999.99853516, 3000.00805664, 3000.00146484, 3000.00415039, 2999.99633789, 2999.99609375,
194 3000.00561523, 2999.99438477, 2999.99584961, 2999.99951172, 2999.99536133, 3000.00341797, 3000.00146484,
195 2999.99536133, 2999.99633789, 3000.00048828, 3000.00268555, 2999.99243164, 3000.00415039, 2999.99853516,
196 3000.00439453, 3000.00708008, 2999.99316406, 3000.00366211, 3000.0, 3000.0065918, 2999.99780273,
197 3000.0, 2999.99121094, 2999.99902344, 3000.00195312, 2999.98901367, 2999.9934082, 2999.99951172,
198 2999.99584961, 3000.00366211, 2999.99829102, 2999.99902344, 3000.00292969, 2999.99853516, 2999.99902344,
199 2999.99658203, 2999.99829102, 2999.99658203, 3000.00146484, 3000.00195312, 3000.00878906, 2999.99414062,
200 3000.00097656, 2499.99438477, 2000.00012207, 2000.00183105, 1999.98620605, 1999.99951172, 1999.99206543,
201 2000.00280762, 1999.99804688, 2000.00195312, 1999.99401855, 1999.99853516, 1999.9901123, 1999.99731445,
202 2000.00256348, 2000.00927734, 1000.00360107, 0.0, 0.0, 0.0, 0.0,
203 0.0, 0.0, 0.0, 0.0, 1000.0012207, 1999.9967041, 1000.00085449,
204 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
205 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
206 0.0, 500.003265381, 999.998596191, 999.997680664, 999.991210938, 999.996032715, 1000.00134277,
207 1000.00958252, 999.99786377, 1000.0055542, 999.996276855, 1000.00384521, 999.996765137, 1000.00292969,
208 1000.00128174, 999.996154785, 1000.0032959, 1000.00061035, 999.99609375, 999.99621582, 999.996520996,
209 1000.00500488, 999.997253418, 999.983886719, 1000.00042725, 999.998596191, 999.996887207, 999.992370605,
210 1000.00213623, 1000.00799561, 999.997680664, 1000.00170898, 1000.00073242, 999.996398926, 1000.00830078,
211 1000.00238037, 999.993774414, 999.993713379, 999.989990234, 999.996765137, 1000.00146484, 1000.00634766,
212 999.990966797, 1000.00305176, 999.996704102, 499.996307373, 0.0, 0.0, 0.0,
213 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
214 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
215 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
216 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ]
217 
218 # indigo gazebo(2) uses 10.0 for inf data
219 if os.environ['ROS_DISTRO'] <= 'indigo':
220  TARGET_RANGES = map(lambda x: x if x != inf else 10.0, TARGET_RANGES)
221 
222 class PointCloudTest(unittest.TestCase):
223  def __init__(self, *args):
224  super(PointCloudTest, self).__init__(*args)
225  self.success = False
226 
227 
228  def printPointCloud(self, cloud):
229  print "["
230  i = 0
231  for pt in cloud.ranges:
232  sys.stdout.write(str(pt) + ", ")
233  i = i + 1
234  if ((i % 7) == 0):
235  print "" #newline
236  print "]"
237 
238  print "["
239  i = 0
240  for pt in cloud.intensities:
241  sys.stdout.write(str(pt) + ", ")
242  i = i + 1
243  if ((i % 7) == 0):
244  print "" #newline
245  print "]"
246 
247 
248  def pointInput(self, cloud):
249  i = 0
250  range_fail_count = 0
251  print "Input laser scan received"
252  self.printPointCloud(cloud) #uncomment to capture new data
253  while (i < len(cloud.ranges) and i < len(TARGET_RANGES)):
254  d = cloud.ranges[i] - TARGET_RANGES[i]
255  if ((d < - ERROR_TOL) or (d > ERROR_TOL)):
256  range_fail_count += 1
257  print "range_fail_count:" + str(range_fail_count) + " failed. error:" + str(d) + " exceeded tolerance:" + str(ERROR_TOL)
258  i = i + 1
259 
260  i = 0
261  intensity_fail_count = 0
262  while (i < len(cloud.intensities) and i < len(TARGET_INTENSITIES)):
263  d = cloud.intensities[i] - TARGET_INTENSITIES[i]
264  if cloud.intensities[i] > 0:
265  d = d/cloud.intensities[i]
266  if cloud.intensities[i] < 0:
267  intensity_fail_count += 1
268  print "intensity_fail_count:" + str(intensity_fail_count) + " failed. intensity <0:" + str(cloud.intensiteis[i])
269  else:
270  if ((d < - ERROR_TOL) or (d > ERROR_TOL)):
271  intensity_fail_count += 1
272  print "intensity_fail_count:" + str(intensity_fail_count) + " failed. error:" + str(d) + " exceeded tolerance:" + str(ERROR_TOL)
273  i = i + 1
274 
275  if range_fail_count > FAIL_COUNT_TOL:
276  print "Range fail count too large (" + str(range_fail_count) + "), failing scan"
277  return
278 
279  if intensity_fail_count > FAIL_COUNT_TOL:
280  print "Intensity fail count too large (" + str(intensity_fail_count) + "), failing scan"
281  return
282 
283  self.success = True
284 
285  def test_scan(self):
286  print "LNK\n"
287  rospy.Subscriber("/base_scan", LaserScan, self.pointInput)
288  rospy.init_node(NAME, anonymous=True)
289  timeout_t = time.time() + TEST_DURATION
290  while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
291  time.sleep(0.1)
292  self.assert_(self.success)
293 
294 
295 
296 
297 if __name__ == '__main__':
298  rostest.run(PKG, sys.argv[0], PointCloudTest, sys.argv) #, text_mode=True)
299 
300 
test_scan.PointCloudTest.__init__
def __init__(self, args)
Definition: test_scan.py:223
test_scan.PointCloudTest.pointInput
def pointInput(self, cloud)
Definition: test_scan.py:248
test_scan.PointCloudTest
Definition: test_scan.py:222
test_scan.PointCloudTest.success
success
Definition: test_scan.py:225
test_scan.PointCloudTest.printPointCloud
def printPointCloud(self, cloud)
Definition: test_scan.py:228
test_scan.PointCloudTest.test_scan
def test_scan(self)
Definition: test_scan.py:285

pr2_gazebo
Author(s): John Hsu
autogenerated on Mon Jun 10 2019 14:28:51