def _convert_video_file( input_file, output_file, export_range, world_timestamps, process_frame, timestamp_export_format, ): yield "Export video", 0.0 input_source = File_Source(SimpleNamespace(), input_file, fill_gaps=True) if not input_source.initialised: yield "Exporting video failed", 0.0 return # yield progress results two times per second update_rate = int(input_source.frame_rate / 2) export_start, export_stop = export_range # export_stop is exclusive export_window = pm.exact_window(world_timestamps, (export_start, export_stop - 1)) (export_from_index, export_to_index) = pm.find_closest( input_source.timestamps, export_window ) # NOTE: Start time of the export recording will be synced with world recording # export! This means that if the recording to export started later than the world # video, the first frame of the exported recording will not be at timestamp 0 in # the recording, but later. Some video players (e.g. VLC on windows) might display # the video weirdly in this case, but we rather want syncronization between the # exported video! start_time = export_window[0] writer = MPEG_Writer(output_file, start_time) input_source.seek_to_frame(export_from_index) next_update_idx = export_from_index + update_rate while True: try: input_frame = input_source.get_frame() except EndofVideoError: break if input_frame.index >= export_to_index: break output_img = process_frame(input_source, input_frame) output_frame = input_frame output_frame._img = output_img # it's ._img because .img has no setter writer.write_video_frame(output_frame) if input_source.get_frame_index() >= next_update_idx: progress = (input_source.get_frame_index() - export_from_index) / ( export_to_index - export_from_index ) yield "Exporting video", progress * 100.0 next_update_idx += update_rate writer.close(timestamp_export_format) input_source.cleanup() yield "Exporting video completed", 100.0
class Realsense2_Source(Base_Source): def __init__( self, g_pool, device_id=None, frame_size=DEFAULT_COLOR_SIZE, frame_rate=DEFAULT_COLOR_FPS, depth_frame_size=DEFAULT_DEPTH_SIZE, depth_frame_rate=DEFAULT_DEPTH_FPS, preview_depth=False, device_options=(), record_depth=True, ): logger.debug("_init_ started") super().__init__(g_pool) self._intrinsics = None self.color_frame_index = 0 self.depth_frame_index = 0 self.context = rs.context() self.pipeline = rs.pipeline(self.context) self.pipeline_profile = None self.preview_depth = preview_depth self.record_depth = record_depth self.depth_video_writer = None self._needs_restart = False self.frame_size_backup = DEFAULT_COLOR_SIZE self.depth_frame_size_backup = DEFAULT_DEPTH_SIZE self.frame_rate_backup = DEFAULT_COLOR_FPS self.depth_frame_rate_backup = DEFAULT_DEPTH_FPS self._initialize_device( device_id, frame_size, frame_rate, depth_frame_size, depth_frame_rate, device_options, ) logger.debug("_init_ completed") def _initialize_device( self, device_id, color_frame_size, color_fps, depth_frame_size, depth_fps, device_options=(), ): self.stop_pipeline() self.last_color_frame_ts = None self.last_depth_frame_ts = None self._recent_frame = None self._recent_depth_frame = None if device_id is None: device_id = self.device_id if device_id is None: # FIXME these two if blocks look ugly. return # use default streams to filter modes by rs_stream and rs_format self._available_modes = self._enumerate_formats(device_id) logger.debug("device_id: {} self._available_modes: {}".format( device_id, str(self._available_modes))) if (color_frame_size is not None and depth_frame_size is not None and color_fps is not None and depth_fps is not None): color_frame_size = tuple(color_frame_size) depth_frame_size = tuple(depth_frame_size) logger.debug("Initialize with Color {}@{}\tDepth {}@{}".format( color_frame_size, color_fps, depth_frame_size, depth_fps)) # make sure the frame rates are compatible with the given frame sizes color_fps = self._get_valid_frame_rate(rs.stream.color, color_frame_size, color_fps) depth_fps = self._get_valid_frame_rate(rs.stream.depth, depth_frame_size, depth_fps) self.frame_size_backup = color_frame_size self.depth_frame_size_backup = depth_frame_size self.frame_rate_backup = color_fps self.depth_frame_rate_backup = depth_fps config = self._prep_configuration(color_frame_size, color_fps, depth_frame_size, depth_fps) else: config = self._get_default_config() self.frame_size_backup = DEFAULT_COLOR_SIZE self.depth_frame_size_backup = DEFAULT_DEPTH_SIZE self.frame_rate_backup = DEFAULT_COLOR_FPS self.depth_frame_rate_backup = DEFAULT_DEPTH_FPS try: self.pipeline_profile = self.pipeline.start(config) except RuntimeError as re: logger.error("Cannot start pipeline! " + str(re)) self.pipeline_profile = None else: self.stream_profiles = { s.stream_type(): s.as_video_stream_profile() for s in self.pipeline_profile.get_streams() } logger.debug("Pipeline started for device " + device_id) logger.debug("Stream profiles: " + str(self.stream_profiles)) self._intrinsics = load_intrinsics(self.g_pool.user_dir, self.name, self.frame_size) self.update_menu() self._needs_restart = False def _prep_configuration( self, color_frame_size=None, color_fps=None, depth_frame_size=None, depth_fps=None, ): config = rs.config() # only use these two formats color_format = rs.format.yuyv depth_format = rs.format.z16 config.enable_stream( rs.stream.depth, depth_frame_size[0], depth_frame_size[1], depth_format, depth_fps, ) config.enable_stream( rs.stream.color, color_frame_size[0], color_frame_size[1], color_format, color_fps, ) return config def _get_default_config(self): config = rs.config() # default config is RGB8, we want YUYV config.enable_stream( rs.stream.color, DEFAULT_COLOR_SIZE[0], DEFAULT_COLOR_SIZE[1], rs.format.yuyv, DEFAULT_COLOR_FPS, ) config.enable_stream( rs.stream.depth, DEFAULT_DEPTH_SIZE[0], DEFAULT_DEPTH_SIZE[1], rs.format.z16, DEFAULT_DEPTH_FPS, ) return config def _get_valid_frame_rate(self, stream_type, frame_size, fps): assert stream_type == rs.stream.color or stream_type == rs.stream.depth if not self._available_modes or stream_type not in self._available_modes: logger.warning( "_get_valid_frame_rate: self._available_modes not set yet. Returning default fps." ) if stream_type == rs.stream.color: return DEFAULT_COLOR_FPS elif stream_type == rs.stream.depth: return DEFAULT_DEPTH_FPS else: raise ValueError( "Unexpected `stream_type`: {}".format(stream_type)) if frame_size not in self._available_modes[stream_type]: logger.error( "Frame size not supported for {}: {}. Returning default fps". format(stream_type, frame_size)) if stream_type == rs.stream.color: return DEFAULT_COLOR_FPS elif stream_type == rs.stream.depth: return DEFAULT_DEPTH_FPS if fps not in self._available_modes[stream_type][frame_size]: old_fps = fps rates = [ abs(r - fps) for r in self._available_modes[stream_type][frame_size] ] best_rate_idx = rates.index(min(rates)) fps = self._available_modes[stream_type][frame_size][best_rate_idx] logger.warning( "{} fps is not supported for ({}) for Color Stream. Fallback to {} fps" .format(old_fps, frame_size, fps)) return fps def _enumerate_formats(self, device_id): """Enumerate formats into hierachical structure: streams: resolutions: framerates """ formats = {} if self.context is None: return formats devices = self.context.query_devices() current_device = None for d in devices: try: serial = d.get_info(rs.camera_info.serial_number) except RuntimeError as re: logger.error("Device no longer available " + str(re)) else: if device_id == serial: current_device = d if current_device is None: return formats logger.debug("Found the current device: " + device_id) sensors = current_device.query_sensors() for s in sensors: stream_profiles = s.get_stream_profiles() for sp in stream_profiles: vp = sp.as_video_stream_profile() stream_type = vp.stream_type() if stream_type not in (rs.stream.color, rs.stream.depth): continue elif vp.format() not in (rs.format.z16, rs.format.yuyv): continue formats.setdefault(stream_type, {}) stream_resolution = (vp.width(), vp.height()) formats[stream_type].setdefault(stream_resolution, []).append(vp.fps()) return formats def stop_pipeline(self): if self.online: try: self.pipeline_profile = None self.stream_profiles = None self.pipeline.stop() logger.debug("Pipeline stopped.") except RuntimeError as re: logger.error("Cannot stop the pipeline: " + str(re)) def cleanup(self): if self.depth_video_writer is not None: self.stop_depth_recording() self.stop_pipeline() def get_init_dict(self): return { "frame_size": self.frame_size, "frame_rate": self.frame_rate, "depth_frame_size": self.depth_frame_size, "depth_frame_rate": self.depth_frame_rate, "preview_depth": self.preview_depth, "record_depth": self.record_depth, } def get_frames(self): if self.online: try: frames = self.pipeline.wait_for_frames(TIMEOUT) except RuntimeError as e: logger.error("get_frames: Timeout!") raise RuntimeError(e) else: current_time = self.g_pool.get_timestamp() color = None # if we're expecting color frames if rs.stream.color in self.stream_profiles: color_frame = frames.get_color_frame() last_color_frame_ts = color_frame.get_timestamp() if self.last_color_frame_ts != last_color_frame_ts: self.last_color_frame_ts = last_color_frame_ts color = ColorFrame( np.asanyarray(color_frame.get_data()), current_time, self.color_frame_index, ) self.color_frame_index += 1 depth = None # if we're expecting depth frames if rs.stream.depth in self.stream_profiles: depth_frame = frames.get_depth_frame() last_depth_frame_ts = depth_frame.get_timestamp() if self.last_depth_frame_ts != last_depth_frame_ts: self.last_depth_frame_ts = last_depth_frame_ts depth = DepthFrame( np.asanyarray(depth_frame.get_data()), current_time, self.depth_frame_index, ) self.depth_frame_index += 1 return color, depth return None, None def recent_events(self, events): if self._needs_restart or not self.online: logger.debug("recent_events -> restarting device") self.restart_device() time.sleep(0.01) return try: color_frame, depth_frame = self.get_frames() except RuntimeError as re: logger.warning("Realsense failed to provide frames." + str(re)) self._recent_frame = None self._recent_depth_frame = None self._needs_restart = True else: if color_frame is not None: self._recent_frame = color_frame events["frame"] = color_frame if depth_frame is not None: self._recent_depth_frame = depth_frame events["depth_frame"] = depth_frame if self.depth_video_writer is not None: self.depth_video_writer.write_video_frame(depth_frame) def deinit_ui(self): self.remove_menu() def init_ui(self): self.add_menu() self.menu.label = "Local USB Video Source" self.update_menu() def update_menu(self): logger.debug("update_menu") try: del self.menu[:] except AttributeError: return from pyglui import ui if not self.online: self.menu.append(ui.Info_Text("Capture initialization failed.")) return self.menu.append( ui.Switch("record_depth", self, label="Record Depth Stream")) self.menu.append( ui.Switch("preview_depth", self, label="Preview Depth")) if self._available_modes is not None: def frame_size_selection_getter(): if self.device_id: frame_size = sorted(self._available_modes[rs.stream.color], reverse=True) labels = [ "({}, {})".format(t[0], t[1]) for t in frame_size ] return frame_size, labels else: return [self.frame_size_backup ], [str(self.frame_size_backup)] selector = ui.Selector( "frame_size", self, selection_getter=frame_size_selection_getter, label="Color Resolution", ) self.menu.append(selector) def frame_rate_selection_getter(): if self.device_id: avail_fps = [ fps for fps in self._available_modes[rs.stream.color][ self.frame_size] ] return avail_fps, [str(fps) for fps in avail_fps] else: return [self.frame_rate_backup ], [str(self.frame_rate_backup)] selector = ui.Selector( "frame_rate", self, selection_getter=frame_rate_selection_getter, label="Color Frame Rate", ) self.menu.append(selector) def depth_frame_size_selection_getter(): if self.device_id: depth_sizes = sorted( self._available_modes[rs.stream.depth], reverse=True) labels = [ "({}, {})".format(t[0], t[1]) for t in depth_sizes ] return depth_sizes, labels else: return ( [self.depth_frame_size_backup], [str(self.depth_frame_size_backup)], ) selector = ui.Selector( "depth_frame_size", self, selection_getter=depth_frame_size_selection_getter, label="Depth Resolution", ) self.menu.append(selector) def depth_frame_rate_selection_getter(): if self.device_id: avail_fps = [ fps for fps in self._available_modes[rs.stream.depth][ self.depth_frame_size] ] return avail_fps, [str(fps) for fps in avail_fps] else: return ( [self.depth_frame_rate_backup], [str(self.depth_frame_rate_backup)], ) selector = ui.Selector( "depth_frame_rate", self, selection_getter=depth_frame_rate_selection_getter, label="Depth Frame Rate", ) self.menu.append(selector) def reset_options(): logger.debug("reset_options") self.reset_device(self.device_id) sensor_control = ui.Growing_Menu(label="Sensor Settings") sensor_control.append( ui.Button("Reset device options to default", reset_options)) self.menu.append(sensor_control) else: logger.debug("update_menu: self._available_modes is None") def gl_display(self): if self.preview_depth and self._recent_depth_frame is not None: self.g_pool.image_tex.update_from_ndarray( self._recent_depth_frame.bgr) gl_utils.glFlush() gl_utils.make_coord_system_norm_based() self.g_pool.image_tex.draw() elif self._recent_frame is not None: self.g_pool.image_tex.update_from_yuv_buffer( self._recent_frame.yuv_buffer, self._recent_frame.width, self._recent_frame.height, ) gl_utils.glFlush() gl_utils.make_coord_system_norm_based() self.g_pool.image_tex.draw() if not self.online: super().gl_display() gl_utils.make_coord_system_pixel_based( (self.frame_size[1], self.frame_size[0], 3)) def reset_device(self, device_id): logger.debug("reset_device") if device_id is None: device_id = self.device_id self.notify_all({ "subject": "realsense2_source.restart", "device_id": device_id, "color_frame_size": None, "color_fps": None, "depth_frame_size": None, "depth_fps": None, "device_options": [], # FIXME }) def restart_device( self, color_frame_size=None, color_fps=None, depth_frame_size=None, depth_fps=None, device_options=None, ): if color_frame_size is None: color_frame_size = self.frame_size if color_fps is None: color_fps = self.frame_rate if depth_frame_size is None: depth_frame_size = self.depth_frame_size if depth_fps is None: depth_fps = self.depth_frame_rate if device_options is None: device_options = [] # FIXME self.notify_all({ "subject": "realsense2_source.restart", "device_id": None, "color_frame_size": color_frame_size, "color_fps": color_fps, "depth_frame_size": depth_frame_size, "depth_fps": depth_fps, "device_options": device_options, }) logger.debug("self.restart_device --> self.notify_all") def on_notify(self, notification): logger.debug('self.on_notify, notification["subject"]: ' + notification["subject"]) if notification["subject"] == "realsense2_source.restart": kwargs = notification.copy() del kwargs["subject"] del kwargs["topic"] self._initialize_device(**kwargs) elif notification["subject"] == "recording.started": self.start_depth_recording(notification["rec_path"], notification["start_time_synced"]) elif notification["subject"] == "recording.stopped": self.stop_depth_recording() def start_depth_recording(self, rec_loc, start_time_synced): if not self.record_depth: return if self.depth_video_writer is not None: logger.warning("Depth video recording has been started already") return video_path = os.path.join(rec_loc, "depth.mp4") self.depth_video_writer = MPEG_Writer(video_path, start_time_synced) def stop_depth_recording(self): if self.depth_video_writer is None: logger.warning("Depth video recording was not running") return self.depth_video_writer.close() self.depth_video_writer = None @property def device_id(self): if self.online: # already running return self.pipeline_profile.get_device().get_info( rs.camera_info.serial_number) else: # set the first available device devices = self.context.query_devices() if devices: logger.info("device_id: first device by default.") return devices[0].get_info(rs.camera_info.serial_number) else: logger.debug("device_id: No device connected.") return None @property def frame_size(self): try: stream_profile = self.stream_profiles[rs.stream.color] # TODO check width & height is in self.available modes return stream_profile.width(), stream_profile.height() except AttributeError: return self.frame_size_backup except KeyError: return self.frame_size_backup except TypeError: return self.frame_size_backup @frame_size.setter def frame_size(self, new_size): if new_size != self.frame_size: self.restart_device(color_frame_size=new_size) @property def frame_rate(self): try: stream_profile = self.stream_profiles[rs.stream.color] # TODO check FPS is in self.available modes return stream_profile.fps() except AttributeError: return self.frame_rate_backup except KeyError: return self.frame_rate_backup except TypeError: return self.frame_rate_backup @frame_rate.setter def frame_rate(self, new_rate): if new_rate != self.frame_rate: self.restart_device(color_fps=new_rate) @property def depth_frame_size(self): try: stream_profile = self.stream_profiles[rs.stream.depth] # TODO check width & height is in self.available modes return stream_profile.width(), stream_profile.height() except AttributeError: return self.depth_frame_size_backup except KeyError: return self.depth_frame_size_backup except TypeError: return self.depth_frame_size_backup @depth_frame_size.setter def depth_frame_size(self, new_size): if new_size != self.depth_frame_size: self.restart_device(depth_frame_size=new_size) @property def depth_frame_rate(self): try: stream_profile = self.stream_profiles[rs.stream.depth] return stream_profile.fps() except AttributeError: return self.depth_frame_rate_backup except KeyError: return self.depth_frame_rate_backup except TypeError: return self.depth_frame_rate_backup @depth_frame_rate.setter def depth_frame_rate(self, new_rate): if new_rate != self.depth_frame_rate: self.restart_device(depth_fps=new_rate) @property def jpeg_support(self): return False @property def online(self): return self.pipeline_profile is not None and self.pipeline is not None @property def name(self): if self.online: return self.pipeline_profile.get_device().get_info( rs.camera_info.name) else: logger.debug( "self.name: Realsense2 not online. Falling back to Ghost capture" ) return "Ghost capture"
class Realsense_Source(Base_Source): """ Camera Capture is a class that encapsualtes pyrs.Device: """ def __init__( self, g_pool, device_id=0, frame_size=(1920, 1080), frame_rate=30, depth_frame_size=(640, 480), depth_frame_rate=60, align_streams=False, preview_depth=False, device_options=(), record_depth=True, stream_preset=None, ): super().__init__(g_pool) self._intrinsics = None self.color_frame_index = 0 self.depth_frame_index = 0 self.device = None self.service = pyrs.Service() self.align_streams = align_streams self.preview_depth = preview_depth self.record_depth = record_depth self.depth_video_writer = None self.controls = None self.pitch = 0 self.yaw = 0 self.mouse_drag = False self.last_pos = (0, 0) self.depth_window = None self._needs_restart = False self.stream_preset = stream_preset self._initialize_device( device_id, frame_size, frame_rate, depth_frame_size, depth_frame_rate, device_options, ) def _initialize_device( self, device_id, color_frame_size, color_fps, depth_frame_size, depth_fps, device_options=(), ): devices = tuple(self.service.get_devices()) color_frame_size = tuple(color_frame_size) depth_frame_size = tuple(depth_frame_size) self.streams = [ColorStream(), DepthStream(), PointStream()] self.last_color_frame_ts = None self.last_depth_frame_ts = None self._recent_frame = None self._recent_depth_frame = None if not devices: if not self._needs_restart: logger.error( "Camera failed to initialize. No cameras connected.") self.device = None self.update_menu() return if self.device is not None: self.device.stop() # only call Device.stop() if its context if device_id >= len(devices): logger.error( "Camera with id {} not found. Initializing default camera.". format(device_id)) device_id = 0 # use default streams to filter modes by rs_stream and rs_format self._available_modes = self._enumerate_formats(device_id) # make sure that given frame sizes and rates are available color_modes = self._available_modes[rs_stream.RS_STREAM_COLOR] if color_frame_size not in color_modes: # automatically select highest resolution color_frame_size = sorted(color_modes.keys(), reverse=True)[0] if color_fps not in color_modes[color_frame_size]: # automatically select highest frame rate color_fps = color_modes[color_frame_size][0] depth_modes = self._available_modes[rs_stream.RS_STREAM_DEPTH] if self.align_streams: depth_frame_size = color_frame_size else: if depth_frame_size not in depth_modes: # automatically select highest resolution depth_frame_size = sorted(depth_modes.keys(), reverse=True)[0] if depth_fps not in depth_modes[depth_frame_size]: # automatically select highest frame rate depth_fps = depth_modes[depth_frame_size][0] colorstream = ColorStream( width=color_frame_size[0], height=color_frame_size[1], fps=color_fps, color_format="yuv", preset=self.stream_preset, ) depthstream = DepthStream( width=depth_frame_size[0], height=depth_frame_size[1], fps=depth_fps, preset=self.stream_preset, ) pointstream = PointStream(width=depth_frame_size[0], height=depth_frame_size[1], fps=depth_fps) self.streams = [colorstream, depthstream, pointstream] if self.align_streams: dacstream = DACStream(width=depth_frame_size[0], height=depth_frame_size[1], fps=depth_fps) dacstream.name = "depth" # rename data accessor self.streams.append(dacstream) # update with correctly initialized streams # always initiliazes color + depth, adds rectified/aligned versions as necessary self.device = self.service.Device(device_id, streams=self.streams) self.controls = Realsense_Controls(self.device, device_options) self._intrinsics = load_intrinsics(self.g_pool.user_dir, self.name, self.frame_size) self.update_menu() self._needs_restart = False def _enumerate_formats(self, device_id): """Enumerate formats into hierachical structure: streams: resolutions: framerates """ formats = {} # only lists modes for native streams (RS_STREAM_COLOR/RS_STREAM_DEPTH) for mode in self.service.get_device_modes(device_id): if mode.stream in (rs_stream.RS_STREAM_COLOR, rs_stream.RS_STREAM_DEPTH): # check if frame size dict is available if mode.stream not in formats: formats[mode.stream] = {} stream_obj = next( (s for s in self.streams if s.stream == mode.stream)) if mode.format == stream_obj.format: size = mode.width, mode.height # check if framerate list is already available if size not in formats[mode.stream]: formats[mode.stream][size] = [] formats[mode.stream][size].append(mode.fps) if self.align_streams: depth_sizes = formats[rs_stream.RS_STREAM_DEPTH].keys() color_sizes = formats[rs_stream.RS_STREAM_COLOR].keys() # common_sizes = depth_sizes & color_sizes discarded_sizes = depth_sizes ^ color_sizes for size in discarded_sizes: for sizes in formats.values(): if size in sizes: del sizes[size] return formats def cleanup(self): if self.depth_video_writer is not None: self.stop_depth_recording() if self.device is not None: self.device.stop() self.service.stop() def get_init_dict(self): return { "device_id": self.device.device_id if self.device is not None else 0, "frame_size": self.frame_size, "frame_rate": self.frame_rate, "depth_frame_size": self.depth_frame_size, "depth_frame_rate": self.depth_frame_rate, "preview_depth": self.preview_depth, "record_depth": self.record_depth, "align_streams": self.align_streams, "device_options": self.controls.export_presets() if self.controls is not None else (), "stream_preset": self.stream_preset, } def get_frames(self): if self.device: self.device.wait_for_frames() current_time = self.g_pool.get_timestamp() last_color_frame_ts = self.device.get_frame_timestamp( self.streams[0].stream) if self.last_color_frame_ts != last_color_frame_ts: self.last_color_frame_ts = last_color_frame_ts color = ColorFrame(self.device) color.timestamp = current_time color.index = self.color_frame_index self.color_frame_index += 1 else: color = None last_depth_frame_ts = self.device.get_frame_timestamp( self.streams[1].stream) if self.last_depth_frame_ts != last_depth_frame_ts: self.last_depth_frame_ts = last_depth_frame_ts depth = DepthFrame(self.device) depth.timestamp = current_time depth.index = self.depth_frame_index self.depth_frame_index += 1 else: depth = None return color, depth return None, None def recent_events(self, events): if self._needs_restart: self.restart_device() time.sleep(0.05) elif not self.online: time.sleep(0.05) return try: color_frame, depth_frame = self.get_frames() except (pyrs.RealsenseError, TimeoutError) as err: logger.warning( "Realsense failed to provide frames. Attempting to reinit.") self._recent_frame = None self._recent_depth_frame = None self._needs_restart = True else: if color_frame and depth_frame: self._recent_frame = color_frame events["frame"] = color_frame if depth_frame: self._recent_depth_frame = depth_frame events["depth_frame"] = depth_frame if self.depth_video_writer is not None: self.depth_video_writer.write_video_frame(depth_frame) def deinit_ui(self): self.remove_menu() def init_ui(self): self.add_menu() self.menu.label = "Local USB Video Source" self.update_menu() def update_menu(self): try: del self.menu[:] except AttributeError: return from pyglui import ui if self.device is None: self.menu.append(ui.Info_Text("Capture initialization failed.")) return def align_and_restart(val): self.align_streams = val self.restart_device() self.menu.append( ui.Switch("record_depth", self, label="Record Depth Stream")) self.menu.append( ui.Switch("preview_depth", self, label="Preview Depth")) self.menu.append( ui.Switch("align_streams", self, label="Align Streams", setter=align_and_restart)) def toggle_depth_display(): def on_depth_mouse_button(window, button, action, mods): if button == glfw.GLFW_MOUSE_BUTTON_LEFT and action == glfw.GLFW_PRESS: self.mouse_drag = True if (button == glfw.GLFW_MOUSE_BUTTON_LEFT and action == glfw.GLFW_RELEASE): self.mouse_drag = False if self.depth_window is None: self.pitch = 0 self.yaw = 0 win_size = glfw.glfwGetWindowSize(self.g_pool.main_window) self.depth_window = glfw.glfwCreateWindow( win_size[0], win_size[1], "3D Point Cloud") glfw.glfwSetMouseButtonCallback(self.depth_window, on_depth_mouse_button) active_window = glfw.glfwGetCurrentContext() glfw.glfwMakeContextCurrent(self.depth_window) gl_utils.basic_gl_setup() gl_utils.make_coord_system_norm_based() # refresh speed settings glfw.glfwSwapInterval(0) glfw.glfwMakeContextCurrent(active_window) native_presets = [ ("None", None), ("Best Quality", rs_preset.RS_PRESET_BEST_QUALITY), ("Largest image", rs_preset.RS_PRESET_LARGEST_IMAGE), ("Highest framerate", rs_preset.RS_PRESET_HIGHEST_FRAMERATE), ] def set_stream_preset(val): if self.stream_preset != val: self.stream_preset = val self.restart_device() self.menu.append( ui.Selector( "stream_preset", self, setter=set_stream_preset, labels=[preset[0] for preset in native_presets], selection=[preset[1] for preset in native_presets], label="Stream preset", )) color_sizes = sorted(self._available_modes[rs_stream.RS_STREAM_COLOR], reverse=True) selector = ui.Selector( "frame_size", self, # setter=, selection=color_sizes, label="Resolution" if self.align_streams else "Color Resolution", ) selector.read_only = self.stream_preset is not None self.menu.append(selector) def color_fps_getter(): avail_fps = [ fps for fps in self._available_modes[rs_stream.RS_STREAM_COLOR] [self.frame_size] if self.depth_frame_rate % fps == 0 ] return avail_fps, [str(fps) for fps in avail_fps] selector = ui.Selector( "frame_rate", self, # setter=, selection_getter=color_fps_getter, label="Color Frame Rate", ) selector.read_only = self.stream_preset is not None self.menu.append(selector) if not self.align_streams: depth_sizes = sorted( self._available_modes[rs_stream.RS_STREAM_DEPTH], reverse=True) selector = ui.Selector( "depth_frame_size", self, # setter=, selection=depth_sizes, label="Depth Resolution", ) selector.read_only = self.stream_preset is not None self.menu.append(selector) def depth_fps_getter(): avail_fps = [ fps for fps in self._available_modes[rs_stream.RS_STREAM_DEPTH] [self.depth_frame_size] if fps % self.frame_rate == 0 ] return avail_fps, [str(fps) for fps in avail_fps] selector = ui.Selector( "depth_frame_rate", self, selection_getter=depth_fps_getter, label="Depth Frame Rate", ) selector.read_only = self.stream_preset is not None self.menu.append(selector) def reset_options(): if self.device: try: self.device.reset_device_options_to_default( self.controls.keys()) except pyrs.RealsenseError as err: logger.info("Resetting some device options failed") logger.debug("Reason: {}".format(err)) finally: self.controls.refresh() self.menu.append(ui.Button("Point Cloud Window", toggle_depth_display)) sensor_control = ui.Growing_Menu(label="Sensor Settings") sensor_control.append( ui.Button("Reset device options to default", reset_options)) for ctrl in sorted(self.controls.values(), key=lambda x: x.range.option): # sensor_control.append(ui.Info_Text(ctrl.description)) if (ctrl.range.min == 0.0 and ctrl.range.max == 1.0 and ctrl.range.step == 1.0): sensor_control.append( ui.Switch("value", ctrl, label=ctrl.label, off_val=0.0, on_val=1.0)) else: sensor_control.append( ui.Slider( "value", ctrl, label=ctrl.label, min=ctrl.range.min, max=ctrl.range.max, step=ctrl.range.step, )) self.menu.append(sensor_control) def gl_display(self): from math import floor if self.depth_window is not None and glfw.glfwWindowShouldClose( self.depth_window): glfw.glfwDestroyWindow(self.depth_window) self.depth_window = None if self.depth_window is not None and self._recent_depth_frame is not None: active_window = glfw.glfwGetCurrentContext() glfw.glfwMakeContextCurrent(self.depth_window) win_size = glfw.glfwGetFramebufferSize(self.depth_window) gl_utils.adjust_gl_view(win_size[0], win_size[1]) pos = glfw.glfwGetCursorPos(self.depth_window) if self.mouse_drag: self.pitch = np.clip(self.pitch + (pos[1] - self.last_pos[1]), -80, 80) self.yaw = np.clip(self.yaw - (pos[0] - self.last_pos[0]), -120, 120) self.last_pos = pos glClearColor(0, 0, 0, 0) glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glMatrixMode(GL_PROJECTION) glLoadIdentity() gluPerspective(60, win_size[0] / win_size[1], 0.01, 20.0) glMatrixMode(GL_MODELVIEW) glLoadIdentity() gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0) glTranslatef(0, 0, 0.5) glRotated(self.pitch, 1, 0, 0) glRotated(self.yaw, 0, 1, 0) glTranslatef(0, 0, -0.5) # glPointSize(2) glEnable(GL_DEPTH_TEST) extrinsics = self.device.get_device_extrinsics( rs_stream.RS_STREAM_DEPTH, rs_stream.RS_STREAM_COLOR) depth_frame = self._recent_depth_frame color_frame = self._recent_frame depth_scale = self.device.depth_scale glEnableClientState(GL_VERTEX_ARRAY) pointcloud = self.device.pointcloud glVertexPointer(3, GL_FLOAT, 0, pointcloud) glEnableClientState(GL_COLOR_ARRAY) depth_to_color = np.zeros( depth_frame.height * depth_frame.width * 3, np.uint8) rsutilwrapper.project_pointcloud_to_pixel( depth_to_color, self.device.depth_intrinsics, self.device.color_intrinsics, extrinsics, pointcloud, self._recent_frame.bgr, ) glColorPointer(3, GL_UNSIGNED_BYTE, 0, depth_to_color) glDrawArrays(GL_POINTS, 0, depth_frame.width * depth_frame.height) gl_utils.glFlush() glDisable(GL_DEPTH_TEST) # gl_utils.make_coord_system_norm_based() glfw.glfwSwapBuffers(self.depth_window) glfw.glfwMakeContextCurrent(active_window) if self.preview_depth and self._recent_depth_frame is not None: self.g_pool.image_tex.update_from_ndarray( self._recent_depth_frame.bgr) gl_utils.glFlush() gl_utils.make_coord_system_norm_based() self.g_pool.image_tex.draw() elif self._recent_frame is not None: self.g_pool.image_tex.update_from_yuv_buffer( self._recent_frame.yuv_buffer, self._recent_frame.width, self._recent_frame.height, ) gl_utils.glFlush() gl_utils.make_coord_system_norm_based() self.g_pool.image_tex.draw() if not self.online: super().gl_display() gl_utils.make_coord_system_pixel_based( (self.frame_size[1], self.frame_size[0], 3)) def restart_device( self, device_id=None, color_frame_size=None, color_fps=None, depth_frame_size=None, depth_fps=None, device_options=None, ): if device_id is None: if self.device is not None: device_id = self.device.device_id else: device_id = 0 if color_frame_size is None: color_frame_size = self.frame_size if color_fps is None: color_fps = self.frame_rate if depth_frame_size is None: depth_frame_size = self.depth_frame_size if depth_fps is None: depth_fps = self.depth_frame_rate if device_options is None: device_options = self.controls.export_presets() if self.device is not None: self.device.stop() self.device = None self.service.stop() self.service.start() self.notify_all({ "subject": "realsense_source.restart", "device_id": device_id, "color_frame_size": color_frame_size, "color_fps": color_fps, "depth_frame_size": depth_frame_size, "depth_fps": depth_fps, "device_options": device_options, }) def on_click(self, pos, button, action): if button == glfw.GLFW_MOUSE_BUTTON_LEFT and action == glfw.GLFW_PRESS: self.mouse_drag = True if button == glfw.GLFW_MOUSE_BUTTON_LEFT and action == glfw.GLFW_RELEASE: self.mouse_drag = False def on_notify(self, notification): if notification["subject"] == "realsense_source.restart": kwargs = notification.copy() del kwargs["subject"] del kwargs["topic"] self._initialize_device(**kwargs) elif notification["subject"] == "recording.started": self.start_depth_recording(notification["rec_path"], notification["start_time_synced"]) elif notification["subject"] == "recording.stopped": self.stop_depth_recording() def start_depth_recording(self, rec_loc, start_time_synced): if not self.record_depth: return if self.depth_video_writer is not None: logger.warning("Depth video recording has been started already") return video_path = os.path.join(rec_loc, "depth.mp4") self.depth_video_writer = MPEG_Writer(video_path, start_time_synced) def stop_depth_recording(self): if self.depth_video_writer is None: logger.warning("Depth video recording was not running") return self.depth_video_writer.close() self.depth_video_writer = None @property def frame_size(self): stream = self.streams[0] return stream.width, stream.height @frame_size.setter def frame_size(self, new_size): if self.device is not None and new_size != self.frame_size: self.restart_device(color_frame_size=new_size) @property def frame_rate(self): return self.streams[0].fps @frame_rate.setter def frame_rate(self, new_rate): if self.device is not None and new_rate != self.frame_rate: self.restart_device(color_fps=new_rate) @property def depth_frame_size(self): stream = self.streams[1] return stream.width, stream.height @depth_frame_size.setter def depth_frame_size(self, new_size): if self.device is not None and new_size != self.depth_frame_size: self.restart_device(depth_frame_size=new_size) @property def depth_frame_rate(self): return self.streams[1].fps @depth_frame_rate.setter def depth_frame_rate(self, new_rate): if self.device is not None and new_rate != self.depth_frame_rate: self.restart_device(depth_fps=new_rate) @property def jpeg_support(self): return False @property def online(self): return self.device and self.device.is_streaming() @property def name(self): # not the same as `if self.device:`! if self.device is not None: return self.device.name else: return "Ghost capture"