rs2_test.py
Go to the documentation of this file.
1 import os
2 import sys
3 from rs2_listener import CWaitForMessage
4 
5 import rosbag
6 from cv_bridge import CvBridge, CvBridgeError
7 import numpy as np
8 import tf
9 import itertools
10 import subprocess
11 import rospy
12 import time
13 import rosservice
14 
15 global tf_timeout
16 tf_timeout = 5
17 
18 def ImuGetData(rec_filename, topic):
19  # res['value'] = first value of topic.
20  # res['max_diff'] = max difference between returned value and all other values of topic in recording.
21 
22  bag = rosbag.Bag(rec_filename)
23  res = dict()
24  res['value'] = None
25  res['max_diff'] = [0,0,0]
26  for topic, msg, t in bag.read_messages(topics=topic):
27  value = np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z])
28  if res['value'] is None:
29  res['value'] = value
30  else:
31  diff = abs(value - res['value'])
32  res['max_diff'] = [max(diff[x], res['max_diff'][x]) for x in range(len(diff))]
33  res['max_diff'] = np.array(res['max_diff'])
34  return res
35 
36 def AccelGetData(rec_filename):
37  return ImuGetData(rec_filename, '/device_0/sensor_2/Accel_0/imu/data')
38 
40  gt_data = AccelGetData(rec_filename)
41  gt_data['ros_value'] = np.array([0.63839424, 0.05380408, 9.85343552])
42  gt_data['ros_max_diff'] = np.array([1.97013582e-02, 4.65862500e-09, 4.06165277e-02])
43  return gt_data
44 
45 def ImuTest(data, gt_data):
46  # check that the imu data received is the same as in the recording.
47  # check that in the rotated imu received the g-accelartation is pointing up according to ROS standards.
48  try:
49  v_data = np.array([data['value'][0].x, data['value'][0].y, data['value'][0].z])
50  v_gt_data = gt_data['value']
51  diff = v_data - v_gt_data
52  max_diff = abs(diff).max()
53  msg = 'original accel: Expect max diff of %.3f. Got %.3f.' % (gt_data['max_diff'].max(), max_diff)
54  print (msg)
55  if max_diff > gt_data['max_diff'].max():
56  return False, msg
57 
58  v_data = data['ros_value'][0]
59  v_gt_data = gt_data['ros_value']
60  diff = v_data - v_gt_data
61  max_diff = abs(diff).max()
62  msg = 'rotated to ROS: Expect max diff of %.3f. Got %.3f.' % (gt_data['ros_max_diff'].max(), max_diff)
63  print (msg)
64  if max_diff > gt_data['ros_max_diff'].max():
65  return False, msg
66  except Exception as e:
67  msg = '%s' % e
68  print ('Test Failed: %s' % msg)
69  return False, msg
70  return True, ''
71 
72 def ImageGetData(rec_filename, topic):
73  bag = rosbag.Bag(rec_filename)
74  bridge = CvBridge()
75  all_avg = []
76  ok_percent = []
77  res = dict()
78 
79  for topic, msg, t in bag.read_messages(topics=topic):
80  try:
81  cv_image = bridge.imgmsg_to_cv2(msg, msg.encoding)
82  except CvBridgeError as e:
83  print(e)
84  continue
85  pyimg = np.asarray(cv_image)
86  ok_number = (pyimg != 0).sum()
87  ok_percent.append(float(ok_number) / (pyimg.shape[0] * pyimg.shape[1]))
88  all_avg.append(pyimg.sum() / ok_number)
89 
90  all_avg = np.array(all_avg)
91  channels = cv_image.shape[2] if len(cv_image.shape) > 2 else 1
92  res['num_channels'] = channels
93  res['shape'] = cv_image.shape
94  res['avg'] = all_avg.mean()
95  res['ok_percent'] = {'value': (np.array(ok_percent).mean()) / channels, 'epsilon': 0.01}
96  res['epsilon'] = max(all_avg.max() - res['avg'], res['avg'] - all_avg.min())
97  res['reported_size'] = [msg.width, msg.height, msg.step]
98 
99  return res
100 
101 
102 def ImageColorGetData(rec_filename):
103  return ImageGetData(rec_filename, '/device_0/sensor_1/Color_0/image/data')
104 
105 
106 def ImageDepthGetData(rec_filename):
107  return ImageGetData(rec_filename, '/device_0/sensor_0/Depth_0/image/data')
108 
109 
111  gt_data = ImageDepthGetData(rec_filename)
112  color_data = ImageColorGetData(rec_filename)
113  gt_data['shape'] = color_data['shape'][:2]
114  gt_data['reported_size'] = color_data['reported_size']
115  gt_data['reported_size'][2] = gt_data['reported_size'][0]*2
116  gt_data['ok_percent']['epsilon'] *= 3
117  return gt_data
118 
120  gt_data = ImageDepthGetData(rec_filename)
121  gt_data['shape'] = [x/2 for x in gt_data['shape']]
122  gt_data['reported_size'] = [x/2 for x in gt_data['reported_size']]
123  gt_data['epsilon'] *= 3
124  return gt_data
125 
126 def ImageColorTest(data, gt_data):
127  # check that all data['num_channels'] are the same as gt_data['num_channels'] and that avg value of all
128  # images are within epsilon of gt_data['avg']
129  try:
130  channels = list(set(data['num_channels']))
131  msg = 'Expect %d channels. Got %d channels.' % (gt_data['num_channels'], channels[0])
132  print (msg)
133  if len(channels) > 1 or channels[0] != gt_data['num_channels']:
134  return False, msg
135  msg = 'Expected all received images to be the same shape. Got %s' % str(set(data['shape']))
136  print (msg)
137  if len(set(data['shape'])) > 1:
138  return False, msg
139  msg = 'Expected shape to be %s. Got %s' % (gt_data['shape'], list(set(data['shape']))[0])
140  print (msg)
141  if (np.array(list(set(data['shape']))[0]) != np.array(gt_data['shape'])).any():
142  return False, msg
143  msg = 'Expected header [width, height, step] to be %s. Got %s' % (gt_data['reported_size'], list(set(data['reported_size']))[0])
144  print (msg)
145  if (np.array(list(set(data['reported_size']))[0]) != np.array(gt_data['reported_size'])).any():
146  return False, msg
147  msg = 'Expect average of %.3f (+-%.3f). Got average of %.3f.' % (gt_data['avg'].mean(), gt_data['epsilon'], np.array(data['avg']).mean())
148  print (msg)
149  if abs(np.array(data['avg']).mean() - gt_data['avg'].mean()) > gt_data['epsilon']:
150  return False, msg
151 
152  msg = 'Expect no holes percent > %.3f. Got %.3f.' % (gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon'], np.array(data['ok_percent']).mean())
153  print (msg)
154  if np.array(data['ok_percent']).mean() < gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon']:
155  return False, msg
156 
157  except Exception as e:
158  msg = '%s' % e
159  print ('Test Failed: %s' % msg)
160  return False, msg
161  return True, ''
162 
163 
164 def ImageColorTest_3epsilon(data, gt_data):
165  gt_data['epsilon'] *= 3
166  return ImageColorTest(data, gt_data)
167 
168 def NotImageColorTest(data, gt_data):
169  res = ImageColorTest(data, gt_data)
170  return (not res[0], res[1])
171 
172 def PointCloudTest(data, gt_data):
173  width = np.array(data['width']).mean()
174  height = np.array(data['height']).mean()
175  msg = 'Expect image size %d(+-%d), %d. Got %d, %d.' % (gt_data['width'][0], gt_data['width'][1], gt_data['height'][0], width, height)
176  print (msg)
177  if abs(width - gt_data['width'][0]) > gt_data['width'][1] or height != gt_data['height'][0]:
178  return False, msg
179  mean_pos = np.array([xx[:3] for xx in data['avg']]).mean(0)
180  msg = 'Expect average position of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][:3], gt_data['epsilon'][0], mean_pos)
181  print (msg)
182  if abs(mean_pos - gt_data['avg'][0][:3]).max() > gt_data['epsilon'][0]:
183  return False, msg
184  mean_col = np.array([xx[3:] for xx in data['avg']]).mean(0)
185  msg = 'Expect average color of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][3:], gt_data['epsilon'][1], mean_col)
186  print (msg)
187  if abs(mean_col - gt_data['avg'][0][3:]).max() > gt_data['epsilon'][1]:
188  return False, msg
189 
190  return True, ''
191 
192 
193 def staticTFTest(data, gt_data):
194  for couple in gt_data.keys():
195  if data[couple] is None:
196  msg = 'Tf is None for couple %s' % '->'.join(couple)
197  return False, msg
198  if any(abs((np.array(data[couple][0]) - np.array(gt_data[couple][0]))) > 1e-5) or \
199  any(abs((np.array(data[couple][1]) - np.array(gt_data[couple][1]))) > 1e-5):
200  msg = 'Tf is changed for couple %s' % '->'.join(couple)
201  return False, msg
202  return True, ''
203 
204 test_types = {'vis_avg': {'listener_theme': 'colorStream',
205  'data_func': ImageColorGetData,
206  'test_func': ImageColorTest},
207  'depth_avg': {'listener_theme': 'depthStream',
208  'data_func': ImageDepthGetData,
209  'test_func': ImageColorTest},
210  'no_file': {'listener_theme': 'colorStream',
211  'data_func': lambda x: None,
212  'test_func': NotImageColorTest},
213  'pointscloud_avg': {'listener_theme': 'pointscloud',
214  'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 80, 160, 240])], 'epsilon': [0.04, 5]},
215  'test_func': PointCloudTest},
216  'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1',
217  'data_func': ImageDepthGetData,
218  'test_func': ImageColorTest},
219  'align_depth_color': {'listener_theme': 'alignedDepthColor',
220  'data_func': ImageDepthInColorShapeGetData,
221  'test_func': ImageColorTest_3epsilon},
222  'depth_avg_decimation': {'listener_theme': 'depthStream',
223  'data_func': ImageDepthGetData_decimation,
224  'test_func': ImageColorTest},
225  'align_depth_ir1_decimation': {'listener_theme': 'alignedDepthInfra1',
226  'data_func': ImageDepthGetData,
227  'test_func': ImageColorTest},
228  'static_tf': {'listener_theme': 'static_tf',
229  'data_func': lambda x: {('camera_link', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]),
230  ('camera_link', 'camera_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]),
231  ('camera_link', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]),
232  ('camera_depth_frame', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]),
233  ('camera_depth_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]),
234  ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])}
235  ,
236  'test_func': staticTFTest},
237  'accel_up': {'listener_theme': 'accelStream',
238  'data_func': AccelGetDataDeviceStandStraight,
239  'test_func': ImuTest},
240  }
241 
242 
243 def run_test(test, listener_res):
244  # gather ground truth with test_types[test['type']]['data_func'] and recording from test['rosbag_filename']
245  # return results from test_types[test['type']]['test_func']
246  test_type = test_types[test['type']]
247  gt_data = test_type['data_func'](test['params']['rosbag_filename'])
248  return test_type['test_func'](listener_res[test_type['listener_theme']], gt_data)
249 
250 
251 def print_results(results):
252  title = 'TEST RESULTS'
253  headers = ['index', 'test name', 'score', 'message']
254  col_0_width = len(headers[0]) + 1
255  col_1_width = max([len(headers[1])] + [len(test[0]) for test in results]) + 1
256  col_2_width = max([len(headers[2]), len('OK'), len('FAILED')]) + 1
257  col_3_width = max([len(headers[3])] + [len(test[1][1]) for test in results]) + 1
258  total_width = col_0_width + col_1_width + col_2_width + col_3_width
259  print
260  print (('{:^%ds}'%total_width).format(title))
261  print ('-'*total_width)
262  print (('{:<%ds}{:<%ds}{:>%ds} : {:<%ds}' % (col_0_width, col_1_width, col_2_width, col_3_width)).format(*headers))
263  print ('-'*(col_0_width-1) + ' '*1 + '-'*(col_1_width-1) + ' '*2 + '-'*(col_2_width-1) + ' '*3 + '-'*(col_3_width-1))
264  print ('\n'.join([('{:<%dd}{:<%ds}{:>%ds} : {:<s}' % (col_0_width, col_1_width, col_2_width)).format(idx, test[0], 'OK' if test[1][0] else 'FAILED', test[1][1]) for idx, test in enumerate(results)]))
265  print
266 
267 
268 def get_tf(tf_listener, from_id, to_id):
269  global tf_timeout
270  try:
271  start_time = time.time()
272  # print 'Waiting for transform: %s -> %s for %.2f(sec)' % (from_id, to_id, tf_timeout)
273  tf_listener.waitForTransform(from_id, to_id, rospy.Time(), rospy.Duration(tf_timeout))
274  res = tf_listener.lookupTransform(from_id, to_id, rospy.Time())
275  except Exception as e:
276  res = None
277  finally:
278  waited_for = time.time() - start_time
279  tf_timeout = max(0.0, tf_timeout - waited_for)
280  return res
281 
282 
283 def run_tests(tests):
284  msg_params = {'timeout_secs': 5}
285  results = []
286  params_strs = set([test['params_str'] for test in tests])
287  for params_str in params_strs:
288  rec_tests = [test for test in tests if test['params_str'] == params_str]
289  themes = [test_types[test['type']]['listener_theme'] for test in rec_tests]
290  msg_retriever = CWaitForMessage(msg_params)
291  print ('*'*30)
292  print ('Running the following tests: %s' % ('\n' + '\n'.join([test['name'] for test in rec_tests])))
293  print ('*'*30)
294  num_of_startups = 5
295  is_node_up = False
296  for run_no in range(num_of_startups):
297  print
298  print ('*'*8 + ' Starting ROS ' + '*'*8)
299  print ('running node (%d/%d)' % (run_no, num_of_startups))
300  cmd_params = ['roslaunch', 'realsense2_camera', 'rs_from_file.launch'] + params_str.split(' ')
301  print ('running command: ' + ' '.join(cmd_params))
302  p_wrapper = subprocess.Popen(cmd_params, stdout=None, stderr=None)
303  time.sleep(2)
304  service_list = rosservice.get_service_list()
305  is_node_up = len([service for service in service_list if 'realsense2_camera/' in service]) > 0
306  if is_node_up:
307  print ('Node is UP')
308  break
309  print ('Node is NOT UP')
310  print ('*'*8 + ' Killing ROS ' + '*'*9)
311  p_wrapper.terminate()
312  p_wrapper.wait()
313  print ('DONE')
314 
315  if is_node_up:
316  listener_res = msg_retriever.wait_for_messages(themes)
317  if 'static_tf' in [test['type'] for test in rec_tests]:
318  print ('Gathering static transforms')
319  frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose']
320  tf_listener = tf.TransformListener()
321  listener_res['static_tf'] = dict([(xx, get_tf(tf_listener, xx[0], xx[1])) for xx in itertools.combinations(frame_ids, 2)])
322  print ('*'*8 + ' Killing ROS ' + '*'*9)
323  p_wrapper.terminate()
324  p_wrapper.wait()
325  else:
326  listener_res = dict([[theme_name, {}] for theme_name in themes])
327 
328  print ('*'*30)
329  print ('DONE run')
330  print ('*'*30)
331 
332  for test in rec_tests:
333  try:
334  res = run_test(test, listener_res)
335  except Exception as e:
336  print ('Test %s Failed: %s' % (test['name'], e))
337  res = False, '%s' % e
338  results.append([test['name'], res])
339 
340  return results
341 
342 
343 def main():
344  outdoors_filename = './records/outdoors_1color.bag'
345  all_tests = [{'name': 'non_existent_file', 'type': 'no_file', 'params': {'rosbag_filename': '/home/non_existent_file.txt'}},
346  {'name': 'vis_avg_2', 'type': 'vis_avg', 'params': {'rosbag_filename': outdoors_filename}},
347  {'name': 'depth_avg_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename}},
348  {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}},
349  {'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}},
350  {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}},
351  {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}},
352  {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation'}},
353  {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation', 'align_depth': 'true'}},
354  # {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename}}, # Not working in Travis...
355  # {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag'}}, # Keeps failing on Travis CI. See https://github.com/IntelRealSense/realsense-ros/pull/1504#issuecomment-744226704
356  ]
357 
358  # Normalize parameters:
359  for test in all_tests:
360  test['params']['rosbag_filename'] = os.path.abspath(test['params']['rosbag_filename'])
361  test['params_str'] = ' '.join([key + ':=' + test['params'][key] for key in sorted(test['params'].keys())])
362 
363  if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv:
364  print ('USAGE:')
365  print ('------')
366  print ('rs2_test.py --all | <test_name> [<test_name> [...]]')
367  print
368  print ('Available tests are:')
369  print ('\n'.join([test['name'] for test in all_tests]))
370  exit(-1)
371 
372  if '--all' in sys.argv[1:]:
373  tests_to_run = all_tests
374  else:
375  tests_to_run = [test for test in all_tests if test['name'] in sys.argv[1:]]
376 
377  results = run_tests(tests_to_run)
378  print_results(results)
379 
380  res = int(all([result[1][0] for result in results])) - 1
381  print ('exit (%d)' % res)
382  exit(res)
383 
384 if __name__ == '__main__':
385  main()
def ImageGetData(rec_filename, topic)
Definition: rs2_test.py:72
def run_test(test, listener_res)
Definition: rs2_test.py:243
def main()
Definition: rs2_test.py:343
def ImageDepthGetData_decimation(rec_filename)
Definition: rs2_test.py:119
def ImuTest(data, gt_data)
Definition: rs2_test.py:45
def ImageColorGetData(rec_filename)
Definition: rs2_test.py:102
def staticTFTest(data, gt_data)
Definition: rs2_test.py:193
def PointCloudTest(data, gt_data)
Definition: rs2_test.py:172
def ImageDepthInColorShapeGetData(rec_filename)
Definition: rs2_test.py:110
def print_results(results)
Definition: rs2_test.py:251
def get_tf(tf_listener, from_id, to_id)
Definition: rs2_test.py:268
def ImageColorTest(data, gt_data)
Definition: rs2_test.py:126
def ImageColorTest_3epsilon(data, gt_data)
Definition: rs2_test.py:164
def NotImageColorTest(data, gt_data)
Definition: rs2_test.py:168
def ImuGetData(rec_filename, topic)
Definition: rs2_test.py:18
def AccelGetData(rec_filename)
Definition: rs2_test.py:36
def run_tests(tests)
Definition: rs2_test.py:283
def AccelGetDataDeviceStandStraight(rec_filename)
Definition: rs2_test.py:39
def ImageDepthGetData(rec_filename)
Definition: rs2_test.py:106


realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Mon Mar 1 2021 03:34:04