def getDepthPipeline(FRAME_WIDTH, FRAME_HEIGHT, id=None): pipeline = rs.pipeline() config = rs.config() try: if (id != None): config.enable_device(id) except Exception as e: print(e) config.enable_stream(rs.stream.color, FRAME_WIDTH, FRAME_HEIGHT, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, FRAME_WIDTH, FRAME_HEIGHT, rs.format.z16, 30) config.enable_stream(rs.stream.infrared, 1, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) config.enable_stream(rs.stream.infrared, 2, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) print("[INFO] Starting streaming...") profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() align_to = rs.stream.color align = rs.align(align_to) return pipeline, config, profile, align, depth_scale
def main(): if not os.path.exists(args.directory): os.mkdir(args.directory) print('Created out_dir') try: config = rs.config() rs.config.enable_device_from_file(config, args.input) pipeline = rs.pipeline() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30) pipeline.start(config) i = 0 while True: print("Saving frame:", i) frames = pipeline.wait_for_frames() depth_frame = frames.get_depth_frame() depth_image = np.asanyarray(depth_frame.get_data()) cv.imwrite(args.directory + "/" + str(i).zfill(6) + "depth.png", depth_image) rgb_frame = frames.get_color_frame() rgb_image = np.asanyarray(rgb_frame.get_data()) r = rgb_image[:,:,0].copy() b = rgb_image[:,:,2].copy() rgb_image[:,:,0] = b rgb_image[:,:,2] = r cv.imwrite(args.directory + "/" + str(i).zfill(6) + "rgb.png", rgb_image) i += 1 finally: pass
def angle(): # gets current angle of the realsense anglepip = rs.pipeline() angleconfig = rs.config() angleconfig.enable_stream(rs.stream.accel) anglepip.start(angleconfig) camera_angle = 0 try: while True: f1 = anglepip.wait_for_frames() accel = f1[0].as_motion_frame().get_motion_data() if not accel: print("no frame at cam ") continue accel_angle_x = math.degrees(math.atan2(accel.y, accel.z)) accel_angle_x = int(accel_angle_x) camera_angle= accel_angle_x break finally: anglepip.stop() print("camera angle = " + str(camera_angle)) return camera_angle
def pub_img(): '''Callback function of subscribed topic. Here images get converted and features detected''' global stamp # cap = cv2.VideoCapture(1) pipe = rs.pipeline() config = rs.config() width = 640; height = 480; config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30) config.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30) profile = pipe.start(config) align_to = rs.stream.color align = rs.align(align_to) pub_rgb = rospy.Publisher("/output/image_raw/compressed_rgb", CompressedImage, queue_size=1) pub_depth = rospy.Publisher("/output/image_raw/compressed_depth", CompressedImage, queue_size=1) print('Hey! I started publishing images.') while(True): temp = pipe.wait_for_frames() aligned_frames = align.process(temp) aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() if not aligned_depth_frame or not color_frame: pass buffer = np.zeros((height,width,4)) buffer[:,:,0:3] = (np.asanyarray(color_frame.get_data(),dtype=np.uint8)) buffer[:,:,3] = np.asanyarray(aligned_depth_frame.get_data(),dtype=np.uint8) #### Create CompressedImage #### msg_rgb = CompressedImage() msg_depth = CompressedImage() msg_rgb.header.stamp = rospy.Time.now() msg_depth.header.stamp = msg_rgb.header.stamp#rospy.Time.now() msg_rgb.format = "rgb"#f'{stamp}' msg_depth.format = "depth" msg_rgb.data = np.array(cv2.imencode('.jpg', buffer[:,:,0:3])[1]).tostring() msg_depth.data = np.array(cv2.imencode('.jpg', buffer[:,:,3])[1]).tostring() # Publish depth frame # pub = rospy.Publisher("/output/image_raw/compressed", CompressedImage, queue_size=1) # r = rospy.Rate(10) # try: pub_rgb.publish(msg_rgb) pub_depth.publish(msg_depth) # r.sleep() # except: # print('Could not publish') stamp += 1 if stamp >= 100000: stamp = 0
def initialize_camera(width, height): """ Set up camera pipeline for given resolution """ pipe = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30) config.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30) profile = pipe.start(config) align_to = rs.stream.color align = rs.align(align_to)
def __init__(self, resolution=(640, 480), depth_framerate=6, color_framerate=30, include_depth=False, decimation_magnitude=1, spatial_smooth=True, temporal_smooth=True): self.include_depth = include_depth self.pipeline = rs.pipeline() #Create a config and configure the pipeline to stream # different resolutions of color and depth streams self.config = rs.config() w, h = resolution self.config.enable_stream(rs.stream.color, w, h, rs.format.bgr8, color_framerate) if include_depth: self.config.enable_stream(rs.stream.depth, w, h, rs.format.z16, depth_framerate) # Start streaming self.profile = self.pipeline.start(self.config) # Getting the depth sensor's depth scale (see rs-align example for explanation) self.color_sensor = self.profile.get_device().first_color_sensor() if include_depth: self.depth_sensor = self.profile.get_device().first_depth_sensor() self.depth_scale = self.depth_sensor.get_depth_scale() # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.depth self.align = rs.align(align_to) # We'll want to store the intrinsics once we start getting images. self.aligned_depth_K = None self.filters = [] # Decimation filter if decimation_magnitude > 1: decimate_filter = rs.decimation_filter() decimate_filter.set_option(rs.option.filter_magnitude, decimation_magnitude) self.filters.append(decimate_filter) if spatial_smooth or temporal_smooth: self.filters.append(rs.disparity_transform()) if spatial_smooth: self.filters.append(rs.spatial_filter()) if temporal_smooth: self.filters.append(rs.temporal_filter()) if spatial_smooth or temporal_smooth: self.filters.append(rs.disparity_transform(False)) # Pointcloud conversion utility. self.pc = rs.pointcloud() self.depth_profile = rs.video_stream_profile(self.profile.get_stream(rs.stream.depth)) self.depth_intrinsics = self.depth_profile.get_intrinsics() self.w, self.h = self.depth_intrinsics.width, self.depth_intrinsics.height
def start_camera_stream(): # comment our when not testing in sim... global pipeline, config pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) pipeline.start(config) profile = pipeline.get_active_profile() image_profile = rs.video_stream_profile(profile.get_stream(rs.stream.color)) image_intrinsics = image_profile.get_intrinsics() frame_w, frame_h = image_intrinsics.width, image_intrinsics.height
def enable(self): logger.info("Enabling camera {0}".format(self.camera_id)) self.pipeline = rs.pipeline() config = rs.config() config.enable_device(self.camera_id) config.enable_stream(rs.stream.infrared, 848, 480, rs.format.y8, 60) config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 60) pipeline_profile = self.pipeline.start(config) pipeline_profile.get_device().sensors[0].set_option( rs.option.emitter_enabled, 0) if self.model == 'resnet': self.resn = poser.DetectorResnet((848, 480), self.optimize) elif self.model == 'densenet': self.resn = poser.DetectorDensenet((848, 480), self.optimize) else: raise RuntimeError(f"Unknown model: {self.model}")
def __init__(self, config, board, usestream=None): # initialize realsense specific settings self.pipeline = rs.pipeline() self.realsense_config = rs.config() # FIXME: missing frames when using videostream or too slow processing # https://github.com/IntelRealSense/librealsense/issues/2216 # initialize the base class super().__init__(config, board, usestream) # Use recorded depth and color streams and its configuration # If problems with colors occur, check bgr/rgb channels configurations if usestream is not None: rs.config.enable_device_from_file(self.realsense_config, usestream) self.realsense_config.enable_all_streams() # Configure depth and color streams else: self.realsense_config.enable_stream(rs.stream.depth, self.width, self.height, rs.format.z16, 30) self.realsense_config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, 30) # Create alignment primitive with color as its target stream: self.alignment_stream = rs.align(rs.stream.color) # Start streaming # TODO: optionally rename variable to a more speaking one # FIXME: program ends here without further message try: self.profile = self.pipeline.start(self.realsense_config) # Getting the depth sensor's depth scale depth_sensor = self.profile.get_device().first_depth_sensor() self.depth_scale = depth_sensor.get_depth_scale() self.initialized = True except RuntimeError as e: logger.fatal("camera could not be initialized: {}".format(e)) self.initialized = False # FIXME: follow up? logger.debug("Depth Scale is: {}".format(self.depth_scale))
def __init__(self, width='640', height='480', fps='30'): # Variabels for setup self.width = width self.height = height self.fps = fps self.imgs = [None] self.img_size = 640 self.half = False # Setup self.pipe = rs.pipeline() self.cfg = rs.config() self.cfg.enable_device('036322250716') # self.cfg.enable_device(source) #036322250716 # Start streaming self.profile = self.pipe.start(self.cfg) self.path = rs.pipeline_profile()
def __init__(self, config = None): Camera_Base.__init__(self, config) if(self.data['camera_name'] != "d400"): raise TypeExcept("Camera name is not d400") #Set pipeline and config self.pipeline = rs.pipeline() cfg = rs.config() cfg.enable_stream(rs.stream.color, self.data['color']['width'], self.data['color']['height'], rs.format.bgr8, self.data['color']['fps']) cfg.enable_stream(rs.stream.depth, self.data['depth']['width'], self.data['depth']['height'], rs.format.z16, self.data['depth']['fps']) # Start streaming profile = self.pipeline.start(cfg) depth_sensor = profile.get_device().first_depth_sensor() self.depth_scale = depth_sensor.get_depth_scale() # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color self.align = rs.align(align_to)
def start(): pipeline = rs.pipeline() config = rs.config() print("Start load conf") config.enable_stream(rs.stream.depth, 1024, 768, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() set_short_range(depth_sensor) colorizer = rs.colorizer() print("Conf loaded") align_to = rs.stream.color align = rs.align(align_to) try: while True: color_image, depth_color_image, depth_image = get_color_depth( pipeline, align, colorizer) if color_image is None and color_image is None and color_image is None: continue color_image, depth_color_image, xy0, xy1, idx_to_coordinates = get_right_hand_coords( color_image, depth_color_image) if xy0 is not None or xy1 is not None: z_val_f, z_val_s, m_xy, c_xy, xy0_f, xy1_f, x, y, z = get_filtered_values( depth_image, xy0, xy1) pyautogui.moveTo(int(x), int(3500 - z)) # , duration=0.05 if draw_cam_out(color_image, depth_color_image, xy0_f, xy1_f, c_xy, m_xy): break finally: hands.close() pipeline.stop()
def camera_process(_running, _collision, _buffer): FRAME_WIDTH = 640 FRAME_HEIGHT = 480 pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, FRAME_WIDTH, FRAME_HEIGHT, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, FRAME_WIDTH, FRAME_HEIGHT, rs.format.z16, 30) config.enable_stream(rs.stream.infrared, 1, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) config.enable_stream(rs.stream.infrared, 2, FRAME_WIDTH, FRAME_HEIGHT, rs.format.y8, 30) print("[INFO] Starting streaming...") profile = pipeline.start(config) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() align_to = rs.stream.color align = rs.align(align_to) clipping_distance_in_meters = 1.5 collision_percent = 0.15 clipping_distance = clipping_distance_in_meters / depth_scale while True: frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame( ) # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() frame = frames.get_color_frame() depth_frame = aligned_frames.get_depth_frame( ) # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() ir_frame_1 = frames.get_infrared_frame(1) ir_frame_2 = frames.get_infrared_frame(2) ir_frame_both = frames.get_infrared_frame() # Convert images to numpy arrays color_image = np.asanyarray(color_frame.get_data()) depth_image = np.asanyarray(aligned_depth_frame.get_data()) grey_color = 0 depth_image_3d = np.dstack( (depth_image, depth_image, depth_image)) #depth image is 1 channel, color is 3 channels ir_image_1 = np.asanyarray(ir_frame_1.get_data()) ir_image_1 = cv2.applyColorMap( cv2.convertScaleAbs(ir_image_1, alpha=1), cv2.COLOR_BGRA2GRAY) ir_image_2 = np.asanyarray(ir_frame_2.get_data()) ir_image_2 = cv2.applyColorMap( cv2.convertScaleAbs(ir_image_2, alpha=1), cv2.COLOR_BGRA2GRAY) depth_image = np.asanyarray(depth_frame.get_data()) depth_colormap = cv2.applyColorMap( cv2.convertScaleAbs(depth_image, alpha=0.02), cv2.COLORMAP_JET) scaled_size = (frame.width, frame.height) # expand image dimensions to have shape: [1, None, None, 3] # i.e. a single-column array, where each item in the column has the pixel RGB value image_expanded = np.expand_dims(color_image, axis=0) frame = cv2.cvtColor(np.asanyarray(frame.get_data()), cv2.COLOR_BGR2RGB) bg_removed = np.where( (depth_image_3d > clipping_distance) | (depth_image_3d <= 0), grey_color, ir_image_1) bg_removed[bg_removed != [0, 0, 0]] = 255 white_count = np.count_nonzero(bg_removed == 255) black_count = np.count_nonzero(bg_removed == 0) pixels = bg_removed.shape[0] * bg_removed.shape[1] * bg_removed.shape[2] if (white_count > pixels * collision_percent): _collision.value = True else: _collision.value = False cv2.imshow("bg_removed", bg_removed) t = cv2.waitKey(1) if t == ord('q'): _running.value = False break # _, _buffer.value = cv2.imencode('.jpg', bg_removed) print(type(base64.b64encode(cv2.imencode('.jpg', bg_removed)[1]))) try: _buffer.value = str( base64.b64encode(cv2.imencode('.jpg', bg_removed)[1])) except Exception as e: print(e)
class Camera(): '''相机函数封装''' pipeline = rs.pipeline() config = rs.config() clipping_distance_in_meters = 1.4 # 深度裁剪范围 def init(self): self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) # Start pipeline attempt = 1 # 尝试次数,成功则为-1 while (attempt > 0): try: profile = self.pipeline.start(self.config) except (RuntimeError): print("\033[0;31m未检测到相机!重试%s/5\033[0m" % (attempt)) attempt += 1 if attempt > 5: os._exit(0) else: attempt = 0 while (attempt >= 0): try: self.frames = self.pipeline.wait_for_frames() except (RuntimeError): print("\033[0;31m相机卡死!重试%s/5\033[0m" % attempt) attempt += 1 if attempt > 5: os._exit(0) else: attempt = -1 color_frame = self.frames.get_color_frame() depth_frame = self.frames.get_depth_frame() # 设置相机参数 depth_sensor = profile.get_device().first_depth_sensor() depth_sensor.set_option(rs.option.motion_range, 140) # 最远深度 220 depth_sensor.set_option(rs.option.accuracy, 3) # max=3 depth_sensor.set_option(rs.option.filter_option, 7) # 7 depth_sensor.set_option(rs.option.confidence_threshold, 15) # 15 self.depth_scale = depth_sensor.get_depth_scale() # 用于将深度图像转为以米为单位 # Color Intrinsics intr = color_frame.profile.as_video_stream_profile().intrinsics camera_parameters = { 'fx': intr.fx, 'fy': intr.fy, 'ppx': intr.ppx, 'ppy': intr.ppy, 'height': intr.height, 'width': intr.width } with open('intrinsics.json', 'w') as fp: json.dump(camera_parameters, fp) depth_intr = depth_frame.profile.as_video_stream_profile().intrinsics depth_parameters = { 'fx': depth_intr.fx, 'fy': depth_intr.fy, 'ppx': depth_intr.ppx, 'ppy': depth_intr.ppy, 'height': depth_intr.height, 'width': depth_intr.width } with open('depth_intrinsics.json', 'w') as fp: json.dump(depth_parameters, fp) depth_to_color_extrin = depth_frame.profile.get_extrinsics_to( color_frame.profile) extrin_parameters = { 'rot': depth_to_color_extrin.rotation, 'tran': depth_to_color_extrin.translation } with open('extrinsics.json', 'w') as fp: json.dump(extrin_parameters, fp) align_to = rs.stream.color self.align = rs.align(align_to) for i in range(30): self.frames = self.pipeline.wait_for_frames() def __del__(self): self.pipeline.stop() def capture(self): '''返回:深度图,彩色图''' attempt = 1 while (attempt >= 0): try: self.frames = self.pipeline.wait_for_frames() except (RuntimeError): print("\033[0;31m相机卡死!重试%s/5\033[0m" % attempt) attempt += 1 if attempt > 5: os._exit(0) else: attempt = -1 # 对齐 aligned_frames = self.align.process(self.frames) aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() # Validate that both frames are valid if not aligned_depth_frame or not color_frame: return depth_image = np.asanyarray( aligned_depth_frame.get_data()) * self.depth_scale color_image = np.asanyarray(color_frame.get_data()) depth_image_3d = np.dstack((depth_image, depth_image, depth_image)) depth_clipped_color_image = np.where( (depth_image_3d > self.clipping_distance_in_meters) | (depth_image_3d <= 0), 0, color_image) # 深度距离限制 深度图彩色图交 倒数第二个参数代表黑色 return depth_image, depth_clipped_color_image
def capture(): global img_q, save w, h = 640, 480 # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, w, h, rs.format.z16, 30) config.enable_stream(rs.stream.color, w, h, rs.format.bgr8, 30) # Start streaming profile = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: " , depth_scale) # We will be removing the background of objects more than # clipping_distance_in_meters meters away clipping_distance_in_meters = 1 #1 meter clipping_distance = clipping_distance_in_meters / depth_scale # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color align = rs.align(align_to) thr = 150 l = 10 while True: frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not aligned_depth_frame or not color_frame: continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) grey_color = 153 out = np.zeros((h, w, 4),np.uint8) depth_image_3d = np.dstack((depth_image,depth_image,depth_image)) out[:,:,:3] = np.where((depth_image_3d > clipping_distance) | (depth_image_3d <= 0), 0, color_image) out[:,:,3] = np.where((depth_image > clipping_distance) | (depth_image <= 0), 0, 255) depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) images = np.hstack((color_image, depth_colormap, out[:,:,:3])) if not img_q.full(): img_q.put_nowait(images) if save.value == 1: save_img(out) save.value = 0
# tracks number of attempts to re-acquire a target (if lost) target_locate_attempts = 0 # Holds the size of a potential target's radius target_circle_radius = 0 # info related to last (potential) target sighting last_obj_lon = None last_obj_lat = None last_obj_alt = None last_obj_heading = None last_point = None # center point in pixels # Uncomment below when using actual realsense camera # Configure realsense camera stream pipeline = rs.pipeline() config = rs.config() # construct the argument parse and parse the arguments ap = argparse.ArgumentParser() ap.add_argument("-p", "--prototxt", required=True, help="path to Caffe 'deploy' prototxt file") ap.add_argument("-m", "--model", required=True, help="path to Caffe pre-trained model") ap.add_argument("-c", "--confidence", type=float, default=0.2, help="minimum probability to filter weak detections") # todo: uncomment when deploying... # args = vars(ap.parse_args()) # initialize the list of class labels MobileNet SSD was trained to
def capture(): global img_q, save w, h = 640, 480 # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, w, h, rs.format.z16, 30) config.enable_stream(rs.stream.color, w, h, rs.format.bgr8, 30) # Start streaming pipeline.start(config) pc = rs.pointcloud() decimate = rs.decimation_filter() decimate.set_option(rs.option.filter_magnitude, 2**1) prepare_pc = 10 thr = 150 l = 10 while True: frames = pipeline.wait_for_frames() depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) dd = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_SUMMER) dd = cv2.normalize(dd, None, 0, 255, cv2.NORM_MINMAX) if prepare_pc > 0: prepare_pc -= 1 points = pc.calculate(depth_frame) pc.map_to(color_frame) out = np.zeros((h, w, 4), np.uint8) depth = cv2.cvtColor(dd, cv2.COLOR_BGR2GRAY) ret, mask = cv2.threshold(depth, thr, 255, cv2.THRESH_BINARY_INV) ret, mask2 = cv2.threshold(depth, thr - l, 255, cv2.THRESH_BINARY) mask = cv2.bitwise_and(mask, mask2) out[:, :, :3] = cv2.bitwise_and(color_image, cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)) out[:, :, 3] = mask images = np.vstack( (np.hstack((color_image, dd)), np.hstack((cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR), out[:, :, :3])))) if not img_q.full(): img_q.put_nowait(images) if save.value == 1: save_img(color_image, dd, depth_image) save.value = 0
def capture(): global img_q, save w, h = 640, 480 #w, h = 1280, 720 # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, w, h, rs.format.z16, 30) config.enable_stream(rs.stream.color, w, h, rs.format.bgr8, 30) # Start streaming profile = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) clipping_distance_in_meters = 2 #1 meter clipping_distance = clipping_distance_in_meters / depth_scale frame_count = 0 t = time.time() with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(101, sess) output_stride = model_cfg['output_stride'] while True: frames = pipeline.wait_for_frames() """ aligned_frames = align.process(frames) aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not aligned_depth_frame or not color_frame: continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data())""" depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue depth_image = np.asanyarray( depth_frame.get_data()) #[:, 300 : 900] color_image = np.asanyarray( color_frame.get_data()) #[:, 300 : 900] out = np.zeros((h, w, 3), np.uint8) depth_image_3d = np.dstack((depth_image, depth_image, depth_image)) out = np.where( (depth_image_3d > clipping_distance) | (depth_image_3d <= 0), 0, color_image) mask = np.where( (depth_image > clipping_distance) | (depth_image <= 0), 0, 255) mask = cv2.applyColorMap(cv2.convertScaleAbs(mask, alpha=1), cv2.COLORMAP_BONE) blur_p = 5 mask = cv2.GaussianBlur(mask, (blur_p, blur_p), 0) #mask = np.dstack((mask,mask,mask)) boxes = [] boxes = get_contours(mask.copy()) #boxes = [] depth_colormap = cv2.applyColorMap( cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_BONE) #boxes = HAAR(color_image) keypoints = dict() for box in boxes: img = out[box[1]:box[3], box[0]:box[2]] img2 = color_image[box[1]:box[3], box[0]:box[2]] if img.shape[0] == 0 or img.shape[1] == 0: continue input_image, display_image, output_scale = process_input( img, scale_factor=0.7125, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale tmp = get_coords(img2, depth_image, depth_scale, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) if len(tmp.keys()) > len(keypoints.keys()): keypoints = tmp.copy() if len(boxes) > 0: ask(url, keypoints)
def __init__(self): super().__init__('pcd_publisher_node') self.declare_parameter('frame_name', 'camera_frame') self.declare_parameter('depth_width', 320) self.declare_parameter('depth_height', 240) self.declare_parameter('depth_fps', 30) param_frame_name = self.get_parameter('frame_name') param_depth_width = self.get_parameter('depth_width') param_depth_height = self.get_parameter('depth_height') param_depth_fps = self.get_parameter('depth_fps') # I create a publisher that publishes sensor_msgs.PointCloud2 to the # topic 'pcd'. The value '5' refers to the history_depth, which I # believe is related to the ROS1 concept of queue size. # Read more here: # http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers self.pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, 'rs2_pc', 5) pipeline = rs.pipeline() config = rs.config() # You should modify resolution for stream here. config.enable_stream(rs.stream.depth, param_depth_width.value, param_depth_height.value, rs.format.z16, param_depth_fps.value) profile = pipeline.start(config) frame_count = 0 try: # prevTime = 0 while True: frames = pipeline.wait_for_frames() depth_frame = frames.get_depth_frame() pc = rs.pointcloud() points = pc.calculate(depth_frame) v = points.get_vertices() verts = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz # Filter only valid points (from BaseRealSenseNode::publishPointCloud in librealsense2) # TODO : Speed up filtering valid points with CUDA # verts_valid = [] # depth_frame_data = np.asarray(depth_frame.get_data()) # index_point = 0 # for x in range(depth_frame_data.shape[0]): # for y in range(depth_frame_data.shape[1]): # if(depth_frame_data[x][y] > 0): # verts_valid.append(verts[index_point]) # index_point = index_point + 1 # verts_valid = np.asanyarray(verts_valid) self.pcd = point_cloud(verts, param_frame_name.value) self.pcd_publisher.publish(self.pcd) # curTime = time.time() # sec = curTime - prevTime # fps = 1/(sec) frame_count += 1 if (frame_count % 10 == 0): self.get_logger().info('frame_count : %d' % frame_count) # print(frame_count) # prevTime = curTime finally: pipeline.stop()
def __init__( self, serial: Optional[str] = None, color_img_size: Tuple[int, int] = (1920, 1080), depth_img_size: Tuple[int, int] = (1280, 720), rectification_function: Optional[RectificationFunctionT] = None, logger: Optional[logging.Logger] = None): """Initialises a single Intel RealSense device Args: serial (string, optional): Used to connect to a specific device. Defaults to None. color_img_size (width, height) of color image, must be supported by the camera depth_img_size (width, height) of depth image, must be supported by camera rectification_function (Callable, optional): function to use for image rectification or None to skip rectification Raises: RuntimeError: If no device is found """ self._logger = logger if logger is not None else logging.getLogger( f"IntelRealSense_{serial}") if rectification_function is None: self._logger.warning( "Running camera without distortion parameters. " \ "Images will NOT be undistored by the camera." ) self._rectification_function = lambda x: x else: self._rectification_function = rectification_function if serial is not None: raise NotImplementedError self._context = rs.context() self._device_list = self._context.query_devices() # Check amount of cameras connected if (self._device_list.size() == 0): raise RuntimeError('No devices found. Is it connected?') elif (self._device_list.size() > 1): self._logger.warning( "There is currently {} devices connected." \ "Only the first device will be used!" .format(self._device_list.size()) ) self._device = self._device_list.front() for sensor in self._device.sensors: if sensor.is_depth_sensor(): self._sensor_depth = sensor else: self._sensor_color = sensor self._sensor_color.set_option(rs.option.enable_auto_exposure, 0) self._sensor_color.set_option(rs.option.enable_auto_white_balance, 0) self._pipeline = rs.pipeline() self._config = rs.config() self._config.enable_stream(rs.stream.color, -1, *color_img_size) self._config.enable_stream(rs.stream.depth, -1, *depth_img_size) #self._config.enable_device(self._device.get_info(rs.camera_info.serial_number)) self._profile = self._pipeline.start(self._config) self._parameters = self._load_parameters_from_camera()
def main(): # load serialized caffe model from disk print("[INFO] loading model...") # for now, just directly supply args here... net = cv2.dnn.readNetFromCaffe("MobileNetSSD_deploy.prototxt.txt", "MobileNetSSD_deploy.caffemodel") # net = cv2.dnn.readNetFromCaffe(args["prototxt"], args["model"]) # initialize the video stream, allow the camera sensor to warm up, # and initialize the FPS counter print("[INFO] starting video stream...") # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() if not args.input is None: rs.config.enable_device_from_file(config, args.input) #config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) #config.enable_stream(rs.stream.color, 1280, 800, rs.format.bgr8, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15) # Start streaming pipeline.start(config) fps = FPS().start() # loop over the frames from the video stream if live: while True: # Wait for a coherent pair of frames: depth and color frames = pipeline.wait_for_frames() frame = frames.get_color_frame() if not frame: continue frame = np.asanyarray(frame.get_data()) detect_from_image(frame, net) key = cv2.waitKey(1) & 0xFF # if the `q` key was pressed, break from the loop if key == ord("q"): cv2.imwrite("savedimage.png", frame) break # update the FPS counter fps.update() else: frame = cv2.imread("off_02.png") detect_from_image(frame, net, True) cv2.waitKey(0) # stop the timer and display FPS information fps.stop() print("[INFO] elapsed time: {:.2f}".format(fps.elapsed())) print("[INFO] approx. FPS:q {:.2f}".format(fps.fps())) # do a bit of cleanup cv2.destroyAllWindows() pipeline.stop()
def main(): if not os.path.exists(args.out_rdir): os.mkdir(args.out_rdir) print('Created out_rgb_dir') if not os.path.exists(args.out_ddir): os.mkdir(args.out_ddir) print('Created out_depth_dir') if not os.path.exists(args.annotations_dir): os.mkdir(args.annotations_dir) print('Created args_out_dir') #Create pipeline for realsense2 pipe = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 360, rs.format.rgb8, 30) profile = pipe.start(config) # Declare filters dc_ftr = rs.decimation_filter () # Decimation - reduces depth frame density st_ftr = rs.spatial_filter() # Spatial - edge-preserving spatial smoothing tp_ftr = rs.temporal_filter() # Temporal - reduces temporal noise th_ftr = rs.threshold_filter() align_to = rs.stream.color align = rs.align(align_to) rgbd = np.zeros((1,4,360,640)) i=0 print('################|Initialization seguence completed.|################') try: while(1): # Get frameset of color and depth frames = pipe.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() # 640x480 color_frame = aligned_frames.get_color_frame() # Validate that both frames are valid if not aligned_depth_frame or not color_frame: continue # Post processing filtered = tp_ftr.process(st_ftr.process(dc_ftr.process(aligned_depth_frame))) filtered = th_ftr.process(filtered)#can be removed if person is outside frame rgb = np.asanyarray(color_frame.get_data()) rgb = np.transpose(rgb,(2,0,1)) depth = np.asanyarray(aligned_depth_frame.get_data(),dtype=np.uint16) depth = depth im.show(rgb,args.bb) im.rgbsave(i, rgb, str('./' + args.out_rdir)) im.dsave(i, depth, str('./' + args.out_ddir)) np.savetxt('./' + args.annotations_dir + '/' + str(i) + '.csv', args.bb, delimiter=',') i = i+ 1 print('saved'+str(i)+'image') # time.sleep(1) finally: pipe.stop() print('################| Error in pipeline |################')
def run(self): # ======================== # 1. Configure all streams # ======================== self.rspipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, self.width, self.height, rs.format.z16, 30) config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, 30) # ====================== # 2. Start the streaming # ====================== print("Starting up the Intel Realsense...") print("") profile = self.rspipeline.start(config) # Load the configuration here self.loadConfiguration(profile, self.json_file) # ================================= # 3. The depth sensor's depth scale # ================================= depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: ", depth_scale) print("") # ========================================== # 4. Create an align object. # Align the depth image to the rgb image. # ========================================== align_to = rs.stream.depth align = rs.align(align_to) try: # =========================================== # 5. Skip the first 30 frames. # This gives the Auto-Exposure time to adjust # =========================================== for x in range(30): frames = self.rspipeline.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) print("Intel Realsense started successfully.") print("") # =========================================== # 6. Setup gstreamer # Usefull gst resources / cheatsheets # https://github.com/matthew1000/gstreamer-cheat-sheet/blob/master/rtmp.md # http://wiki.oz9aec.net/index.php/Gstreamer_cheat_sheet # https://github.com/matthew1000/gstreamer-cheat-sheet/blob/master/mixing.md # =========================================== CLI = '' caps = 'caps="video/x-raw,format=BGR,width=' + str( self.width ) + ',height=' + str( self.height * 2 ) + ',framerate=(fraction)30/1,pixel-aspect-ratio=(fraction)1/1"' if platform.system() == "Linux": #assuming Linux means RPI CLI = 'flvmux name=mux streamable=true latency=3000000000 ! rtmpsink location="' + self.rtmp_url + ' live=1 flashver=FME/3.0%20(compatible;%20FMSc%201.0)" \ appsrc name=mysource format=TIME do-timestamp=TRUE is-live=TRUE ' + str( caps) + ' ! \ videoconvert ! omxh264enc ! h264parse ! video/x-h264 ! \ queue max-size-buffers=0 max-size-bytes=0 max-size-time=180000000 min-threshold-buffers=1 leaky=upstream ! mux. \ alsasrc ! audio/x-raw, format=S16LE, rate=44100, channels=1 ! voaacenc bitrate=44100 ! aacparse ! audio/mpeg, mpegversion=4 ! \ queue max-size-buffers=0 max-size-bytes=0 max-size-time=4000000000 min-threshold-buffers=1 ! mux.' elif platform.system() == "Darwin": #macos #CLI='flvmux name=mux streamable=true ! rtmpsink location="'+ self.rtmp_url +' live=1 flashver=FME/3.0%20(compatible;%20FMSc%201.0)" \ # appsrc name=mysource format=TIME do-timestamp=TRUE is-live=TRUE '+ str(caps) +' ! \ # videoconvert ! vtenc_h264 ! video/x-h264 ! h264parse ! video/x-h264 ! \ # queue max-size-buffers=4 ! flvmux name=mux. \ # osxaudiosrc do-timestamp=true ! audioconvert ! audioresample ! audio/x-raw,rate=48000 ! faac bitrate=48000 ! audio/mpeg ! aacparse ! audio/mpeg, mpegversion=4 ! \ # queue max-size-buffers=4 ! mux.' CLI = 'appsrc name=mysource format=TIME do-timestamp=TRUE is-live=TRUE caps="video/x-raw,format=BGR,width=' + str( self.width ) + ',height=' + str( self.height * 2 ) + ',framerate=(fraction)30/1,pixel-aspect-ratio=(fraction)1/1" ! videoconvert ! vtenc_h264 ! video/x-h264 ! h264parse ! video/x-h264 ! queue max-size-buffers=4 ! flvmux name=mux ! rtmpsink location="' + self.rtmp_url + '" sync=true osxaudiosrc do-timestamp=true ! audioconvert ! audioresample ! audio/x-raw,rate=48000 ! faac bitrate=48000 ! audio/mpeg ! aacparse ! audio/mpeg, mpegversion=4 ! queue max-size-buffers=4 ! mux.' #TODO: windows print(CLI) self.gstpipe = Gst.parse_launch(CLI) self.appsrc = self.gstpipe.get_by_name("mysource") self.appsrc.set_property('emit-signals', True) #tell sink to emit signals # Set up a pipeline bus watch to catch errors. self.bus = self.gstpipe.get_bus() self.bus.connect("message", self.on_bus_message) self.gstpipe.set_state(Gst.State.PLAYING) intrinsics = True buff = Gst.Buffer.new_allocate(None, self.width * self.height * 3 * 2, None) buff_depth_index = (self.width * self.height * 3) - 1 hsv = np.zeros((self.height, self.width, 3), dtype=np.float32) hsv8 = np.zeros((self.height, self.width, 3), dtype=np.int8) depth_hsv = np.zeros((self.height, self.width, 3), dtype=np.float32) depth_image = np.zeros((self.height, self.width, 3), dtype=np.float32) start = timer() while not self.exit.is_set(): # ====================================== # 7. Wait for a coherent pair of frames: # ====================================== frames = self.rspipeline.wait_for_frames(1000) framestart = timer() # ======================================= # 8. Align the depth frame to color frame # ======================================= aligned_frames = align.process(frames) # ================================================ # 9. Fetch the depth and colour frames from stream # ================================================ depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() if not depth_frame or not color_frame: pass # print the camera intrinsics just once. it is always the same if intrinsics: print("Intel Realsense Camera Intrinsics: ") print("========================================") print(depth_frame.profile.as_video_stream_profile(). intrinsics) print(color_frame.profile.as_video_stream_profile(). intrinsics) print("") intrinsics = False # ===================================== # 10. Apply filtering to the depth image # ===================================== # Apply a spatial filter without hole_filling (i.e. holes_fill=0) # depth_frame = self.spatial_filtering(depth_frame, magnitude=2, alpha=0.5, delta=10, holes_fill=1) # Apply hole filling filter # depth_frame = self.hole_filling(depth_frame) # ================================== # 11. Convert images to numpy arrays # ================================== depth_image = np.asanyarray(depth_frame.get_data()).astype( np.float32) color_image = np.asanyarray(color_frame.get_data()) # ====================================================================== # 12. Conver depth to hsv # ================================== # We need to encode/pack the 16bit depth value to RGB # we do this by treating it as the Hue in HSV. # we then encode HSV to RGB and stream that # on the other end we reverse RGB to HSV, H will give us the depth value back. # HSV elements are in the 0-1 range so we need to normalize the depth array to 0-1 # First set a far plane and set everything beyond that to 0 clipped = depth_image > 4000 depth_image[clipped] = 0 # Now normalize using that far plane # cv expects the H in degrees, not 0-1 :( depth_image *= (360 / 4000) depth_hsv[:, :, 0] = depth_image depth_hsv[:, :, 1] = 1 depth_hsv[:, :, 2] = 1 discard = depth_image == 0 s = depth_hsv[:, :, 1] v = depth_hsv[:, :, 2] s[discard] = 0 v[discard] = 0 # cv2.cvtColor to convert HSV to RGB hsv = cv2.cvtColor(depth_hsv, cv2.COLOR_HSV2BGR_FULL) # cv2 needs hsv to 8bit (0-255) to stack with the color image hsv8 = (hsv * 255).astype(np.uint8) buff.fill(0, color_image.tobytes()) buff.fill(buff_depth_index, hsv8.tobytes()) self.appsrc.emit("push-buffer", buff) #process any messages from gstreamer msg = self.bus.pop_filtered(Gst.MessageType.ERROR | Gst.MessageType.WARNING | Gst.MessageType.EOS | Gst.MessageType.INFO | Gst.MessageType.STATE_CHANGED) while (msg): self.on_bus_message(msg) msg = self.bus.pop_filtered( Gst.MessageType.ERROR | Gst.MessageType.WARNING | Gst.MessageType.EOS | Gst.MessageType.INFO | Gst.MessageType.STATE_CHANGED) if (not self.exit.is_set()): try: if (not self.previewQueue.full()): self.previewQueue.put_nowait((color_image, hsv8)) except: pass msgprocesstime = timer() print("gstreamer fps:%s d:%s" % (str(1 / (msgprocesstime - start)), str(framestart - start))) start = timer() except: e = sys.exc_info()[0] print("Unexpected Error: %s" % e) self.statusQueue.put_nowait("ERROR: Unexpected Error: %s" % e) finally: # Stop streaming print("Stop realsense pipeline") self.rspipeline.stop() print("Pause gstreamer pipe") try: if (self.gstpipe.get_state()[1] is not Gst.State.PAUSED): self.gstpipe.set_state(Gst.State.PAUSED) except: self.statusQueue.put_nowait("ERROR: Error pausing gstreamer") print("Error pausing gstreamer") self.statusQueue.put_nowait("INFO: Exiting Realsense Capture process") print("Exiting capture loop")
def start(self): try: # Setup: pipe = rs.pipeline() #configure profiles config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 6) config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 6) profile = pipe.start(config) # Skip 5 first frames to give the Auto-Exposure time to adjust for x in range(5): pipe.wait_for_frames() while True: # Store next frameset for later processing: frameset = pipe.wait_for_frames() color_frame = frameset.get_color_frame() color = np.asanyarray(color_frame.get_data()) # Standard OpenCV boilerplate for running the net:# Stand height, width = color.shape[:2] expected = 300 aspect = width / height resized_image = cv2.resize( color, (int(round(expected * aspect)), expected)) crop_start = int(round(expected * (aspect - 1) / 2)) crop_img = resized_image[0:expected, crop_start:crop_start + expected] net = cv2.dnn.readNetFromCaffe( "MobileNetSSD_deploy.prototxt", "MobileNetSSD_deploy.caffemodel") in_scale_factor = 0.007843 mean_val = 127.53 class_names = ("background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor") blob = cv2.dnn.blobFromImage(crop_img, in_scale_factor, (expected, expected), mean_val, False) net.setInput(blob, "data") detections = net.forward("detection_out") label = detections[0, 0, 0, 1] xmin = detections[0, 0, 0, 3] ymin = detections[0, 0, 0, 4] xmax = detections[0, 0, 0, 5] ymax = detections[0, 0, 0, 6] class_name = class_names[int(label)] align = rs.align(rs.stream.color) frameset = align.process(frameset) aligned_depth_frame = frameset.get_depth_frame() depth = np.asanyarray(aligned_depth_frame.get_data()) # Crop depth data: scale = height / expected xmin_depth = int((xmin * expected + crop_start) * scale) ymin_depth = int((ymin * expected) * scale) xmax_depth = int((xmax * expected + crop_start) * scale) ymax_depth = int((ymax * expected) * scale) depth = depth[xmin_depth:xmax_depth, ymin_depth:ymax_depth].astype(float) # Get data scale from the device and convert to meters depth_scale = profile.get_device().first_depth_sensor( ).get_depth_scale() depth = depth * depth_scale dist, _, _, _ = cv2.mean(depth) print("Detected a {0} {1:.3} meters away.".format( class_name, dist)) json_object = json.dumps(DataObject(class_name, dist), default=jsonDefault) self.send_to_hub_callback(json_object) except Exception as e: print(e) pass
def main(queue, testing): if testing: CMDS = { "roll": 1500, "pitch": 1500, "throttle": 1500, "yaw": 1500, "aux1": 1800, "aux2": 1800 } roll_cmd = 1000 while True: roll_cmd = roll_cmd + 1 CMDS["roll"] = roll_cmd queue.put(CMDS) print("commmands sent: ", CMDS) time.sleep(0.1) else: try: # Create a context object. This object owns the handles to all connected realsense devices print("creating RS pipeline...") pipeline = rs.pipeline() # Configure streams print("configure streams...") config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) # try fast stream, narrow vertical FOV print("enable fast stream...") # config.enable_stream(rs.stream.depth, 0, 848, 100, rs.format.z16, 300) #config.enable_stream(rs.stream.depth) # Start streaming print("start streaming...") pipeline.start(config) # y-values to detect distances between detection_lower = 430 detection_upper = 800 det_left = 200 det_right = 400 loop_number = 0 while True: # This call waits until a new coherent set of frames is available on a device # Calls to get_frame_data(...) and get_frame_timestamp(...) on a device will return stable values until wait_for_frames(...) is called loop_number = loop_number + 1 print("realsense loop number: ", loop_number) if loop_number >= 1000: break print("getting frame data...") frames = pipeline.wait_for_frames() depth = frames.get_depth_frame() if not depth: continue depth_image = np.asanyarray(depth.get_data()) scaled_depth = cv2.convertScaleAbs(depth_image, alpha=0.08) truncated_depth = thresholdDepth(scaled_depth) truncated_depth, points = detectBlobs(truncated_depth) # drive by avoiding objects logic # example detections: [(489.6097106933594, 442.31201171875), (364.44451904296875, 405.34393310546875), (376.8372497558594, 278.6385803222656), (132.40438842773438, 347.2413024902344)] # (x, y) detections = fourSectionAvgColor(truncated_depth) if display_image: print("showing image...") depth_colormap = cv2.applyColorMap(scaled_depth, cv2.COLORMAP_JET) cv2.imshow('RealSense', depth_colormap) cv2.imshow('Truncated Depth', truncated_depth) if (sum(detections[1]) <= 20 and sum(detections[2]) <= 20): # drive_func(drive, "0") mode = "MODE: FORWARD" CMDS = { "roll": 1500, "pitch": 1550, "throttle": 1550, "yaw": 1500, "aux1": 1800, "aux2": 1800 } elif sum(detections[1]) >= 20: #drive_func(drive, "1") mode = "MODE: FLY RIGHT" CMDS = { "roll": 1500, "pitch": 1500, "throttle": 1550, "yaw": 1550, "aux1": 1800, "aux2": 1800 } elif sum(detections[2]) >= 20: #drive_func(drive, "2") mode = "MODE: FLY LEFT" CMDS = { "roll": 1500, "pitch": 1500, "throttle": 1550, "yaw": 1450, "aux1": 1800, "aux2": 1800 } elif (sum(detections[1]) >= 20 and sum(detections[2]) >= 20): #drive_func(drive, "4") mode = "MODE: STOPPED" CMDS = { "roll": 1500, "pitch": 1500, "throttle": 1500, "yaw": 1500, "aux1": 1800, "aux2": 1800 } queue.put(CMDS) print("commmands sent: ", CMDS) print(mode) # if detection_lower <= point[1] <= detection_upper: # if det_left <= point[0] <= det_right: # subprocess.run(['python3', 'drive.py', '--direction_index', '0']) # elif det_left <= point[0] <= det_right: # subprocess.run(['python3', 'drive.py', '--direction_index', '0']) k = cv2.waitKey(1) & 0xFF if k == 27: break # done with loop print("finished loop, landing!") CMDS = "land" mode = "MODE: STOPPED" print("commmands sent: ", CMDS) print(mode) queue.put(CMDS) pipeline.stop() exit(0) except KeyboardInterrupt: CMDS = "land" queue.put(CMDS) pipeline.stop() exit(0) except Exception as e: print(e) pass