def startLoop(self): cliArgs = CliArgs() args = vars(cliArgs.parse_args()) configMan = DepthConfigManager(args) if is_rpi and args['pointcloud']: raise NotImplementedError( "Point cloud visualization is currently not supported on RPI") # these are largely for debug and dev. cmd_file, debug_mode = configMan.getCommandFile() usb2_mode = configMan.getUsb2Mode() # decode_nn and show_nn are functions that are dependent on the neural network that's being run. decode_nn = configMan.decode_nn show_nn = configMan.show_nn # Labels for the current neural network. They are parsed from the blob config file. labels = configMan.labels NN_json = configMan.NN_config # This json file is sent to DepthAI. It communicates what options you'd like to enable and what model you'd like to run. config = configMan.jsonConfig # Create a list of enabled streams () stream_names = [ stream if isinstance(stream, str) else stream['name'] for stream in configMan.stream_list ] enable_object_tracker = 'object_tracker' in stream_names # grab video file, if option exists video_file = configMan.video_file self.device = None if debug_mode: print('Cmd file: ', cmd_file, ' args["device_id"]: ', args['device_id']) self.device = depthai.Device(cmd_file, args['device_id']) else: self.device = depthai.Device(args['device_id'], usb2_mode) print(stream_names) print('Available streams: ' + str(self.device.get_available_streams())) # create the pipeline, here is the first connection with the device p = self.device.create_pipeline(config=config) if p is None: print('Pipeline is not created.') exit(3) nn2depth = self.device.get_nn_to_depth_bbox_mapping() t_start = time() frame_count = {} frame_count_prev = {} nnet_prev = {} nnet_prev["entries_prev"] = {} nnet_prev["nnet_source"] = {} frame_count['nn'] = {} frame_count_prev['nn'] = {} NN_cams = {'rgb', 'left', 'right'} for cam in NN_cams: nnet_prev["entries_prev"][cam] = None nnet_prev["nnet_source"][cam] = None frame_count['nn'][cam] = 0 frame_count_prev['nn'][cam] = 0 stream_windows = [] for s in stream_names: if s == 'previewout': for cam in NN_cams: stream_windows.append(s + '-' + cam) else: stream_windows.append(s) for w in stream_windows: frame_count[w] = 0 frame_count_prev[w] = 0 tracklets = None self.reset_process_wd() time_start = time() def print_packet_info_header(): print( '[hostTimestamp streamName] devTstamp seq camSrc width height Bpp' ) def print_packet_info(packet, stream_name): meta = packet.getMetadata() print("[{:.6f} {:15s}]".format(time() - time_start, stream_name), end='') if meta is not None: source = meta.getCameraName() if stream_name.startswith( 'disparity') or stream_name.startswith('depth'): source += '(rectif)' print(" {:.6f}".format(meta.getTimestamp()), meta.getSequenceNum(), source, end='') print('', meta.getFrameWidth(), meta.getFrameHeight(), meta.getFrameBytesPP(), end='') print() return def keypress_handler(self, key, stream_names): cams = ['rgb', 'mono'] self.cam_idx = getattr(self, 'cam_idx', 0) # default: 'rgb' cam = cams[self.cam_idx] cam_c = depthai.CameraControl.CamId.RGB cam_l = depthai.CameraControl.CamId.LEFT cam_r = depthai.CameraControl.CamId.RIGHT cmd_ae_region = depthai.CameraControl.Command.AE_REGION cmd_exp_comp = depthai.CameraControl.Command.EXPOSURE_COMPENSATION cmd_set_focus = depthai.CameraControl.Command.MOVE_LENS cmd_set_exp = depthai.CameraControl.Command.AE_MANUAL keypress_handler_lut = { ord('f'): lambda: self.device.request_af_trigger(), ord('1'): lambda: self.device.request_af_mode(depthai.AutofocusMode. AF_MODE_AUTO), ord('2'): lambda: self.device.request_af_mode(depthai.AutofocusMode. AF_MODE_CONTINUOUS_VIDEO), # 5,6,7,8,9,0: short example for using ISP 3A controls for Mono cameras ord('5'): lambda: self.device.send_camera_control( cam_l, cmd_ae_region, '0 0 200 200 1'), ord('6'): lambda: self.device.send_camera_control( cam_l, cmd_ae_region, '1000 0 200 200 1'), ord('7'): lambda: self.device.send_camera_control( cam_l, cmd_exp_comp, '-2'), ord('8'): lambda: self.device.send_camera_control( cam_l, cmd_exp_comp, '+2'), ord('9'): lambda: self.device.send_camera_control( cam_r, cmd_exp_comp, '-2'), ord('0'): lambda: self.device.send_camera_control( cam_r, cmd_exp_comp, '+2'), } if key in keypress_handler_lut: keypress_handler_lut[key]() elif key == ord('c'): if 'jpegout' in stream_names: self.device.request_jpeg() else: print( "'jpegout' stream not enabled. Try settings -s jpegout to enable it" ) elif key == ord( 's'): # switch selected camera for manual exposure control self.cam_idx = (self.cam_idx + 1) % len(cams) print("======================= Current camera to control:", cams[self.cam_idx]) # RGB manual focus/exposure controls: # Control: key[dec/inc] min..max # exposure time: i o 1..33333 [us] # sensitivity iso: k l 100..1600 # focus: , . 0..255 [far..near] elif key == ord('i') or key == ord('o') or key == ord( 'k') or key == ord('l'): max_exp_us = int(1000 * 1000 / config['camera'][cam]['fps']) self.rgb_exp = getattr(self, 'rgb_exp', 20000) # initial self.rgb_iso = getattr(self, 'rgb_iso', 800) # initial rgb_iso_step = 50 rgb_exp_step = max_exp_us // 20 # split in 20 steps if key == ord('i'): self.rgb_exp -= rgb_exp_step if key == ord('o'): self.rgb_exp += rgb_exp_step if key == ord('k'): self.rgb_iso -= rgb_iso_step if key == ord('l'): self.rgb_iso += rgb_iso_step if self.rgb_exp < 1: self.rgb_exp = 1 if self.rgb_exp > max_exp_us: self.rgb_exp = max_exp_us if self.rgb_iso < 100: self.rgb_iso = 100 if self.rgb_iso > 1600: self.rgb_iso = 1600 print("===================================", cam, "set exposure:", self.rgb_exp, "iso:", self.rgb_iso) exp_arg = str(self.rgb_exp) + ' ' + str( self.rgb_iso) + ' 33333' if cam == 'rgb': self.device.send_camera_control(cam_c, cmd_set_exp, exp_arg) elif cam == 'mono': self.device.send_camera_control(cam_l, cmd_set_exp, exp_arg) self.device.send_camera_control(cam_r, cmd_set_exp, exp_arg) elif key == ord(',') or key == ord('.'): self.rgb_manual_focus = getattr(self, 'rgb_manual_focus', 200) # initial rgb_focus_step = 3 if key == ord(','): self.rgb_manual_focus -= rgb_focus_step if key == ord('.'): self.rgb_manual_focus += rgb_focus_step if self.rgb_manual_focus < 0: self.rgb_manual_focus = 0 if self.rgb_manual_focus > 255: self.rgb_manual_focus = 255 print( "========================================== RGB set focus:", self.rgb_manual_focus) focus_arg = str(self.rgb_manual_focus) self.device.send_camera_control(cam_c, cmd_set_focus, focus_arg) return for stream in stream_names: if stream in ["disparity", "disparity_color", "depth"]: cv2.namedWindow(stream) trackbar_name = 'Disparity confidence' conf_thr_slider_min = 0 conf_thr_slider_max = 255 cv2.createTrackbar(trackbar_name, stream, conf_thr_slider_min, conf_thr_slider_max, self.on_trackbar_change) cv2.setTrackbarPos(trackbar_name, stream, args['disparity_confidence_threshold']) right_rectified = None pcl_converter = None ops = 0 prevTime = time() if args['verbose']: print_packet_info_header() while self.runThread: # retreive data from the device # data is stored in packets, there are nnet (Neural NETwork) packets which have additional functions for NNet result interpretation self.nnet_packets, self.data_packets = p.get_available_nnet_and_data_packets( blocking=True) ### Uncomment to print ops # ops = ops + 1 # if time() - prevTime > 1.0: # print('OPS: ', ops) # ops = 0 # prevTime = time() packets_len = len(self.nnet_packets) + len(self.data_packets) if packets_len != 0: self.reset_process_wd() else: cur_time = monotonic() if cur_time > wd_cutoff: print("process watchdog timeout") os._exit(10) for _, nnet_packet in enumerate(self.nnet_packets): if args['verbose']: print_packet_info(nnet_packet, 'NNet') meta = nnet_packet.getMetadata() camera = 'rgb' if meta != None: camera = meta.getCameraName() nnet_prev["nnet_source"][camera] = nnet_packet nnet_prev["entries_prev"][camera] = decode_nn(nnet_packet, config=config, NN_json=NN_json) frame_count['metaout'] += 1 frame_count['nn'][camera] += 1 for packet in self.data_packets: window_name = packet.stream_name if packet.stream_name not in stream_names: continue # skip streams that were automatically added if args['verbose']: print_packet_info(packet, packet.stream_name) packetData = packet.getData() if packetData is None: print('Invalid packet data!') continue elif packet.stream_name == 'previewout': meta = packet.getMetadata() camera = 'rgb' if meta != None: camera = meta.getCameraName() window_name = 'previewout-' + camera cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) # the format of previewout image is CHW (Chanel, Height, Width), but OpenCV needs HWC, so we # change shape (3, 300, 300) -> (300, 300, 3) data0 = packetData[0, :, :] data1 = packetData[1, :, :] data2 = packetData[2, :, :] frame = cv2.merge([data0, data1, data2]) if nnet_prev["entries_prev"][camera] is not None: frame = show_nn(nnet_prev["entries_prev"][camera], frame, NN_json=NN_json, config=config) if enable_object_tracker and tracklets is not None: frame = show_tracklets(tracklets, frame, labels) cv2.putText(frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) cv2.putText( frame, "NN fps: " + str(frame_count_prev['nn'][camera]), (2, frame.shape[0] - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0)) cv2.imshow(window_name, frame) elif packet.stream_name in [ 'left', 'right', 'disparity', 'rectified_left', 'rectified_right' ]: frame_bgr = packetData if args['pointcloud'] and packet.stream_name == 'rectified_right': right_rectified = packetData cv2.putText(frame_bgr, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) cv2.putText(frame_bgr, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) if args['draw_bb_depth']: camera = args['cnn_camera'] if packet.stream_name == 'disparity': if camera == 'left_right': camera = 'right' elif camera != 'rgb': camera = packet.getMetadata().getCameraName() if nnet_prev["entries_prev"][camera] is not None: frame_bgr = show_nn( nnet_prev["entries_prev"][camera], frame_bgr, NN_json=NN_json, config=config, nn2depth=nn2depth) cv2.imshow(window_name, frame_bgr) elif packet.stream_name.startswith( 'depth') or packet.stream_name == 'disparity_color': frame = packetData if len(frame.shape) == 2: if frame.dtype == np.uint8: # grayscale cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255)) cv2.putText( frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255)) else: # uint16 if args['pointcloud'] and "depth" in stream_names and "rectified_right" in stream_names and right_rectified is not None: try: from depthai_helpers.projector_3d import PointCloudVisualizer except ImportError as e: raise ImportError( f"\033[1;5;31mError occured when importing PCL projector: {e} \033[0m " ) if pcl_converter is None: pcl_converter = PointCloudVisualizer( self.device.get_right_intrinsic(), 1280, 720) right_rectified = cv2.flip(right_rectified, 1) pcl_converter.rgbd_to_projection( frame, right_rectified) pcl_converter.visualize_pcd() frame = (65535 // frame).astype(np.uint8) # colorize depth map, comment out code below to obtain grayscale frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT) # frame = cv2.applyColorMap(frame, cv2.COLORMAP_JET) cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255) cv2.putText( frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255) else: # bgr cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 255)) cv2.putText( frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255) if args['draw_bb_depth']: camera = args['cnn_camera'] if camera == 'left_right': camera = 'right' if nnet_prev["entries_prev"][camera] is not None: frame = show_nn(nnet_prev["entries_prev"][camera], frame, NN_json=NN_json, config=config, nn2depth=nn2depth) cv2.imshow(window_name, frame) elif packet.stream_name == 'jpegout': jpg = packetData mat = cv2.imdecode(jpg, cv2.IMREAD_COLOR) cv2.imshow('jpegout', mat) elif packet.stream_name == 'video': videoFrame = packetData videoFrame.tofile(video_file) # mjpeg = packetData # mat = cv2.imdecode(mjpeg, cv2.IMREAD_COLOR) # cv2.imshow('mjpeg', mat) elif packet.stream_name == 'color': meta = packet.getMetadata() w = meta.getFrameWidth() h = meta.getFrameHeight() yuv420p = packetData.reshape((h * 3 // 2, w)) bgr = cv2.cvtColor(yuv420p, cv2.COLOR_YUV2BGR_IYUV) scale = configMan.getColorPreviewScale() bgr = cv2.resize(bgr, (int(w * scale), int(h * scale)), interpolation=cv2.INTER_AREA) cv2.putText(bgr, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) cv2.putText(bgr, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) cv2.imshow("color", bgr) elif packet.stream_name == 'meta_d2h': str_ = packet.getDataAsStr() dict_ = json.loads(str_) print( 'meta_d2h Temp', ' CSS:' + '{:6.2f}'.format( dict_['sensors']['temperature']['css']), ' MSS:' + '{:6.2f}'.format( dict_['sensors']['temperature']['mss']), ' UPA:' + '{:6.2f}'.format( dict_['sensors']['temperature']['upa0']), ' DSS:' + '{:6.2f}'.format( dict_['sensors']['temperature']['upa1'])) elif packet.stream_name == 'object_tracker': tracklets = packet.getObjectTracker() frame_count[window_name] += 1 t_curr = time() if t_start + 1.0 < t_curr: t_start = t_curr # print("metaout fps: " + str(frame_count_prev["metaout"])) stream_windows = [] for s in stream_names: if s == 'previewout': for cam in NN_cams: stream_windows.append(s + '-' + cam) frame_count_prev['nn'][cam] = frame_count['nn'][ cam] frame_count['nn'][cam] = 0 else: stream_windows.append(s) for w in stream_windows: frame_count_prev[w] = frame_count[w] frame_count[w] = 0 key = cv2.waitKey(1) if key == ord('q'): break else: keypress_handler(self, key, stream_names) del p # in order to stop the pipeline object should be deleted, otherwise device will continue working. This is required if you are going to add code after the main loop, otherwise you can ommit it. del self.device cv2.destroyAllWindows() # Close video output file if was opened if video_file is not None: video_file.close() print('py: DONE.')
def __init__(self): # note the topic name will change if a device is specified. the name will be <stream>+<device_id>. eg preview3. super().__init__('depthai_subscriber') # setup our params self.paramName = "cliArgs" paramDefault = "" paramDesc = ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description= "arguments that match the command line script depthai-demo.py input" ) self.declare_parameter(self.paramName, paramDefault, paramDesc) # get param my_param = self.get_parameter( self.paramName).get_parameter_value().string_value new_argv = [sys.argv[0]] if my_param is not "": paramList = my_param.split(" ") for param in paramList: new_argv.append(param) # setup argv for parsing. self.old_argv = sys.argv sys.argv = new_argv # parse params cliArgs = CliArgs() self.args = vars(cliArgs.parse_args()) print(self.args) self.configMan = DepthConfigManager(self.args) # "." is not allowed in topic names so we replaced "." with "_" on the server. self.targetDev = self.args['device_id'].replace(".", "_") # get nn2depth params, if necessary. if self.args['draw_bb_depth']: self.nn2depth = self.get_nn2depth_sync(self.targetDev) print(self.nn2depth) # subscribe to topics self.previewSubName = 'preview' + self.targetDev self.previewSub = self.create_subscription(UInt8MultiArray, self.previewSubName, self.preview_callback, 10) self.previewSub # prevent unused variable warning self.leftSubName = 'left' + self.targetDev self.leftSub = self.create_subscription(UInt8MultiArray, self.leftSubName, self.left_callback, 10) self.leftSub # prevent unused variable warning self.rightSubName = 'right' + self.targetDev self.rightSub = self.create_subscription(UInt8MultiArray, self.rightSubName, self.right_callback, 10) self.rightSub # prevent unused variable warning self.disparitySubName = 'disparity' + self.targetDev self.disparitySub = self.create_subscription(UInt8MultiArray, self.disparitySubName, self.disparity_callback, 10) self.disparitySub # prevent unused variable warning self.depthSubName = 'depth' + self.targetDev self.depthSub = self.create_subscription(UInt8MultiArray, self.depthSubName, self.depth_callback, 10) self.depthSub # prevent unused variable warning self.d2hSubName = 'd2h' + self.targetDev self.d2hSub = self.create_subscription(String, self.d2hSubName, self.d2h_callback, 10) self.depthSub # prevent unused variable warning self.metaSubName = 'meta' + self.targetDev self.metaSub = self.create_subscription(String, self.metaSubName, self.meta_callback, 10) self.metaSub # prevent unused variable warning
def __init__(self): super().__init__('depthai_publisher') # setup a cli param for passing in command line arguments. self.cliParamName = "cliArgs" paramDefault = "" paramDesc = ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description= "arguments that match the command line script depthai-demo.py input" ) self.declare_parameter(self.cliParamName, paramDefault, paramDesc) # get cli param my_param = self.get_parameter( self.cliParamName).get_parameter_value().string_value new_argv = [sys.argv[0]] if my_param is not "": paramList = my_param.split(" ") for param in paramList: new_argv.append(param) # setup argv for parsing. self.old_argv = sys.argv sys.argv = new_argv # parse params cliArgs = CliArgs() self.args = vars(cliArgs.parse_args()) print(self.args) configMan = DepthConfigManager(self.args) self.cmd_file, self.debug_mode = configMan.getCommandFile() self.usb2_mode = configMan.getUsb2Mode() self.decode_nn = configMan.decode_nn self.decode_nn_json = configMan.decode_nn_json self.show_nn = configMan.show_nn self.labels = configMan.labels self.NN_json = configMan.NN_config # This json file is sent to DepthAI. It communicates what options you'd like to enable and what model you'd like to run. self.config = configMan.jsonConfig self.targetDev = self.args['device_id'].replace(".", "_") # setup publishers self.previewPublisher = self.create_publisher( UInt8MultiArray, 'preview' + self.targetDev, 10) self.previewmsg = UInt8MultiArray() self.leftPublisher = self.create_publisher(UInt8MultiArray, 'left' + self.targetDev, 10) self.leftmsg = UInt8MultiArray() self.rightPublisher = self.create_publisher(UInt8MultiArray, 'right' + self.targetDev, 10) self.rightmsg = UInt8MultiArray() self.disparityPublisher = self.create_publisher( UInt8MultiArray, 'disparity' + self.targetDev, 10) self.disparitymsg = UInt8MultiArray() self.depthPublisher = self.create_publisher(UInt8MultiArray, 'depth' + self.targetDev, 10) self.depthmsg = UInt8MultiArray() self.d2hPublisher = self.create_publisher(String, 'd2h' + self.targetDev, 10) self.d2hmsg = String() self.nnResultPublisher = self.create_publisher(String, 'meta' + self.targetDev, 10) self.nnmsg = String() # Create a list of enabled streams () self.stream_names = [ stream if isinstance(stream, str) else stream['name'] for stream in configMan.stream_list ] self.enable_object_tracker = 'object_tracker' in self.stream_names
def startLoop(self): cliArgs = CliArgs() args = vars(cliArgs.parse_args()) configMan = DepthConfigManager(args) if is_rpi and args['pointcloud']: raise NotImplementedError("Point cloud visualization is currently not supported on RPI") # these are largely for debug and dev. cmd_file, debug_mode = configMan.getCommandFile() usb2_mode = configMan.getUsb2Mode() # decode_nn and show_nn are functions that are dependent on the neural network that's being run. decode_nn = configMan.decode_nn show_nn = configMan.show_nn # Labels for the current neural network. They are parsed from the blob config file. labels = configMan.labels NN_json = configMan.NN_config # This json file is sent to DepthAI. It communicates what options you'd like to enable and what model you'd like to run. config = configMan.jsonConfig # Create a list of enabled streams () stream_names = [stream if isinstance(stream, str) else stream['name'] for stream in configMan.stream_list] enable_object_tracker = 'object_tracker' in stream_names # grab video file, if option exists video_file = configMan.video_file self.device = None if debug_mode: print('Cmd file: ', cmd_file, ' args["device_id"]: ', args['device_id']) self.device = depthai.Device(cmd_file, args['device_id']) else: self.device = depthai.Device(args['device_id'], usb2_mode) print(stream_names) print('Available streams: ' + str(self.device.get_available_streams())) # create the pipeline, here is the first connection with the device p = self.device.create_pipeline(config=config) if p is None: print('Pipeline is not created.') exit(3) nn2depth = self.device.get_nn_to_depth_bbox_mapping() t_start = time() frame_count = {} frame_count_prev = {} nnet_prev = {} nnet_prev["entries_prev"] = {} nnet_prev["nnet_source"] = {} frame_count['nn'] = {} frame_count_prev['nn'] = {} NN_cams = {'rgb', 'left', 'right'} for cam in NN_cams: nnet_prev["entries_prev"][cam] = None nnet_prev["nnet_source"][cam] = None frame_count['nn'][cam] = 0 frame_count_prev['nn'][cam] = 0 stream_windows = [] for s in stream_names: if s == 'previewout': for cam in NN_cams: stream_windows.append(s + '-' + cam) else: stream_windows.append(s) for w in stream_windows: frame_count[w] = 0 frame_count_prev[w] = 0 tracklets = None self.reset_process_wd() time_start = time() def print_packet_info(packet): meta = packet.getMetadata() print("[{:.6f} {:15s}]".format(time()-time_start, packet.stream_name), end='') if meta is not None: print(" {:.6f}".format(meta.getTimestamp()), meta.getSequenceNum(), end='') if not (packet.stream_name.startswith('disparity') or packet.stream_name.startswith('depth')): print('', meta.getCameraName(), end='') print() return for stream in stream_names: if stream in ["disparity", "disparity_color", "depth"]: cv2.namedWindow(stream) trackbar_name = 'Disparity confidence' conf_thr_slider_min = 0 conf_thr_slider_max = 255 cv2.createTrackbar(trackbar_name, stream, conf_thr_slider_min, conf_thr_slider_max, self.on_trackbar_change) cv2.setTrackbarPos(trackbar_name, stream, args['disparity_confidence_threshold']) right_rectified = None pcl_not_set = True # ops = 0 # prevTime = time() # task = False if is_rpi and args['motor']: """ Insert Code for App 2 Here """ if args['cnn_model'] == "app2": """ Insert Code for App 2 Here """ try: pickle_file_data = read_pickle("face_encodings") except: pickle_file_data = [] self.schedule_task_() if is_rpi: """ Insert Code for App 1 Here """ while self.runThread: # retreive data from the device # data is stored in packets, there are nnet (Neural NETwork) packets which have additional functions for NNet result interpretation self.nnet_packets, self.data_packets = p.get_available_nnet_and_data_packets(blocking=True) detected_label = [] ### Uncomment to print ops # ops = ops + 1 # if time() - prevTime > 1.0: # print('OPS: ', ops) # ops = 0 # prevTime = time() packets_len = len(self.nnet_packets) + len(self.data_packets) if packets_len != 0: self.reset_process_wd() else: cur_time=monotonic() if cur_time > wd_cutoff: print("process watchdog timeout") os._exit(10) for _, nnet_packet in enumerate(self.nnet_packets): if args['verbose']: print_packet_info(nnet_packet) meta = nnet_packet.getMetadata() camera = 'rgb' if meta != None: camera = meta.getCameraName() nnet_prev["nnet_source"][camera] = nnet_packet nnet_prev["entries_prev"][camera] = decode_nn(nnet_packet, config=config, NN_json=NN_json) frame_count['metaout'] += 1 frame_count['nn'][camera] += 1 for packet in self.data_packets: window_name = packet.stream_name if packet.stream_name not in stream_names: continue # skip streams that were automatically added if args['verbose']: print_packet_info(packet) packetData = packet.getData() if packetData is None: print('Invalid packet data!') continue elif packet.stream_name == 'previewout': meta = packet.getMetadata() camera = 'rgb' if meta != None: camera = meta.getCameraName() window_name = 'previewout-' + camera # the format of previewout image is CHW (Chanel, Height, Width), but OpenCV needs HWC, so we # change shape (3, 300, 300) -> (300, 300, 3) data0 = packetData[0,:,:] data1 = packetData[1,:,:] data2 = packetData[2,:,:] frame = cv2.merge([data0, data1, data2]) if nnet_prev["entries_prev"][camera] is not None: try: global task_run_motor, task_play_sound, task_start_led if args['cnn_model2'] == "app5_landmark": """ Insert Code for App 5 Here """ elif args['cnn_model'] == "app4": """ Insert Code for App 4 Here """ elif args['social_distance']: """ Insert Code for App 2 Here """ else: frame, detected_label = show_nn(nnet_prev["entries_prev"][camera], frame, NN_json=NN_json, config=config) print(detected_label) for label in detected_label: if str(label) == "mask" and is_rpi and args['motor'] and task_run_motor: """ Insert Code for App 3 Here """ if str(label) == "person" and args['play_sound'] and task_play_sound: """ Insert Code for App 1 Here """ if is_rpi and args['cnn_model'] == "app1" and str(label) \ """ Insert Code for App 1 Here """ except: frame = show_nn(nnet_prev["entries_prev"][camera], frame, NN_json=NN_json, config=config) if enable_object_tracker and tracklets is not None: frame = show_tracklets(tracklets, frame, labels) #cv2.putText(frame, "fps: " + str(frame_count_prev[window_name]), (0, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0),2) cv2.putText(frame, "NN fps: " + str(frame_count_prev['nn'][camera]), (2, frame.shape[0]-4), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0)) cv2.imshow(window_name, frame) elif packet.stream_name in ['left', 'right', 'disparity', 'rectified_left', 'rectified_right']: frame_bgr = packetData if args['pointcloud'] and packet.stream_name == 'rectified_right': right_rectified = packetData cv2.putText(frame_bgr, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) cv2.putText(frame_bgr, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) camera = None if args['draw_bb_depth']: camera = args['cnn_camera'] if packet.stream_name == 'disparity': if camera == 'left_right': camera = 'right' elif camera != 'rgb': camera = packet.getMetadata().getCameraName() if nnet_prev["entries_prev"][camera] is not None: frame_bgr, detected_label = show_nn(nnet_prev["entries_prev"][camera], frame_bgr, NN_json=NN_json, config=config, nn2depth=nn2depth) cv2.imshow(window_name, frame_bgr) elif packet.stream_name.startswith('depth') or packet.stream_name == 'disparity_color': frame = packetData if len(frame.shape) == 2: if frame.dtype == np.uint8: # grayscale cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255)) cv2.putText(frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255)) else: # uint16 if args['pointcloud'] and "depth" in stream_names and "rectified_right" in stream_names and right_rectified is not None: if pcl_not_set: pcl_converter = PointCloudVisualizer(self.device.get_right_intrinsic(), 1280, 720) pcl_not_set = False right_rectified = cv2.flip(right_rectified, 1) pcd = pcl_converter.rgbd_to_projection(frame, right_rectified) pcl_converter.visualize_pcd() frame = (65535 // frame).astype(np.uint8) #colorize depth map, comment out code below to obtain grayscale frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT) # frame = cv2.applyColorMap(frame, cv2.COLORMAP_JET) cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255) cv2.putText(frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255) else: # bgr cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 255)) cv2.putText(frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255) if args['draw_bb_depth']: camera = args['cnn_camera'] if camera == 'left_right': camera = 'right' if nnet_prev["entries_prev"][camera] is not None: frame, detected_label = show_nn(nnet_prev["entries_prev"][camera], frame, NN_json=NN_json, config=config, nn2depth=nn2depth) cv2.imshow(window_name, frame) elif packet.stream_name == 'jpegout': jpg = packetData mat = cv2.imdecode(jpg, cv2.IMREAD_COLOR) cv2.imshow('jpegout', mat) elif packet.stream_name == 'video': videoFrame = packetData videoFrame.tofile(video_file) #mjpeg = packetData #mat = cv2.imdecode(mjpeg, cv2.IMREAD_COLOR) #cv2.imshow('mjpeg', mat) elif packet.stream_name == 'color': meta = packet.getMetadata() w = meta.getFrameWidth() h = meta.getFrameHeight() yuv420p = packetData.reshape( (h * 3 // 2, w) ) bgr = cv2.cvtColor(yuv420p, cv2.COLOR_YUV2BGR_IYUV) scale = configMan.getColorPreviewScale() bgr = cv2.resize(bgr, ( int(w*scale), int(h*scale) ), interpolation = cv2.INTER_AREA) cv2.putText(bgr, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) cv2.putText(bgr, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0)) cv2.imshow("color", bgr) elif packet.stream_name == 'meta_d2h': str_ = packet.getDataAsStr() dict_ = json.loads(str_) print('meta_d2h Temp', ' CSS:' + '{:6.2f}'.format(dict_['sensors']['temperature']['css']), ' MSS:' + '{:6.2f}'.format(dict_['sensors']['temperature']['mss']), ' UPA:' + '{:6.2f}'.format(dict_['sensors']['temperature']['upa0']), ' DSS:' + '{:6.2f}'.format(dict_['sensors']['temperature']['upa1'])) elif packet.stream_name == 'object_tracker': tracklets = packet.getObjectTracker() frame_count[window_name] += 1 t_curr = time() if t_start + 1.0 < t_curr: t_start = t_curr # print("metaout fps: " + str(frame_count_prev["metaout"])) stream_windows = [] for s in stream_names: if s == 'previewout': for cam in NN_cams: stream_windows.append(s + '-' + cam) frame_count_prev['nn'][cam] = frame_count['nn'][cam] frame_count['nn'][cam] = 0 else: stream_windows.append(s) for w in stream_windows: frame_count_prev[w] = frame_count[w] frame_count[w] = 0 key = cv2.waitKey(1) if key == ord('c'): if 'jpegout' in stream_names == 0: print("'jpegout' stream not enabled. Try settings -s jpegout to enable it") else: self.device.request_jpeg() elif key == ord('f'): self.device.request_af_trigger() elif key == ord('1'): self.device.request_af_mode(depthai.AutofocusMode.AF_MODE_AUTO) elif key == ord('2'): self.device.request_af_mode(depthai.AutofocusMode.AF_MODE_CONTINUOUS_VIDEO) elif key == ord('a'): user_name = input() add_face = True elif key == ord('q'): break del p # in order to stop the pipeline object should be deleted, otherwise device will continue working. This is required if you are going to add code after the main loop, otherwise you can ommit it. del self.device # Close video output file if was opened if video_file is not None: video_file.close() print('py: DONE.')
def start_loop(self): top_left = (137, 204) width = 300 height = 150 bottom_right = (top_left[0] + width, top_left[1] + height) mode = "both" # "3dvis", "wls" or "both" persist = False output_path = "../../oakmower_training_data" persister = Persister(output_path) depth_classifier = DisparityClassifier(top_left, width, height) plane_classifier = PlaneClassifier() motion_evaluator = MotionEvaluator() if mode == "3dvis": streams = ["disparity", 'rectified_right'] fit_plane = True filter_wls = False elif mode == "wls": streams = ["disparity", 'rectified_right'] fit_plane = False filter_wls = True elif mode == "both": streams = [ 'previewout', 'metaout', "disparity", 'rectified_right', ] fit_plane = True filter_wls = True else: streams = ["rectified_right"] fit_plane = False filter_wls = False print("no mode selected") wls_filter = WLSFilter(persister=persister, show_single_windows=self.show_single_windows) rectified_right = None pcl_projector = None detections = [] cliArgs = CliArgs() args = vars(cliArgs.parse_args()) configMan = DepthConfigManager(args) config = configMan.jsonConfig config["streams"] = streams config["camera"]["mono"]["resolution_h"] = 400 config["camera"]["mono"]["fps"] = 10 config["camera"]["rgb"]["fps"] = 10 self.device = depthai.Device(args['device_id'], False) p = self.device.create_pipeline(config=config) if p is None: print('Pipeline is not created.') exit(3) self.reset_process_wd() while self.runThread: nnet_packets, data_packets = p.get_available_nnet_and_data_packets( True) packets_len = len(nnet_packets) + len(data_packets) if packets_len != 0: self.reset_process_wd() else: cur_time = time.monotonic() if cur_time > wd_cutoff: print("process watchdog timeout") os._exit(10) for nnet_packet in nnet_packets: detections = list(nnet_packet.getDetectedObjects()) for packet in data_packets: if packet.stream_name == 'previewout': data = packet.getData() data0 = data[0, :, :] img_h = data0.shape[0] img_w = data0.shape[1] data1 = data[1, :, :] data2 = data[2, :, :] alpha = 0.3 beta = (1.0 - alpha) red_im = np.ones_like(data2) * 255 data2[int(img_h / 2):, :] = cv2.addWeighted( data2[int(img_h / 2):, :], alpha, red_im[int(img_h / 2):, :], beta, 0.0) frame = cv2.merge([data0, data1, data2]) self.status["objects_clear"] = True for detection in detections: pt1 = int(detection.x_min * img_w), int( detection.y_min * img_h) pt2 = int(detection.x_max * img_w), int( detection.y_max * img_h) if pt2[1] < int(img_h / 2) or ( (pt2[0] - pt1[0]) > 0.95 * img_w) or ( (pt2[1] - pt1[1]) > 0.95 * img_h): cv2.rectangle(frame, pt1, pt2, (255, 255, 255), 2) else: cv2.rectangle(frame, pt1, pt2, (0, 0, 255), 2) self.status["objects_clear"] = False objects_frame = np.zeros([400, 640, 3], dtype=np.uint8) objects_frame[54:354, 137:437] = frame if self.show_single_windows: cv2.imshow('cockpit', objects_frame) self.objects_frame = self.label_imgs( objects_frame, "Object detection") if persister is not None: persister.add_preview_bb_free(frame) if packet.stream_name == "rectified_right": rectified_right = packet.getData() rectified_right = cv2.flip(rectified_right, flipCode=1) motion_evaluator.update( np.copy(rectified_right[top_left[1]:bottom_right[1], top_left[0]:bottom_right[0]])) self.status[ "moving_forward"] = motion_evaluator.get_motion() rect_right_bb = wls_filter.update(rectified_right, packet.stream_name) if rect_right_bb is not None: self.rectified_right_frame = self.label_imgs( cv2.cvtColor(rect_right_bb, cv2.COLOR_GRAY2BGR), "Right camera") elif packet.stream_name == "depth" or packet.stream_name == "disparity": if packet.stream_name == "disparity": disparity_frame = packet.getData() if fit_plane: frame_f64 = disparity_frame.astype(np.float64) frame_f64[frame_f64 < 1] = 0.000001 depth = 75 * 883.14 / frame_f64 frame16 = depth.astype(np.uint16) frame16[frame16 < 1] = 65535 depth_frame = frame16 else: depth_frame = np.zeros_like(disparity_frame) else: depth_frame = packet.getData() disparity_frame = None if filter_wls: wls_filter.update(frame=disparity_frame, stream_name=packet.stream_name) if fit_plane: if rectified_right is not None: if pcl_projector is None: fd, path = tempfile.mkstemp(suffix='.json') with os.fdopen(fd, 'w') as tmp: json.dump( { "width": 600, "height": 400, "intrinsic_matrix": [ item for row in self.device. get_right_intrinsic() for item in row ] }, tmp) pcl_projector = PointCloudProjector( path, persister=persister) plane_parameters = pcl_projector.rgbd_to_projection( depth_frame, np.ones_like(rectified_right)) self.status["plane"] = plane_classifier.classify( plane_parameters) pcl_projector.visualize_pcd() depth_frame8 = (65535 // depth_frame).astype(np.uint8) # colorize depth map, comment out code below to obtain grayscale depth_frame8 = cv2.applyColorMap(depth_frame8, cv2.COLORMAP_HOT) depth_frame8 = cv2.rectangle(depth_frame8, top_left, bottom_right, (255, 255, 255), 2) if self.show_single_windows: cv2.imshow("depth", depth_frame8) self.depth_frame = self.label_imgs(depth_frame8, "Depth") if filter_wls: colored_wls = wls_filter.filter() if colored_wls is not None: self.wls_frame = self.label_imgs( colored_wls, "Disparity (filtered using right camera)") if wls_filter.filtered_disp is not None: self.status[ "disparity_clear"] = depth_classifier.classify( wls_filter.filtered_disp) if self.show_overview: comb_frame = np.vstack([ self.visualize_classifications(), np.hstack([self.objects_frame, self.wls_frame]), np.hstack( [self.depth_frame, self.rectified_right_frame]) ]) cv2.imshow("overview", comb_frame) print(self.status) if cv2.waitKey(1) == ord("s"): print("Writing to disk") persister.write_to_disk() persister.entries.clear() if cv2.waitKey(1) == ord("c"): persister.entries.clear() if cv2.waitKey(1) == ord("p"): if persist: print("Accumulating data stopped") persist = False else: print("Accumulating data") persist = True if cv2.waitKey(1) == ord("q"): break del p # in order to stop the pipeline object should be deleted, otherwise device will continue working. This is required if you are going to add code after the main loop, otherwise you can ommit it. del self.device