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"
