def recent_events(self, events):
if self.check_space():
disk_space = available_gb(self.rec_root_dir)
if (disk_space < self.warning_low_disk_space_th
and self.low_disk_space_thumb not in self.g_pool.quickbar):
self.g_pool.quickbar.append(self.low_disk_space_thumb)
elif (disk_space >= self.warning_low_disk_space_th
and self.low_disk_space_thumb in self.g_pool.quickbar):
self.g_pool.quickbar.remove(self.low_disk_space_thumb)
if self.running and disk_space <= self.stop_rec_low_disk_space_th:
self.stop()
logger.error("Recording was stopped due to low disk space!")
if self.running:
for key, data in events.items():
if key not in ("dt",
"depth_frame") and not key.startswith("frame"):
try:
writer = self.pldata_writers[key]
except KeyError:
writer = PLData_Writer(self.rec_path, key)
self.pldata_writers[key] = writer
writer.extend(data)
if "frame" in events:
frame = events["frame"]
self.writer.write_video_frame(frame)
self.frame_count += 1
# # cv2.putText(frame.img, "Frame %s"%self.frame_count,(200,200), cv2.FONT_HERSHEY_SIMPLEX,1,(255,100,100))
self.button.status_text = self.get_rec_time_str()
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 on_notify(self, notification):
"""Handles recorder notifications
Reacts to notifications:
``recording.should_start``: Starts a new recording session.
fields:
- 'session_name' change session name
start with `/` to ingore the rec base dir and start from root instead.
- `record_eye` boolean that indicates recording of the eyes, defaults to current setting
- `rec_root_dir` full path to recording directory root, defaults to current setting
``recording.should_stop``: Stops current recording session
Emits notifications:
``recording.started``: New recording session started
``recording.stopped``: Current recording session stopped
Args:
notification (dictionary): Notification dictionary
"""
# notification wants to be recorded
if notification.get('record', False) and self.running:
if 'timestamp' not in notification:
logger.error(
"Notification without timestamp will not be saved.")
else:
notification['topic'] = 'notify.' + notification['subject']
try:
writer = self.pldata_writers['notify']
except KeyError:
writer = PLData_Writer(self.rec_path, 'notify')
self.pldata_writers['notify'] = writer
writer.append(notification)
elif notification['subject'] == 'recording.should_start':
if self.running:
logger.info('Recording already running!')
else:
self.record_eye = notification.get('record_eye',
self.record_eye)
if notification.get("session_name", ""):
self.set_session_name(notification["session_name"])
if notifcation.get("rec_root_dir", ""):
self.set_rec_root_dir(notification["rec_root_dir"])
self.start()
elif notification['subject'] == 'recording.should_stop':
if self.running:
self.stop()
else:
logger.info('Recording already stopped!')
def on_notify(self, notification):
"""Handles recorder notifications
Reacts to notifications:
``recording.should_start``: Starts a new recording session.
fields:
- 'session_name' change session name
start with `/` to ingore the rec base dir and start from root instead.
- `record_eye` boolean that indicates recording of the eyes, defaults to current setting
``recording.should_stop``: Stops current recording session
Emits notifications:
``recording.started``: New recording session started
``recording.stopped``: Current recording session stopped
Args:
notification (dictionary): Notification dictionary
"""
# notification wants to be recorded
if notification.get("record", False) and self.running:
if "timestamp" not in notification:
logger.error(
"Notification without timestamp will not be saved.")
notification["timestamp"] = self.g_pool.get_timestamp()
# else:
notification["topic"] = "notify." + notification["subject"]
try:
writer = self.pldata_writers["notify"]
except KeyError:
writer = PLData_Writer(self.rec_path, "notify")
self.pldata_writers["notify"] = writer
writer.append(notification)
elif notification["subject"] == "recording.should_start":
if self.running:
logger.info("Recording already running!")
else:
self.record_eye = notification.get("record_eye",
self.record_eye)
if notification.get("session_name", ""):
self.set_session_name(notification["session_name"])
self.start()
elif notification["subject"] == "recording.should_stop":
if self.running:
self.stop()
else:
logger.info("Recording already stopped!")
def on_notify(self, notification):
"""Handles recorder notifications
Reacts to notifications:
``recording.should_start``: Starts a new recording session.
fields:
- 'session_name' change session name
start with `/` to ingore the rec base dir and start from root instead.
- `record_eye` boolean that indicates recording of the eyes, defaults to current setting
``recording.should_stop``: Stops current recording session
Emits notifications:
``recording.started``: New recording session started
``recording.stopped``: Current recording session stopped
Args:
notification (dictionary): Notification dictionary
"""
# notification wants to be recorded
if notification.get("record", False) and self.running:
if "timestamp" not in notification:
logger.error("Notification without timestamp will not be saved.")
else:
notification["topic"] = "notify." + notification["subject"]
try:
writer = self.pldata_writers["notify"]
except KeyError:
writer = PLData_Writer(self.rec_path, "notify")
self.pldata_writers["notify"] = writer
writer.append(notification)
elif notification["subject"] == "recording.should_start":
if self.running:
logger.info("Recording already running!")
else:
self.record_eye = notification.get("record_eye", self.record_eye)
if notification.get("session_name", ""):
self.set_session_name(notification["session_name"])
self.start()
elif notification["subject"] == "recording.should_stop":
if self.running:
self.stop()
else:
logger.info("Recording already stopped!")
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 recent_events(self, events):
if self.check_space():
disk_space = available_gb(self.rec_root_dir)
if (
disk_space < self.warning_low_disk_space_th
and self.low_disk_space_thumb not in self.g_pool.quickbar
):
self.g_pool.quickbar.append(self.low_disk_space_thumb)
elif (
disk_space >= self.warning_low_disk_space_th
and self.low_disk_space_thumb in self.g_pool.quickbar
):
self.g_pool.quickbar.remove(self.low_disk_space_thumb)
if self.running and disk_space <= self.stop_rec_low_disk_space_th:
self.stop()
logger.error("Recording was stopped due to low disk space!")
if self.running:
for key, data in events.items():
if key not in ("dt", "depth_frame") and not key.startswith("frame"):
try:
writer = self.pldata_writers[key]
except KeyError:
writer = PLData_Writer(self.rec_path, key)
self.pldata_writers[key] = writer
writer.extend(data)
if "frame" in events:
frame = events["frame"]
self.writer.write_video_frame(frame)
self.frame_count += 1
# # cv2.putText(frame.img, "Frame %s"%self.frame_count,(200,200), cv2.FONT_HERSHEY_SIMPLEX,1,(255,100,100))
self.button.status_text = self.get_rec_time_str()
def start(self):
self.start_time = time()
start_time_synced = self.g_pool.get_timestamp()
if isinstance(self.g_pool.capture, NDSI_Source):
# If the user did not enable TimeSync, the timestamps will be way off and
# the recording code will crash. We check the difference between the last
# frame's time and the start_time_synced and if this does not match, we stop
# the recording and show a warning instead.
TIMESTAMP_ERROR_THRESHOLD = 5.0
frame = self.g_pool.capture._recent_frame
if frame is None:
logger.error(
"Your connection does not seem to be stable enough for "
"recording Pupil Mobile via WiFi. We recommend recording "
"on the phone.")
return
if abs(frame.timestamp -
start_time_synced) > TIMESTAMP_ERROR_THRESHOLD:
logger.error(
"Pupil Mobile stream is not in sync. Aborting recording."
" Enable the Time Sync plugin and try again.")
return
session = os.path.join(self.rec_root_dir, self.session_name)
try:
os.makedirs(session, exist_ok=True)
logger.debug(
"Created new recordings session dir {}".format(session))
except OSError:
logger.error(
"Could not start recording. Session dir {} not writable.".
format(session))
return
self.pldata_writers = {}
self.frame_count = 0
self.running = True
self.menu.read_only = True
recording_uuid = uuid.uuid4()
# set up self incrementing folder within session folder
counter = 0
while True:
self.rec_path = os.path.join(session, "{:03d}/".format(counter))
try:
os.mkdir(self.rec_path)
logger.debug("Created new recording dir {}".format(
self.rec_path))
break
except FileExistsError:
logger.debug(
"We dont want to overwrite data, incrementing counter & trying to make new data folder"
)
counter += 1
self.meta_info = RecordingInfoFile.create_empty_file(self.rec_path)
self.meta_info.recording_software_name = (
RecordingInfoFile.RECORDING_SOFTWARE_NAME_PUPIL_CAPTURE)
self.meta_info.recording_software_version = str(self.g_pool.version)
self.meta_info.recording_name = self.session_name
self.meta_info.start_time_synced_s = start_time_synced
self.meta_info.start_time_system_s = self.start_time
self.meta_info.recording_uuid = recording_uuid
self.meta_info.system_info = get_system_info()
self.video_path = os.path.join(self.rec_path, "world.mp4")
if self.raw_jpeg and self.g_pool.capture.jpeg_support:
self.writer = JPEG_Writer(self.video_path, start_time_synced)
elif hasattr(self.g_pool.capture._recent_frame, "h264_buffer"):
self.writer = H264Writer(
self.video_path,
self.g_pool.capture.frame_size[0],
self.g_pool.capture.frame_size[1],
int(self.g_pool.capture.frame_rate),
)
else:
self.writer = MPEG_Writer(self.video_path, start_time_synced)
calibration_data_notification_classes = [
CalibrationSetupNotification,
CalibrationResultNotification,
]
writer = PLData_Writer(self.rec_path, "notify")
for note_class in calibration_data_notification_classes:
try:
file_path = os.path.join(self.g_pool.user_dir,
note_class.file_name())
note = note_class.from_dict(load_object(file_path))
note_dict = note.as_dict()
note_dict["topic"] = "notify." + note_dict["subject"]
writer.append(note_dict)
except FileNotFoundError:
continue
self.pldata_writers["notify"] = writer
if self.show_info_menu:
self.open_info_menu()
logger.info("Started Recording.")
self.notify_all({
"subject": "recording.started",
"rec_path": self.rec_path,
"session_name": self.session_name,
"record_eye": self.record_eye,
"compression": self.raw_jpeg,
"start_time_synced": float(start_time_synced),
})
def start(self):
session = os.path.join(self.rec_root_dir, self.session_name)
try:
os.makedirs(session, exist_ok=True)
logger.debug("Created new recordings session dir {}".format(session))
except OSError:
logger.error(
"Could not start recording. Session dir {} not writable.".format(
session
)
)
return
self.pldata_writers = {}
self.frame_count = 0
self.running = True
self.menu.read_only = True
self.start_time = time()
start_time_synced = self.g_pool.get_timestamp()
recording_uuid = uuid.uuid4()
# set up self incrementing folder within session folder
counter = 0
while True:
self.rec_path = os.path.join(session, "{:03d}/".format(counter))
try:
os.mkdir(self.rec_path)
logger.debug("Created new recording dir {}".format(self.rec_path))
break
except:
logger.debug(
"We dont want to overwrite data, incrementing counter & trying to make new data folder"
)
counter += 1
self.meta_info_path = os.path.join(self.rec_path, "info.csv")
with open(self.meta_info_path, "w", newline="", encoding="utf-8") as csvfile:
csv_utils.write_key_value_file(
csvfile,
{
"Recording Name": self.session_name,
"Start Date": strftime("%d.%m.%Y", localtime(self.start_time)),
"Start Time": strftime("%H:%M:%S", localtime(self.start_time)),
"Start Time (System)": self.start_time,
"Start Time (Synced)": start_time_synced,
"Recording UUID": recording_uuid,
},
)
self.video_path = os.path.join(self.rec_path, "world.mp4")
if self.raw_jpeg and self.g_pool.capture.jpeg_support:
self.writer = JPEG_Writer(self.video_path, self.g_pool.capture.frame_rate)
elif hasattr(self.g_pool.capture._recent_frame, "h264_buffer"):
self.writer = H264Writer(
self.video_path,
self.g_pool.capture.frame_size[0],
self.g_pool.capture.frame_size[1],
int(self.g_pool.capture.frame_rate),
)
else:
self.writer = AV_Writer(self.video_path, fps=self.g_pool.capture.frame_rate)
try:
cal_pt_path = os.path.join(self.g_pool.user_dir, "user_calibration_data")
cal_data = load_object(cal_pt_path)
notification = {"subject": "calibration.calibration_data", "record": True}
notification.update(cal_data)
notification["topic"] = "notify." + notification["subject"]
writer = PLData_Writer(self.rec_path, "notify")
writer.append(notification)
self.pldata_writers["notify"] = writer
except FileNotFoundError:
pass
if self.show_info_menu:
self.open_info_menu()
logger.info("Started Recording.")
self.notify_all(
{
"subject": "recording.started",
"rec_path": self.rec_path,
"session_name": self.session_name,
"record_eye": self.record_eye,
"compression": self.raw_jpeg,
}
)
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()