class RealSense_Stream_Body(Plugin):
""" Pupil Capture plugin for the Intel RealSense T265 tracking camera.
This plugin, when activated, will start a connected RealSense T265 tracking
camera and fetch odometry data (position, orientation, velocities) from it.
When recording, the odometry will be saved to `odometry.pldata`.
Note that the recorded timestamps come from uvc.get_time_monotonic and will
probably have varying latency wrt the actual timestamp of the odometry
data. Because of this, the timestamp from the T265 device is also recorded
to the field `rs_timestamp`.
"""
uniqueness = "unique"
icon_font = "roboto"
icon_chr = "B"
def __init__(self, g_pool):
""" Constructor. """
super().__init__(g_pool)
logger.info(str(g_pool.process))
self.proxy = None
self.writer = None
self.max_latency_ms = 1.
self.verbose = False
# initialize empty menu
self.menu = None
self.infos = {
'sampling_rate': None,
'confidence': None,
'position': None,
'orientation': None,
'angular_velocity': None,
'linear_velocity': None,
}
self.frame_queue = mp.Queue()
self.pipeline = None
self.started = False
self._t_last = 0.
@classmethod
def start_pipeline(cls, callback=None):
""" Start the RealSense pipeline. """
pipeline = rs.pipeline()
logger.info("LOADED PIPELINE")
config = rs.config()
#config.enable_device('0000909212111129')
logger.info("CONFIG")
config.enable_stream(rs.stream.pose)
logger.info("STREAM")
#sn = rs.camera_info.serial_number #THIS DOESNT WORK
#logger.info(sn)
if callback is None:
pipeline.start(config)
else:
pipeline.start(config, callback)
return pipeline
def frame_callback(self, rs_frame):
""" Callback for new RealSense frames. """
if rs_frame.is_pose_frame() and self.frame_queue is not None:
odometry = self.get_odometry(rs_frame, self._t_last)
# TODO writing to the class attribute might be the reason for the
# jittery pupil timestamp. Maybe do the sample rate calculation
# in the main thread, assuming frames aren't dropped.
self._t_last = odometry[0]
self.frame_queue.put(odometry)
@classmethod
def get_odometry(cls, rs_frame, t_last):
""" Get odometry data from RealSense pose frame. """
t = rs_frame.get_timestamp() / 1e3
t_pupil = get_time_monotonic()
f = 1. / (t - t_last)
pose = rs_frame.as_pose_frame()
c = pose.pose_data.tracker_confidence
p = pose.pose_data.translation
q = pose.pose_data.rotation
v = pose.pose_data.velocity
w = pose.pose_data.angular_velocity
return t, t_pupil, f, c, \
(p.x, p.y, p.z), (q.w, q.x, q.y, q.z), \
(v.x, v.y, v.z), (w.x, w.y, w.z)
@classmethod
def odometry_to_list_of_dicts(cls, odometry_data):
""" Convert list of tuples to list of dicts. """
return [{
'topic': 'odometry_body',
'timestamp': t_pupil,
'rs_timestamp': t,
'tracker_confidence': c,
'position': p,
'orientation': q,
'linear_velocity': v,
'angular_velocity': w
} for t, t_pupil, f, c, p, q, v, w in odometry_data]
@classmethod
def get_info_str(cls, values, axes, unit=None):
""" Get string with current values for display. """
if unit is None:
return ', '.join(f'{a}: {v:.2f}' for v, a in zip(values, axes))
else:
return ', '.join(f'{a}: {v:.2f} {unit}'
for v, a in zip(values, axes))
def show_infos(self, t, t_pupil, f, c, p, q, v, w):
""" Show current RealSense data in the plugin menu. """
f = np.mean(f)
c = np.mean(c)
p = tuple(map(np.mean, zip(*p)))
q = tuple(map(np.mean, zip(*q)))
v = tuple(map(np.mean, zip(*v)))
w = tuple(map(np.mean, zip(*w)))
if self.infos['linear_velocity'] is not None:
self.infos['sampling_rate'].text = f'Sampling rate: {f:.2f} Hz'
self.infos['confidence'].text = f'Confidence: {c}'
self.infos['position'].text = self.get_info_str(
p, ('x', 'y', 'z'), 'm')
self.infos['orientation'].text = self.get_info_str(
q, ('w', 'x', 'y', 'z'))
self.infos['linear_velocity'].text = self.get_info_str(
v, ('x', 'y', 'z'), 'm/s')
self.infos['angular_velocity'].text = self.get_info_str(
w, ('x', 'y', 'z'), 'rad/s')
def recent_events(self, events):
""" Main loop callback. """
if not self.started:
return
try:
t = 0.
t0 = self.g_pool.get_timestamp()
#t0 = get_time_monotonic()
odometry_data = []
# Only get new frames from the queue for self.max_latency_ms
while (t - t0) < self.max_latency_ms / 1000. \
and not self.frame_queue.empty():
odometry_data.append(self.frame_queue.get())
#t = get_time_monotonic()
t = self.g_pool.get_timestamp()
else:
if self.verbose:
logger.info(
f'Stopped after fetching {len(odometry_data)} '
f'odometry frames. Will resume after next world '
f'frame.')
except RuntimeError as e:
logger.error(str(e))
return
# Show (and possibly record) collected odometry data
if len(odometry_data) > 0:
self.show_infos(*zip(*odometry_data))
if self.writer is not None:
for d in self.odometry_to_list_of_dicts(odometry_data):
try:
self.writer.append(d)
except AttributeError:
pass
def cleanup(self):
""" Cleanup callback. """
if self.pipeline is not None:
self.pipeline.stop()
self.pipeline = None
self.started = False
def on_notify(self, notification):
""" Callback for notifications. """
# Start or stop recording base on notification
if notification['subject'] == 'recording.started':
if self.writer is None:
self.writer = PLData_Writer(notification['rec_path'],
'odometry_body')
if notification['subject'] == 'recording.stopped':
if self.writer is not None:
self.writer.close()
self.writer = None
def add_info_menu(self, measure):
""" Add growing menu with infos. """
self.infos[measure] = ui.Info_Text('Waiting...')
info_menu = ui.Growing_Menu(measure.replace('_', ' ').capitalize())
info_menu.append(self.infos[measure])
self.menu.append(info_menu)
def init_ui(self):
""" Initialize plugin UI. """
self.add_menu()
self.menu.label = "RealSense Stream Body"
self.infos['sampling_rate'] = ui.Info_Text('Waiting...')
self.menu.append(self.infos['sampling_rate'])
self.infos['confidence'] = ui.Info_Text('')
self.menu.append(self.infos['confidence'])
self.add_info_menu('position')
self.add_info_menu('orientation')
self.add_info_menu('linear_velocity')
self.add_info_menu('angular_velocity')
try:
# TODO dispatch to thread to avoid blocking
self.pipeline = self.start_pipeline(self.frame_callback)
self.started = True
except Exception as e:
logger.error(traceback.format_exc())
def deinit_ui(self):
""" De-initialize plugin UI. """
self.remove_menu()
self.cleanup()
class T265_Recorder(Plugin):
""" Pupil Capture plugin for the Intel RealSense T265 tracking camera.
This plugin will start a connected RealSense T265 tracking camera and
fetch odometry data (position, orientation, velocities) from it. When
recording, the odometry will be saved to `odometry.pldata`.
Note that the recorded timestamps come from uvc.get_time_monotonic and will
probably have varying latency wrt the actual timestamp of the odometry
data. Because of this, the timestamp from the T265 device is also recorded
to the field `rs_timestamp`.
"""
uniqueness = "unique"
icon_chr = chr(0xE061)
icon_font = "pupil_icons"
def __init__(self, g_pool):
""" Constructor. """
super().__init__(g_pool)
self.writer = None
self.max_latency_ms = 1.0
self.verbose = False
self.menu = None
self.buttons = {"start_stop_button": None}
self.infos = {
"sampling_rate": None,
"confidence": None,
"position": None,
"orientation": None,
"angular_velocity": None,
"linear_velocity": None,
}
self.g_pool.t265_extrinsics = self.load_extrinsics(
self.g_pool.user_dir)
self.odometry_queue = mp.Queue()
self.video_queue = mp.Queue()
self.pipeline = None
self.t265_window = None
self.t265_window_should_close = False
self.last_video_frame = None
@classmethod
def get_serial_numbers(cls, suffix="T265"):
""" Return serial numbers of connected devices.
based on https://github.com/IntelRealSense/librealsense/issues/2332
"""
serials = []
context = rs.context()
for d in context.devices:
if suffix and not d.get_info(rs.camera_info.name).endswith(suffix):
continue
serial = d.get_info(rs.camera_info.serial_number)
serials.append(serial)
return serials
@classmethod
def start_pipeline(cls, callback=None, odometry=True, video=True):
""" Start the RealSense pipeline. """
try:
serials = cls.get_serial_numbers()
except RuntimeError as e:
raise CannotStartPipeline(
f"Could not determine connected T265 devices. Reason: {e}")
# check number of connected devices
if len(serials) == 0:
raise CannotStartPipeline("No T265 device connected.")
elif len(serials) > 1:
raise CannotStartPipeline(
"Multiple T265 devices not yet supported.")
try:
pipeline = rs.pipeline()
config = rs.config()
# TODO not working with librealsense 2.32.1 but is necessary for
# simultaneously operating multiple devices:
# config.enable_device(serials[0])
if odometry:
config.enable_stream(rs.stream.pose)
if video:
config.enable_stream(rs.stream.fisheye, 1)
config.enable_stream(rs.stream.fisheye, 2)
if callback is None:
pipeline.start(config)
else:
pipeline.start(config, callback)
except RuntimeError as e:
raise CannotStartPipeline(
f"Could not start RealSense pipeline. Maybe another plugin "
f"is using it. Reason: {e}")
return pipeline
def start_stop_callback(self):
""" Callback for the start/stop button. """
if self.pipeline is None:
try:
self.pipeline = self.start_pipeline(
callback=self.frame_callback,
odometry=self.odometry_queue is not None,
video=self.video_queue is not None,
)
self.buttons["start_stop_button"].label = "Stop Pipeline"
logger.info("RealSense pipeline started.")
except CannotStartPipeline as e:
logger.error(str(e))
else:
self.pipeline.stop()
self.pipeline = None
self.buttons["start_stop_button"].label = "Start Pipeline"
logger.info("RealSense pipeline stopped.")
# Close video stream video if open
if self.t265_window is not None:
self.t265_window_should_close = True
def frame_callback(self, rs_frame):
""" Callback for new RealSense frames. """
if rs_frame.is_pose_frame() and self.odometry_queue is not None:
odometry = self.get_odometry_data(rs_frame)
self.odometry_queue.put(odometry)
elif rs_frame.is_frameset() and self.video_queue is not None:
video_data = self.get_video_data(rs_frame)
self.video_queue.put(video_data)
@classmethod
def save_extrinsics(cls, directory, t265_extrinsics, side="left"):
""" Save extrinsics to user dir. """
save_path = os.path.join(directory, f"t265_{side}.extrinsics")
save_object(t265_extrinsics, save_path)
logger.info(f"Extrinsics for {side} T265 camera saved to {save_path}")
@classmethod
def load_extrinsics(cls, directory, side="left"):
""" Load extrinsics from user dir. """
load_path = os.path.join(directory, f"t265_{side}.extrinsics")
try:
extrinsics = load_object(load_path)
logger.info(f"Loaded t265_{side}.extrinsics")
return extrinsics
except OSError:
logger.warning("No extrinsics found. Use the T265 Calibration "
"plugin to calculate extrinsics.")
@classmethod
def get_odometry_data(cls, rs_frame):
""" Get odometry data from RealSense pose frame. """
t = rs_frame.get_timestamp() / 1e3
t_pupil = get_time_monotonic()
pose = rs_frame.as_pose_frame()
c = pose.pose_data.tracker_confidence
p = pose.pose_data.translation
q = pose.pose_data.rotation
v = pose.pose_data.velocity
w = pose.pose_data.angular_velocity
return {
"topic": "odometry",
"timestamp": t_pupil,
"rs_timestamp": t,
"tracker_confidence": c,
"position": (p.x, p.y, p.z),
"orientation": (q.w, q.x, q.y, q.z),
"linear_velocity": (v.x, v.y, v.z),
"angular_velocity": (w.x, w.y, w.z),
}
@classmethod
def get_video_data(cls, rs_frame, side="both"):
""" Extract video frame and timestamp from a RealSense frame. """
t = rs_frame.get_timestamp() / 1e3
t_pupil = get_time_monotonic()
frameset = rs_frame.as_frameset()
if side == "left":
video_frame = np.asanyarray(
frameset.get_fisheye_frame(1).as_video_frame().get_data())
elif side == "right":
video_frame = np.asanyarray(
frameset.get_fisheye_frame(2).as_video_frame().get_data())
elif side == "both":
video_frame = np.hstack([
np.asanyarray(
frameset.get_fisheye_frame(1).as_video_frame().get_data()),
np.asanyarray(
frameset.get_fisheye_frame(2).as_video_frame().get_data()),
])
else:
raise ValueError(f"Unsupported mode: {side}")
return {
"topic": "fisheye",
"timestamp": t_pupil,
"rs_timestamp": t,
"frame": video_frame,
}
@classmethod
def get_info_str(cls, values, axes, unit=None):
""" Get string with current values for display. """
if unit is None:
return ", ".join(f"{a}: {v: .2f}" for v, a in zip(values, axes))
else:
return ", ".join(f"{a}: {v: .2f} {unit}"
for v, a in zip(values, axes))
def show_infos(self, odometry_data):
""" Show current RealSense data in the plugin menu. """
odometry_dict = {
k: np.array([d[k] for d in odometry_data])
for k in odometry_data[0]
}
if len(odometry_data) > 1:
f_odometry = np.mean(1 / np.diff(odometry_dict["rs_timestamp"]))
else:
f_odometry = 0.0
c = np.mean(odometry_dict["tracker_confidence"])
p = np.mean(odometry_dict["position"], axis=0)
q = np.mean(odometry_dict["orientation"], axis=0)
v = np.mean(odometry_dict["linear_velocity"], axis=0)
w = np.mean(odometry_dict["angular_velocity"], axis=0)
if self.pipeline is not None:
self.infos[
"sampling_rate"].text = f"Odometry sampling rate: {f_odometry:.2f} Hz"
self.infos["confidence"].text = f"Tracker confidence: {c}"
self.infos["position"].text = self.get_info_str(
p, ("x", "y", "z"), "m")
self.infos["orientation"].text = self.get_info_str(
q, ("w", "x", "y", "z"))
self.infos["linear_velocity"].text = self.get_info_str(
v, ("x", "y", "z"), "m/s")
self.infos["angular_velocity"].text = self.get_info_str(
w, ("x", "y", "z"), "rad/s")
else:
for info in self.infos.values():
info.text = "Waiting..."
def recent_events(self, events):
""" Main loop callback. """
try:
odometry_data = []
video_data = []
while not self.odometry_queue.empty():
odometry_data.append(self.odometry_queue.get())
while not self.video_queue.empty():
video_data.append(self.video_queue.get())
self.last_video_frame = video_data[-1]["frame"]
except RuntimeError as e:
logger.error(str(e))
return
# Show (and possibly record) collected odometry data
if len(odometry_data) > 0:
self.show_infos(odometry_data)
if self.writer is not None:
self.write(odometry_data)
if self.t265_window_should_close:
self.close_t265_window()
def write(self, odometry_data):
""" Write new odometry to the .pldata file """
for d in odometry_data:
try:
self.writer.append(d)
except AttributeError:
pass
def cleanup(self):
""" Cleanup callback. """
if self.pipeline is not None:
self.start_stop_callback()
def open_t265_window(self):
""" Open a window to show the T265 video stream. """
if self.pipeline is None:
logger.error("Start pipeline to show T265 video stream")
return
if not self.t265_window:
width, height = 1696, 800
self.t265_window = glfwCreateWindow(
width,
height,
"T265 Video Stream",
monitor=None,
share=glfwGetCurrentContext(),
)
glfwSetWindowPos(self.t265_window, 200, 31)
# Register callbacks
glfwSetFramebufferSizeCallback(self.t265_window, on_resize)
glfwSetWindowCloseCallback(self.t265_window, self.on_t265_close)
on_resize(self.t265_window,
*glfwGetFramebufferSize(self.t265_window))
# gl_state settings
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self.t265_window)
basic_gl_setup()
glfwMakeContextCurrent(active_window)
def gl_display(self):
""" Display routines called by the world process after each frame. """
if self.t265_window:
self.gl_display_in_t265_window()
def gl_display_in_t265_window(self):
""" Show new frame in window. """
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self.t265_window)
clear_gl_screen()
if self.last_video_frame is not None:
make_coord_system_norm_based()
draw_gl_texture(self.last_video_frame)
glfwSwapBuffers(self.t265_window)
glfwMakeContextCurrent(active_window)
def on_t265_close(self, window=None):
""" Callback when windows is closed. """
self.t265_window_should_close = True
def close_t265_window(self):
""" Close T265 video stream window. """
self.t265_window_should_close = False
if self.t265_window:
glfwDestroyWindow(self.t265_window)
self.t265_window = None
def on_notify(self, notification):
""" Callback for notifications. """
# Start or stop recording based on notification
if notification["subject"] == "recording.started":
if self.pipeline is None:
logger.warning(
"Pipeline is not running. T265 Recorder will not record "
"any data.")
return
if self.writer is None:
self.writer = PLData_Writer(notification["rec_path"],
"odometry")
if notification["subject"] == "recording.stopped":
if self.writer is not None:
self.writer.close()
if hasattr(self.g_pool, "t265_extrinsics"):
self.save_extrinsics(notification["rec_path"],
self.g_pool.t265_extrinsics)
def add_info_menu(self, measure):
""" Add growing menu with infos. """
self.infos[measure] = ui.Info_Text("Waiting...")
info_menu = ui.Growing_Menu(measure.replace("_", " ").capitalize())
info_menu.append(self.infos[measure])
self.menu.append(info_menu)
def init_ui(self):
""" Initialize plugin UI. """
self.add_menu()
self.menu.label = "T265 Recorder"
self.buttons["start_stop_button"] = ui.Button("Start Pipeline",
self.start_stop_callback)
self.menu.append(self.buttons["start_stop_button"])
self.buttons["show_video_button"] = ui.Button("Show T265 Video Stream",
self.open_t265_window)
self.menu.append(self.buttons["show_video_button"])
self.infos["sampling_rate"] = ui.Info_Text("Waiting...")
self.menu.append(self.infos["sampling_rate"])
self.infos["confidence"] = ui.Info_Text("")
self.menu.append(self.infos["confidence"])
self.add_info_menu("position")
self.add_info_menu("orientation")
self.add_info_menu("linear_velocity")
self.add_info_menu("angular_velocity")
def deinit_ui(self):
""" De-initialize plugin UI. """
self.remove_menu()
# TODO do we need to call cleanup() here?
self.cleanup()