def write_labels(raw_data_path, frame_id, ego_vehicle_label, tmp_bboxes, pointcloud, index, camera_id, calib_info): ego_vehicle_name = ego_vehicle_label[0] + '_' + ego_vehicle_label[1] dataset_path = (COOK_DATA_PATH / raw_data_path.stem / ego_vehicle_name).as_posix() # dataset_path = 'dataset/' + raw_data_path[4:] + '/' + ego_vehicle_name _raw_data_path = (raw_data_path / ego_vehicle_name).as_posix() # raw_data_path += '/' + ego_vehicle_name sensor_raw_path = os.listdir(_raw_data_path) if not os.path.exists(dataset_path + '/label0' + str(index)): os.makedirs(dataset_path + '/label0' + str(index)) if not os.path.exists(dataset_path + '/image0' + str(index)): os.makedirs(dataset_path + '/image0' + str(index)) if not os.path.exists(dataset_path + '/calib0' + str(index)): os.makedirs(dataset_path + '/calib0' + str(index)) if not os.path.exists(dataset_path + '/velodyne'): os.makedirs(dataset_path + '/velodyne') for _tmp in sensor_raw_path: if str(camera_id) in _tmp and 'label' not in _tmp: image_path = Path(_raw_data_path, _tmp, frame_id[:-3] + 'png').as_posix() # image_path = raw_data_path + '/' + _tmp + '/' + frame_id[:-3] + 'png' break shutil.copy(image_path, dataset_path + '/image0' + str(index)) # np.savetxt(dataset_path + '/calib0'+str(index)+'/' + frame_id, np.array(calib_info), fmt='%s', delimiter=' ') calib = Calibration(dataset_path + '/calib0' + str(index) + '/' + frame_id) for idx, tmp_bbox in enumerate(tmp_bboxes): coordinate = np.array(list(map(float, tmp_bbox[11:14]))).reshape(1, 3) coordinate_camera = calib.lidar_to_rect(coordinate).flatten() tmp_bboxes[idx][11:14] = coordinate_camera np.savetxt(dataset_path + '/label0' + str(index) + '/' + frame_id, np.array(tmp_bboxes), fmt='%s', delimiter=' ') pointcloud = np.array(pointcloud.points, dtype=np.dtype('f4')) # pointcloud[:,2] = pointcloud[:,2] * -1 # points_r = np.zeros((pointcloud.shape[0],1),dtype=np.dtype('f4')) points_R = np.exp(-0.05 * np.sqrt(np.sum(pointcloud**2, axis=1))).reshape( -1, 1) pointcloud = np.concatenate((pointcloud, points_R), axis=1) pointcloud.tofile(dataset_path + '/velodyne/' + frame_id[:-3] + 'bin') return image_path
def get_calib_file(raw_data_path, frame_id, ego_vehicle_label, index, calib_info): ego_vehicle_name = ego_vehicle_label[0] + '_' + ego_vehicle_label[1] dataset_path = (COOK_DATA_PATH / raw_data_path.stem / ego_vehicle_name).as_posix() # dataset_path = 'dataset/' + raw_data_path[4:] + '/' + ego_vehicle_name if not os.path.exists(dataset_path + '/calib0' + str(index)): os.makedirs(dataset_path + '/calib0' + str(index)) np.savetxt(dataset_path + '/calib0' + str(index) + '/' + frame_id, np.array(calib_info), fmt='%s', delimiter=' ') calib = Calibration(dataset_path + '/calib0' + str(index) + '/' + frame_id) return calib
def run(self): test_date = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S') try: # if not Instruments(controller=self).check_instr(): # return if self.type_test == 'FufuiDOBR': # if not self.curr_parent.check_calibration(): # return if not self.system_login(): self.send_msg('w', 'CobhamUtils', 'System login fail', 1) return self.send_com_command('axsh SET NIC eth0 DYNAMIC') self.get_ip() self.curr_test = FufuiDOBR(self) self.db.set_test_type(self.type_test) count = self.curr_parent.w_main.tests_tab.rowCount() tests_queue = self.curr_parent.tests_queue for x in range(0, count): tmp = self.curr_parent.w_main.tests_tab.item(x, 1).text() for i in list(tests_queue.values()): if i[0] == tmp: test = i[1] break is_enable = self.curr_parent.w_main.tests_tab.item( x, 0).checkState() if is_enable == 2: curr_test = self.curr_parent.w_main.tests_tab.item( x, 1).text() while True: result = self.curr_test.start_current_test(test) for_save = { 'type_test': self.type_test, 'system_asis': self.curr_parent.w_main.asis_lbl.text(), 'date_test': test_date, 'meas_name': curr_test, 'meas_func': test, 'status': result } if not result or type(result) == Exception: q = self.send_msg('w', 'Error', 'Test {} fail'.format(test), 3) if q == QMessageBox.Ignore: break if q == QMessageBox.Retry: result = None self.progress_bar_signal.emit(0, 0) continue if q == QMessageBox.Cancel: self.db.save_test_result(for_save) self.test_result_signal.emit(x, result) return else: break self.db.save_test_result(for_save) self.test_result_signal.emit(x, result) if self.type_test == 'FufuMtdi': if not self.system_login(): self.send_msg('w', 'CobhamUtils', 'System login fail', 1) return self.send_com_command('axsh SET NIC eth0 DYNAMIC') self.get_ip() self.curr_test = FufuMtdi(controller=self) self.db.set_test_type(self.type_test) count = self.curr_parent.w_main.tests_tab.rowCount() tests_queue = self.curr_parent.tests_queue for x in range(0, count): tmp = self.curr_parent.w_main.tests_tab.item(x, 1).text() for i in list(tests_queue.values()): if i[0] == tmp: test = i[1] break is_enable = self.curr_parent.w_main.tests_tab.item( x, 0).checkState() if is_enable == 2: curr_test = self.curr_parent.w_main.tests_tab.item( x, 1).text() while True: try: result = self.curr_test.start_current_test( test) except serial.serialutil.SerialException as e: q = self.send_msg('w', 'COM port error', str(e), 5) if q == QMessageBox.Retry: continue # self.logger.info(str(e)) return for_save = { 'type_test': self.type_test, 'system_asis': self.curr_parent.w_main.asis_lbl.text(), 'system_type': self.curr_parent.system_type, 'date_test': test_date, 'meas_name': curr_test, 'meas_func': test, 'status': result } if not result or type(result) == Exception: q = self.send_msg('w', 'Error', 'Test {} fail'.format(test), 3) if q == QMessageBox.Ignore: break if q == QMessageBox.Retry: result = None self.progress_bar_signal.emit(0, 0) continue if q == QMessageBox.Cancel: self.db.save_test_result(for_save) self.test_result_signal.emit(x, result) return else: break print(for_save) # self.db.save_FufuMtdi_result(for_save) self.test_result_signal.emit(x, result) if self.type_test == 'calibration': self.curr_test = Calibration(controller=self) self.curr_test.run_calibration() # except ValueError as e: # if str(e) == 'com_port': return # self.send_msg('w', 'CobhamUtils', str(e), 1) # return # except serial.serialutil.SerialException as e: # self.send_msg('w', 'COM port error', str(e), 1) # self.logger.info(str(e)) # return except Exception as e: traceback_str = ''.join(traceback.format_tb(e.__traceback__)) self.log_signal_arg.emit(traceback_str, -1) self.send_msg('c', 'Error', str(e), 1) self.logger.error(str(e)) self.logger.info(traceback_str) return
class TestController(QtCore.QThread): LOG_FILENAME = './Log/cobham_utils.log' # # logging.basicConfig(filename=LOG_FILENAME, level=logging.ERROR) # logger = logging.FileHandler(LOG_FILENAME) # logger.setLevel(logging.DEBUG) # # logger = logging.getLogger('cobham_utils') # create logger with 'spam_application' logger = logging.getLogger('cobham_utils') logger.setLevel(logging.DEBUG) # create file handler which logs even debug messages fh = logging.FileHandler(LOG_FILENAME) fh.setLevel(logging.DEBUG) # create console handler with a higher log level ch = logging.StreamHandler() ch.setLevel(logging.ERROR) # create formatter and add it to the handlers formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') fh.setFormatter(formatter) ch.setFormatter(formatter) # add the handlers to the logger logger.addHandler(fh) logger.addHandler(ch) log_signal = QtCore.pyqtSignal(str) log_signal_arg = QtCore.pyqtSignal(str, int) timer_signal = QtCore.pyqtSignal(float) msg_signal = QtCore.pyqtSignal(str, str, str, int) msg_img_signal = QtCore.pyqtSignal(str, str) input_signal = QtCore.pyqtSignal(str) set_label_signal = QtCore.pyqtSignal(str, str) check_com_signal = QtCore.pyqtSignal(bool) test_result_signal = QtCore.pyqtSignal(int, bool) progress_bar_signal = QtCore.pyqtSignal(int, int) def __init__(self, parent=None, **kwargs): QtCore.QThread.__init__(self, parent) self.startTime = datetime.datetime.now().timestamp() self.curr_parent = kwargs.get('curr_parent') self.type_test = kwargs.get('type_test') self.settings = CobhamDB().get_all_data('settings') self.login_msg = self.curr_parent.login_msg self.isSystemBoot = False self.curr_test = None self.db = CobhamDB() """ Run selected tests or calibration """ def run(self): test_date = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S') try: # if not Instruments(controller=self).check_instr(): # return if self.type_test == 'FufuiDOBR': # if not self.curr_parent.check_calibration(): # return if not self.system_login(): self.send_msg('w', 'CobhamUtils', 'System login fail', 1) return self.send_com_command('axsh SET NIC eth0 DYNAMIC') self.get_ip() self.curr_test = FufuiDOBR(self) self.db.set_test_type(self.type_test) count = self.curr_parent.w_main.tests_tab.rowCount() tests_queue = self.curr_parent.tests_queue for x in range(0, count): tmp = self.curr_parent.w_main.tests_tab.item(x, 1).text() for i in list(tests_queue.values()): if i[0] == tmp: test = i[1] break is_enable = self.curr_parent.w_main.tests_tab.item( x, 0).checkState() if is_enable == 2: curr_test = self.curr_parent.w_main.tests_tab.item( x, 1).text() while True: result = self.curr_test.start_current_test(test) for_save = { 'type_test': self.type_test, 'system_asis': self.curr_parent.w_main.asis_lbl.text(), 'date_test': test_date, 'meas_name': curr_test, 'meas_func': test, 'status': result } if not result or type(result) == Exception: q = self.send_msg('w', 'Error', 'Test {} fail'.format(test), 3) if q == QMessageBox.Ignore: break if q == QMessageBox.Retry: result = None self.progress_bar_signal.emit(0, 0) continue if q == QMessageBox.Cancel: self.db.save_test_result(for_save) self.test_result_signal.emit(x, result) return else: break self.db.save_test_result(for_save) self.test_result_signal.emit(x, result) if self.type_test == 'FufuMtdi': if not self.system_login(): self.send_msg('w', 'CobhamUtils', 'System login fail', 1) return self.send_com_command('axsh SET NIC eth0 DYNAMIC') self.get_ip() self.curr_test = FufuMtdi(controller=self) self.db.set_test_type(self.type_test) count = self.curr_parent.w_main.tests_tab.rowCount() tests_queue = self.curr_parent.tests_queue for x in range(0, count): tmp = self.curr_parent.w_main.tests_tab.item(x, 1).text() for i in list(tests_queue.values()): if i[0] == tmp: test = i[1] break is_enable = self.curr_parent.w_main.tests_tab.item( x, 0).checkState() if is_enable == 2: curr_test = self.curr_parent.w_main.tests_tab.item( x, 1).text() while True: try: result = self.curr_test.start_current_test( test) except serial.serialutil.SerialException as e: q = self.send_msg('w', 'COM port error', str(e), 5) if q == QMessageBox.Retry: continue # self.logger.info(str(e)) return for_save = { 'type_test': self.type_test, 'system_asis': self.curr_parent.w_main.asis_lbl.text(), 'system_type': self.curr_parent.system_type, 'date_test': test_date, 'meas_name': curr_test, 'meas_func': test, 'status': result } if not result or type(result) == Exception: q = self.send_msg('w', 'Error', 'Test {} fail'.format(test), 3) if q == QMessageBox.Ignore: break if q == QMessageBox.Retry: result = None self.progress_bar_signal.emit(0, 0) continue if q == QMessageBox.Cancel: self.db.save_test_result(for_save) self.test_result_signal.emit(x, result) return else: break print(for_save) # self.db.save_FufuMtdi_result(for_save) self.test_result_signal.emit(x, result) if self.type_test == 'calibration': self.curr_test = Calibration(controller=self) self.curr_test.run_calibration() # except ValueError as e: # if str(e) == 'com_port': return # self.send_msg('w', 'CobhamUtils', str(e), 1) # return # except serial.serialutil.SerialException as e: # self.send_msg('w', 'COM port error', str(e), 1) # self.logger.info(str(e)) # return except Exception as e: traceback_str = ''.join(traceback.format_tb(e.__traceback__)) self.log_signal_arg.emit(traceback_str, -1) self.send_msg('c', 'Error', str(e), 1) self.logger.error(str(e)) self.logger.info(traceback_str) return def system_up_check(self): res = self.send_com_command('') if not self.login_msg in res: match = re.search(r'(Axell-Controller-)(\w{4}\s)(login:)', res) if match: self.system_login() else: t = threading.Thread(target=self.axell_boot) t.start() start = datetime.datetime.now().timestamp() while not self.isSystemBoot: delta = datetime.datetime.now().timestamp() - start self.timer_signal.emit(delta) time.sleep(1) # ToDo: add timeout( return False) return True def axell_boot(self): self.log_signal_arg.emit("Waiting End of Axell boot", 0) with ComPort.get_connection() as ser: while ser.is_open: try: try: out = str(ser.readline(), 'utf-8').replace("\n", "") except: out = ser.readline() if len(out) != 0: # if out not string if not isinstance(out, str): continue self.set_label_signal.emit('console_lbl', out) if 'End of Axell boot' in out: self.isSystemBoot = True self.curr_parent.send_log("Done") time.sleep(3) self.system_login() except serial.SerialException as e: return e def system_login(self): res = self.send_com_command('') print("login -> {}".format(res)) re_string = '({})@[\S]+\[{}\]'.format(self.settings.get('user_name'), self.settings.get('user_name')) if res is None: return if re.search(re_string, res): self.log_signal.emit('System already login ...') return True else: self.log_signal.emit('Login in system ...') self.send_com_command(self.settings.get('user_name')) time.sleep(1) res = self.send_com_command(self.settings.get('user_pass')) if re.search(re_string, res): self.log_signal.emit('Login in system complete') return True else: raise ValueError('Login fail. Fix the problem and try again.') def send_com_command(self, cmd): if cmd == self.settings.get('user_name') or \ cmd == self.settings.get('user_pass') or \ len(cmd) == 0: is_login = True else: is_login = False with ComPort.get_connection() as ser: try: cmd = cmd + '\r' ser.write(cmd.encode('utf-8')) res = ser.read() self.check_com_signal.emit(ser.is_open) repeat = 0 while 1: time.sleep(.5) data_left = ser.inWaiting() res += ser.read(data_left) if data_left == 0: if len(res) == (len(cmd) + 1) and repeat < 20: repeat += 1 continue else: break res = str(res, 'utf-8') # print(res) # if 'timeout end with no input' in res or 'ERROR' in res: # ser.close() # self.send_com_command(cmd_tmp) if '-sh:' in res: # self.send_msg('w', 'Error', res, 1) raise ValueError(res) if res is None: q = self.send_msg( 'q', 'Cobham utils', 'Command "{}" received None.'.format(cmd)) if q == QMessageBox.Retry: self.send_com_command(cmd) if q == QMessageBox.Cancel: pass if not is_login: res = str(res).replace(cmd, '').replace( 'root@AxellShell[root]>', '') return res except Exception as e: # q = self.send_msg('q', 'Cobham utils', str(e), 5) q = self.send_img_msg('Cobham utils', str(e)) if q == QMessageBox.Retry: self.send_com_command(cmd) if q == QMessageBox.Cancel: raise e def get_ip(self): cmd = 'axsh get nic eth0' res = self.send_com_command(cmd).strip().split(' ') ip = res[1] self.set_label_signal.emit('ip_lbl', ip) self.log_signal.emit('ip = {}'.format(ip)) return res def get_bands(self): bands = [] q = self.send_com_command('udp_bridge1 list').split('\n') for i in q: r = re.search('(ABCD\d\d)', i) if r is not None: bands.append(r.group(0)) # if len(bands) != 4: # self.send_msg('w', 'Error', 'Found only {} bands'.format(len(bands)), 1) # self.log_signal.emit(self.send_com_command('udp_bridge1 list')) # input('Press Enter for return...') return bands def get_bands_sn(self): bands = self.get_bands() bands_sn = {} for i, val in enumerate(bands): self.log_signal.emit( 'Getting serial for board with SnapId = {}'.format(val)) if i / 2 < 1: bands_sn.update({val: self.get_band_serial(True, i + 1)}) else: bands_sn.update({val: self.get_band_serial(False, i - 1)}) self.log_signal.emit('Serial number: {}'.format(bands_sn.get(val))) def get_band_serial(self, master, n): print(n) if master: res = self.send_com_command('dobr_partNum GET {}'.format(n)) else: res = self.send_com_command( 'send_msg -d 172.24.30.2 -c dobr_partNum GET {}'.format(n)) match = re.search(r'(part\snumber\s=\s)\w{4}', res) print(res) if match: for_return = match.group().split('=')[1].strip() if for_return is None: self.get_band_serial(master, n) return for_return else: self.get_band_serial(master, n) def send_msg(self, icon, msgTitle, msgText, typeQestions): self.msg_signal.emit(icon, msgTitle, msgText, typeQestions) while self.curr_parent.answer is None: time.sleep(.5) else: for_return = self.curr_parent.answer self.curr_parent.answer = None return for_return def send_img_msg(self, msgText, msgImage): self.msg_img_signal.emit(msgText, msgImage) while self.curr_parent.answer is None: time.sleep(.05) else: for_return = self.curr_parent.answer self.curr_parent.answer = None return for_return @staticmethod def str_to_dict(val): start = val.find('{') stop = 0 for i, j in enumerate(val): if j == '}': stop = i return ast.literal_eval(val[start:stop + 1]) def send_test_name(self, name, status): if status == 'completed': status = status + '\n' self.log_signal.emit('*** {} *** {}'.format(name, status)) @staticmethod def true_to_pass(val): if val: return 'PASS' return 'FAIL' def test(self): print(self.curr_test) with futures.ThreadPoolExecutor(1) as executor: print(self.curr_test) executor.submit(self.curr_test.stop_test)
# print(frame_id) # if not os.path.exists(figure_root+vehicle_id): # os.makedirs(figure_root+vehicle_id) figure_file = figure_root + vehicle_id + '/' + frame_id[:-3] + 'png' gt_file = root_path + '/' + vehicle_id + '/label00/' + frame_id pcd_file = root_path + '/' + vehicle_id + '/velodyne/' + frame_id[: -3] + 'bin' calib_file = root_path + '/' + vehicle_id + '/calib00/' + frame_id pretrain_file = root_path + '/' + vehicle_id + '/federated_test/' + frame_id pretrain_file_ = root_path[: -1] + '/' + vehicle_id + '/federated_test_fusion/' + frame_id # pretrain_file = root_path + '/' + vehicle_id + '/federated_test/' + frame_id pretrain_fusion = root_path + '/dynamic/pretrain_test_fusion/' + vehicle_id + '/' + frame_id pretrain_fusion_file = root_path + '/' + vehicle_id + '/federated_test/' + frame_id calib = Calibration(calib_file) pcd = get_pcd( np.fromfile(pcd_file, dtype=np.dtype('f4'), count=-1).reshape([-1, 4]), calib) gt_label, pretrain_label, pretrain_fusion_label = get_label( gt_file), get_label(pretrain_file), get_label( pretrain_fusion_file) gt_bbox = get_bboxes(gt_label, calib, [1., 0., 0.]) pretrain_bbox = get_bboxes(pretrain_label, calib, [0.4, 0.4, 0.4], False, pcd, pretrain_file_) # print(pretrain_file) pretrain_fusion_bbox = get_bboxes(pretrain_fusion_label, calib, [0.7, 0.7, 0.7]) # print(pretrain_fusion_file) mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=10) # geometry_list = [mesh] + pcd + gt_bbox + pretrain_fusion_bbox# + pretrain_fusion_bbox
def __init__(self, sequence_path, config_file='config/config.yaml'): """ Initialise the class Sequence. This class contains the methods related to access the sensor and annotation information at certain timestamp :type sequence_path: string :param sequence_path: path/to/sequence_root :type config_file: string :param config_file: the path to the configuration files """ self.sequence_path = sequence_path # load annotations self.annotations_path = os.path.join( self.sequence_path, 'annotations', 'annotations.json') self.__load_annotations() # load parameters and calibration file with open(config_file, 'r') as file: self.config = yaml.full_load(file) with open(self.config['calib_file'], 'r') as file: self.calib = yaml.full_load(file) self.config.update(self.calib) # generate calibration matrices from calib file self.calib = Calibration(self.config) # output folder self.output_folder = os.path.join( self.config['output_folder'], os.path.basename(self.sequence_path)) # colors used for display self.colors = {'car': (1, 0, 0), 'bus': (0, 1, 0), 'truck': (0, 0, 1), 'pedestrian': (1.0, 1.0, 0.0), 'van': (1.0, 0.3, 0.0), 'group_of_pedestrians': (1.0, 1.0, 0.3), 'motorbike': (0.0, 1.0, 1.0), 'bicycle': (0.3, 1.0, 1.0), 'vehicle': (1.0, 0.0, 0.0) } # average object height self.heights = {'car': 1.5, 'bus': 3, 'truck': 2.5, 'pedestrian': 1.8, 'van': 2, 'group_of_pedestrians': 1.8, 'motorbike': 1.5, 'bicycle': 1.5, 'vehicle': 1.5 } # load timestamps self.timestamp_camera = self.load_timestamp(os.path.join( self.sequence_path, self.config['camera_timestamp_file'])) self.timestamp_radar = self.load_timestamp(os.path.join( self.sequence_path, self.config['radar_timestamp_file'])) self.timestamp_lidar = self.load_timestamp(os.path.join( self.sequence_path, self.config['lidar_timestamp_file'])) # get minimum timestamp self.init_timestamp = np.min([self.timestamp_camera['time'][0], self.timestamp_lidar['time'][0], self.timestamp_radar['time'][0]]) # get end timestamp self.end_timestamp = np.max([self.timestamp_camera['time'][-1], self.timestamp_lidar['time'][-1], self.timestamp_radar['time'][-1]])
global_bboxes = get_global_bboxes(frame_label) # geometry_list += global_bboxes cluster_data = [] for index, test_id in enumerate(test_list): # if one_frame: # if tmp_vehicle_id not in test_id: # continue # else: # tmp_vehicle_id = test_id # else: # pass test_path = test_list_path + '/' + test_id ego_calib_file, ego_label_file, ego_data_file, ego_pointcloud_file = get_ego_file( test_path, frame_id, task) calib = Calibration(ego_calib_file) ego_location, ego_rotation, ego_vehicle = get_ego_location( test_id[-3:], frame_label) ego_bboxes, ego_cluster_data = get_ego_bboxes( get_ego_data(ego_data_file), ego_location, ego_rotation, calib, color=color[index], ego_vehicle_data=ego_vehicle, index=index) ego_gt_bboxes, __ = get_ego_bboxes(get_ego_data(ego_label_file), ego_location, ego_rotation, calib) cluster_data += ego_cluster_data if visualization_o3d: