00001 import os
00002 import sys
00003 from rs2_listener import CWaitForMessage
00004
00005 import rosbag
00006 from cv_bridge import CvBridge, CvBridgeError
00007 import numpy as np
00008 import tf
00009 import itertools
00010 import subprocess
00011 import rospy
00012 import time
00013 import rosservice
00014
00015 global tf_timeout
00016 tf_timeout = 5
00017
00018 def ImuGetData(rec_filename, topic):
00019
00020
00021
00022 bag = rosbag.Bag(rec_filename)
00023 res = dict()
00024 res['value'] = None
00025 res['max_diff'] = [0,0,0]
00026 for topic, msg, t in bag.read_messages(topics=topic):
00027 value = np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z])
00028 if res['value'] is None:
00029 res['value'] = value
00030 else:
00031 diff = abs(value - res['value'])
00032 res['max_diff'] = [max(diff[x], res['max_diff'][x]) for x in range(len(diff))]
00033 res['max_diff'] = np.array(res['max_diff'])
00034 return res
00035
00036 def AccelGetData(rec_filename):
00037 return ImuGetData(rec_filename, '/device_0/sensor_2/Accel_0/imu/data')
00038
00039 def AccelGetDataDeviceStandStraight(rec_filename):
00040 gt_data = AccelGetData(rec_filename)
00041 gt_data['ros_value'] = np.array([0.63839424, 0.05380408, 9.85343552])
00042 gt_data['ros_max_diff'] = np.array([1.97013582e-02, 4.65862500e-09, 4.06165277e-02])
00043 return gt_data
00044
00045 def ImuTest(data, gt_data):
00046
00047
00048 try:
00049 v_data = np.array([data['value'][0].x, data['value'][0].y, data['value'][0].z])
00050 v_gt_data = gt_data['value']
00051 diff = v_data - v_gt_data
00052 max_diff = abs(diff).max()
00053 msg = 'original accel: Expect max diff of %.3f. Got %.3f.' % (gt_data['max_diff'].max(), max_diff)
00054 print msg
00055 if max_diff > gt_data['max_diff'].max():
00056 return False, msg
00057
00058 v_data = data['ros_value'][0]
00059 v_gt_data = gt_data['ros_value']
00060 diff = v_data - v_gt_data
00061 max_diff = abs(diff).max()
00062 msg = 'rotated to ROS: Expect max diff of %.3f. Got %.3f.' % (gt_data['ros_max_diff'].max(), max_diff)
00063 print msg
00064 if max_diff > gt_data['ros_max_diff'].max():
00065 return False, msg
00066 except Exception as e:
00067 msg = '%s' % e
00068 print 'Test Failed: %s' % msg
00069 return False, msg
00070 return True, ''
00071
00072 def ImageGetData(rec_filename, topic):
00073 bag = rosbag.Bag(rec_filename)
00074 bridge = CvBridge()
00075 all_avg = []
00076 ok_percent = []
00077 res = dict()
00078
00079 for topic, msg, t in bag.read_messages(topics=topic):
00080 try:
00081 cv_image = bridge.imgmsg_to_cv2(msg, msg.encoding)
00082 except CvBridgeError as e:
00083 print(e)
00084 continue
00085 pyimg = np.asarray(cv_image)
00086 ok_number = (pyimg != 0).sum()
00087 ok_percent.append(float(ok_number) / (pyimg.shape[0] * pyimg.shape[1]))
00088 all_avg.append(pyimg.sum() / ok_number)
00089
00090 all_avg = np.array(all_avg)
00091 channels = cv_image.shape[2] if len(cv_image.shape) > 2 else 1
00092 res['num_channels'] = channels
00093 res['shape'] = cv_image.shape
00094 res['avg'] = all_avg.mean()
00095 res['ok_percent'] = {'value': (np.array(ok_percent).mean()) / channels, 'epsilon': 0.01}
00096 res['epsilon'] = max(all_avg.max() - res['avg'], res['avg'] - all_avg.min())
00097 res['reported_size'] = [msg.width, msg.height, msg.step]
00098
00099 return res
00100
00101
00102 def ImageColorGetData(rec_filename):
00103 return ImageGetData(rec_filename, '/device_0/sensor_1/Color_0/image/data')
00104
00105
00106 def ImageDepthGetData(rec_filename):
00107 return ImageGetData(rec_filename, '/device_0/sensor_0/Depth_0/image/data')
00108
00109
00110 def ImageDepthInColorShapeGetData(rec_filename):
00111 gt_data = ImageDepthGetData(rec_filename)
00112 color_data = ImageColorGetData(rec_filename)
00113 gt_data['shape'] = color_data['shape'][:2]
00114 gt_data['reported_size'] = color_data['reported_size']
00115 gt_data['reported_size'][2] = gt_data['reported_size'][0]*2
00116 gt_data['ok_percent']['epsilon'] *= 3
00117 return gt_data
00118
00119 def ImageDepthGetData_decimation(rec_filename):
00120 gt_data = ImageDepthGetData(rec_filename)
00121 gt_data['shape'] = [x/2 for x in gt_data['shape']]
00122 gt_data['reported_size'] = [x/2 for x in gt_data['reported_size']]
00123 gt_data['epsilon'] *= 3
00124 return gt_data
00125
00126 def ImageColorTest(data, gt_data):
00127
00128
00129 try:
00130 channels = list(set(data['num_channels']))
00131 msg = 'Expect %d channels. Got %d channels.' % (gt_data['num_channels'], channels[0])
00132 print msg
00133 if len(channels) > 1 or channels[0] != gt_data['num_channels']:
00134 return False, msg
00135 msg = 'Expected all received images to be the same shape. Got %s' % str(set(data['shape']))
00136 print msg
00137 if len(set(data['shape'])) > 1:
00138 return False, msg
00139 msg = 'Expected shape to be %s. Got %s' % (gt_data['shape'], list(set(data['shape']))[0])
00140 print msg
00141 if (np.array(list(set(data['shape']))[0]) != np.array(gt_data['shape'])).any():
00142 return False, msg
00143 msg = 'Expected header [width, height, step] to be %s. Got %s' % (gt_data['reported_size'], list(set(data['reported_size']))[0])
00144 print msg
00145 if (np.array(list(set(data['reported_size']))[0]) != np.array(gt_data['reported_size'])).any():
00146 return False, msg
00147 msg = 'Expect average of %.3f (+-%.3f). Got average of %.3f.' % (gt_data['avg'].mean(), gt_data['epsilon'], np.array(data['avg']).mean())
00148 print msg
00149 if abs(np.array(data['avg']).mean() - gt_data['avg'].mean()) > gt_data['epsilon']:
00150 return False, msg
00151
00152 msg = 'Expect no holes percent > %.3f. Got %.3f.' % (gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon'], np.array(data['ok_percent']).mean())
00153 print msg
00154 if np.array(data['ok_percent']).mean() < gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon']:
00155 return False, msg
00156
00157 except Exception as e:
00158 msg = '%s' % e
00159 print 'Test Failed: %s' % msg
00160 return False, msg
00161 return True, ''
00162
00163
00164 def ImageColorTest_3epsilon(data, gt_data):
00165 gt_data['epsilon'] *= 3
00166 return ImageColorTest(data, gt_data)
00167
00168 def NotImageColorTest(data, gt_data):
00169 res = ImageColorTest(data, gt_data)
00170 return (not res[0], res[1])
00171
00172 def PointCloudTest(data, gt_data):
00173 width = np.array(data['width']).mean()
00174 height = np.array(data['height']).mean()
00175 msg = 'Expect image size %d(+-%d), %d. Got %d, %d.' % (gt_data['width'][0], gt_data['width'][1], gt_data['height'][0], width, height)
00176 print msg
00177 if abs(width - gt_data['width'][0]) > gt_data['width'][1] or height != gt_data['height'][0]:
00178 return False, msg
00179 mean_pos = np.array([xx[:3] for xx in data['avg']]).mean(0)
00180 msg = 'Expect average position of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][:3], gt_data['epsilon'][0], mean_pos)
00181 print msg
00182 if abs(mean_pos - gt_data['avg'][0][:3]).max() > gt_data['epsilon'][0]:
00183 return False, msg
00184 mean_col = np.array([xx[3:] for xx in data['avg']]).mean(0)
00185 msg = 'Expect average color of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][3:], gt_data['epsilon'][1], mean_col)
00186 print msg
00187 if abs(mean_col - gt_data['avg'][0][3:]).max() > gt_data['epsilon'][1]:
00188 return False, msg
00189
00190 return True, ''
00191
00192
00193 def staticTFTest(data, gt_data):
00194 for couple in gt_data.keys():
00195 if data[couple] is None:
00196 msg = 'Tf is None for couple %s' % '->'.join(couple)
00197 return False, msg
00198 if any(abs((np.array(data[couple][0]) - np.array(gt_data[couple][0]))) > 1e-5) or \
00199 any(abs((np.array(data[couple][1]) - np.array(gt_data[couple][1]))) > 1e-5):
00200 msg = 'Tf is changed for couple %s' % '->'.join(couple)
00201 return False, msg
00202 return True, ''
00203
00204 test_types = {'vis_avg': {'listener_theme': 'colorStream',
00205 'data_func': ImageColorGetData,
00206 'test_func': ImageColorTest},
00207 'depth_avg': {'listener_theme': 'depthStream',
00208 'data_func': ImageDepthGetData,
00209 'test_func': ImageColorTest},
00210 'no_file': {'listener_theme': 'colorStream',
00211 'data_func': lambda x: None,
00212 'test_func': NotImageColorTest},
00213 'pointscloud_avg': {'listener_theme': 'pointscloud',
00214
00215
00216 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 123, 124, 113])], 'epsilon': [0.04, 5]},
00217 'test_func': PointCloudTest},
00218 'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1',
00219 'data_func': ImageDepthGetData,
00220 'test_func': ImageColorTest},
00221 'align_depth_color': {'listener_theme': 'alignedDepthColor',
00222 'data_func': ImageDepthInColorShapeGetData,
00223 'test_func': ImageColorTest_3epsilon},
00224 'depth_avg_decimation': {'listener_theme': 'depthStream',
00225 'data_func': ImageDepthGetData_decimation,
00226 'test_func': ImageColorTest},
00227 'align_depth_ir1_decimation': {'listener_theme': 'alignedDepthInfra1',
00228 'data_func': ImageDepthGetData,
00229 'test_func': ImageColorTest},
00230 'static_tf': {'listener_theme': 'static_tf',
00231 'data_func': lambda x: {('camera_link', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]),
00232 ('camera_link', 'camera_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]),
00233 ('camera_link', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]),
00234 ('camera_depth_frame', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]),
00235 ('camera_depth_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]),
00236 ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])}
00237 ,
00238 'test_func': staticTFTest},
00239 'accel_up': {'listener_theme': 'accelStream',
00240 'data_func': AccelGetDataDeviceStandStraight,
00241 'test_func': ImuTest},
00242 }
00243
00244
00245 def run_test(test, listener_res):
00246
00247
00248 test_type = test_types[test['type']]
00249 gt_data = test_type['data_func'](test['params']['rosbag_filename'])
00250 return test_type['test_func'](listener_res[test_type['listener_theme']], gt_data)
00251
00252
00253 def print_results(results):
00254 title = 'TEST RESULTS'
00255 headers = ['index', 'test name', 'score', 'message']
00256 col_0_width = len(headers[0]) + 1
00257 col_1_width = max([len(headers[1])] + [len(test[0]) for test in results]) + 1
00258 col_2_width = max([len(headers[2]), len('OK'), len('FAILED')]) + 1
00259 col_3_width = max([len(headers[3])] + [len(test[1][1]) for test in results]) + 1
00260 total_width = col_0_width + col_1_width + col_2_width + col_3_width
00261 print
00262 print ('{:^%ds}'%total_width).format(title)
00263 print '-'*total_width
00264 print ('{:<%ds}{:<%ds}{:>%ds} : {:<%ds}' % (col_0_width, col_1_width, col_2_width, col_3_width)).format(*headers)
00265 print '-'*(col_0_width-1) + ' '*1 + '-'*(col_1_width-1) + ' '*2 + '-'*(col_2_width-1) + ' '*3 + '-'*(col_3_width-1)
00266 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)])
00267 print
00268
00269
00270 def get_tf(tf_listener, from_id, to_id):
00271 global tf_timeout
00272 try:
00273 start_time = time.time()
00274
00275 tf_listener.waitForTransform(from_id, to_id, rospy.Time(), rospy.Duration(tf_timeout))
00276 res = tf_listener.lookupTransform(from_id, to_id, rospy.Time())
00277 except Exception as e:
00278 res = None
00279 finally:
00280 waited_for = time.time() - start_time
00281 tf_timeout = max(0.0, tf_timeout - waited_for)
00282 return res
00283
00284
00285 def run_tests(tests):
00286 msg_params = {'timeout_secs': 5}
00287 results = []
00288 params_strs = set([test['params_str'] for test in tests])
00289 for params_str in params_strs:
00290 rec_tests = [test for test in tests if test['params_str'] == params_str]
00291 themes = [test_types[test['type']]['listener_theme'] for test in rec_tests]
00292 msg_retriever = CWaitForMessage(msg_params)
00293 print '*'*30
00294 print 'Running the following tests: %s' % ('\n' + '\n'.join([test['name'] for test in rec_tests]))
00295 print '*'*30
00296 num_of_startups = 5
00297 is_node_up = False
00298 for run_no in range(num_of_startups):
00299 print
00300 print '*'*8 + ' Starting ROS ' + '*'*8
00301 print 'running node (%d/%d)' % (run_no, num_of_startups)
00302 cmd_params = ['roslaunch', 'realsense2_camera', 'rs_from_file.launch'] + params_str.split(' ')
00303 print 'running command: ' + ' '.join(cmd_params)
00304 p_wrapper = subprocess.Popen(cmd_params, stdout=None, stderr=None)
00305 time.sleep(2)
00306 service_list = rosservice.get_service_list()
00307 is_node_up = len([service for service in service_list if 'realsense2_camera/' in service]) > 0
00308 if is_node_up:
00309 print 'Node is UP'
00310 break
00311 print 'Node is NOT UP'
00312 print '*'*8 + ' Killing ROS ' + '*'*9
00313 p_wrapper.terminate()
00314 p_wrapper.wait()
00315 print 'DONE'
00316
00317 if is_node_up:
00318 listener_res = msg_retriever.wait_for_messages(themes)
00319 if 'static_tf' in [test['type'] for test in rec_tests]:
00320 print 'Gathering static transforms'
00321 frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose']
00322 tf_listener = tf.TransformListener()
00323 listener_res['static_tf'] = dict([(xx, get_tf(tf_listener, xx[0], xx[1])) for xx in itertools.combinations(frame_ids, 2)])
00324 print '*'*8 + ' Killing ROS ' + '*'*9
00325 p_wrapper.terminate()
00326 p_wrapper.wait()
00327 else:
00328 listener_res = dict([[theme_name, {}] for theme_name in themes])
00329
00330 print '*'*30
00331 print 'DONE run'
00332 print '*'*30
00333
00334 for test in rec_tests:
00335 try:
00336 res = run_test(test, listener_res)
00337 except Exception as e:
00338 print 'Test %s Failed: %s' % (test['name'], e)
00339 res = False, '%s' % e
00340 results.append([test['name'], res])
00341
00342 return results
00343
00344 def main():
00345 all_tests = [{'name': 'non_existent_file', 'type': 'no_file', 'params': {'rosbag_filename': '/home/non_existent_file.txt'}},
00346 {'name': 'vis_avg_2', 'type': 'vis_avg', 'params': {'rosbag_filename': './records/outdoors.bag'}},
00347 {'name': 'depth_avg_1', 'type': 'depth_avg', 'params': {'rosbag_filename': './records/outdoors.bag'}},
00348 {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': './records/outdoors.bag', 'enable_pointcloud': 'true'}},
00349 {'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': './records/outdoors.bag', 'enable_pointcloud': 'true'}},
00350 {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': './records/outdoors.bag', 'align_depth': 'true'}},
00351 {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': './records/outdoors.bag', 'align_depth': 'true'}},
00352 {'name': 'depth_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': './records/outdoors.bag', 'filters': 'decimation', 'align_depth': 'true'}},
00353 {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': './records/outdoors.bag', 'filters': 'decimation'}},
00354 {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': './records/outdoors.bag', 'filters': 'decimation', 'align_depth': 'true'}},
00355
00356 {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag'}},
00357 ]
00358
00359
00360 for test in all_tests:
00361 test['params']['rosbag_filename'] = os.path.abspath(test['params']['rosbag_filename'])
00362 test['params_str'] = ' '.join([key + ':=' + test['params'][key] for key in sorted(test['params'].keys())])
00363
00364 if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv:
00365 print 'USAGE:'
00366 print '------'
00367 print 'rs2_test.py --all | <test_name> [<test_name> [...]]'
00368 print
00369 print 'Available tests are:'
00370 print '\n'.join([test['name'] for test in all_tests])
00371 exit(-1)
00372
00373 if '--all' in sys.argv[1:]:
00374 tests_to_run = all_tests
00375 else:
00376 tests_to_run = [test for test in all_tests if test['name'] in sys.argv[1:]]
00377
00378 results = run_tests(tests_to_run)
00379 print_results(results)
00380
00381 res = int(all([result[1][0] for result in results])) - 1
00382 print 'exit (%d)' % res
00383 exit(res)
00384
00385 if __name__ == '__main__':
00386 main()