def recent_events(self, events):
try:
frame = self.uvc_capture.get_frame(0.05)
if self.preferred_exposure_time:
target = self.preferred_exposure_time.calculate_based_on_frame(frame)
if target is not None:
self.exposure_time = target
if self.checkframestripes and self.checkframestripes.require_restart(frame):
# set the self.frame_rate in order to restart
self.frame_rate = self.frame_rate
logger.info("Stripes detected")
except uvc.StreamError:
self._recent_frame = None
self._restart_logic()
except (AttributeError, uvc.InitError):
self._recent_frame = None
time.sleep(0.02)
self._restart_logic()
else:
if (
self.ts_offset
): # c930 timestamps need to be set here. The camera does not provide valid pts from device
frame.timestamp = uvc.get_time_monotonic() + self.ts_offset
frame.timestamp -= self.g_pool.timebase.value
self._recent_frame = frame
events["frame"] = frame
self._restart_in = 3
def uptime():
if args.use_uvc_time:
import uvc
return uvc.get_time_monotonic(
) * 1000000 # keep the result with optitrack
else:
return round((time.time() - boot_time) * 1e6)
def recent_events(self, events):
try:
frame = self.uvc_capture.get_frame(0.05)
if self.preferred_exposure_time:
target = self.preferred_exposure_time.calculate_based_on_frame(frame)
if target is not None:
self.exposure_time = target
if self.checkframestripes and self.checkframestripes.require_restart(frame):
# set the self.frame_rate in order to restart
self.frame_rate = self.frame_rate
logger.info("Stripes detected")
except uvc.StreamError:
self._recent_frame = None
self._restart_logic()
except (AttributeError, uvc.InitError):
self._recent_frame = None
time.sleep(0.02)
self._restart_logic()
else:
if (
self.ts_offset
): # c930 timestamps need to be set here. The camera does not provide valid pts from device
frame.timestamp = uvc.get_time_monotonic() + self.ts_offset
frame.timestamp -= self.g_pool.timebase.value
self._recent_frame = frame
events["frame"] = frame
self._restart_in = 3
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,
}
def process(self, img):
if img is None:
return
height, width = img.shape[0], img.shape[1]
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
timestamp = uvc.get_time_monotonic()
roi = None
if self.bbox is not None:
xmin, ymin, w, h = self.bbox
roi = Roi(xmin, ymin, xmin+w, ymin+h)
result = self.detector.detect(gray, timestamp, roi=roi)
#print(result)
if result["model_confidence"] > 0.25:
sphere = result["projected_sphere"]
self.__draw_ellipse(sphere, img, (255,120,120), 1)
if result["confidence"] > 0.5:
n = np.array(result['circle_3d']['normal'])
self.bbox = self.__get_bbox(result, img)
self.__draw_tracking_info(result, img)
# cv2.imshow("testando", img)
# cv2.waitKey(1)
self.pos = np.array([n[0], n[1], n[2], time.monotonic()])
self.countdown = 5
else:
self.countdown -= 1
if self.countdown <= 0:
self.pos = None
self.bbox = None
return img
def recent_events(self, events):
was_online = self.online
try:
frame = self.uvc_capture.get_frame(0.05)
if np.isclose(frame.timestamp, 0):
# sometimes (probably only on windows) after disconnections, the first frame has 0 ts
logger.debug(
"Received frame with invalid timestamp."
" This can happen after a disconnect."
" Frame will be dropped!"
)
return
if self.preferred_exposure_time:
target = self.preferred_exposure_time.calculate_based_on_frame(frame)
if target is not None:
self.exposure_time = target
if self.stripe_detector and self.stripe_detector.require_restart(frame):
# set the self.frame_rate in order to restart
self.frame_rate = self.frame_rate
logger.info("Stripes detected")
except uvc.StreamError:
self._recent_frame = None
self._restart_logic()
except (AttributeError, uvc.InitError):
self._recent_frame = None
time.sleep(0.02)
self._restart_logic()
else:
if self.ts_offset is not None:
# c930 timestamps need to be set here. The camera does not provide valid pts from device
frame.timestamp = uvc.get_time_monotonic() + self.ts_offset
if self._last_ts is not None and frame.timestamp <= self._last_ts:
logger.debug(
"Received non-monotonic timestamps from UVC! Dropping frame."
f" Last: {self._last_ts}, current: {frame.timestamp}"
)
else:
self._last_ts = frame.timestamp
frame.timestamp -= self.g_pool.timebase.value
self._recent_frame = frame
events["frame"] = frame
self._restart_in = 3
if was_online != self.online:
self.update_menu()
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)
def recent_events(self, events):
try:
frame = self.uvc_capture.get_frame(0.05)
except uvc.StreamError:
self._recent_frame = None
self._restart_logic()
except (AttributeError, uvc.InitError):
self._recent_frame = None
time.sleep(0.02)
self._restart_logic()
else:
if self.ts_offset: #c930 timestamps need to be set here. The camera does not provide valid pts from device
frame.timestamp = uvc.get_time_monotonic() + self.ts_offset
frame.timestamp -= self.g_pool.timebase.value
self._recent_frame = frame
events['frame'] = frame
self._restart_in = 3
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),
}
def get_timestamp():
return get_time_monotonic() - g_pool.timebase.value
def get_time():
return get_time_monotonic() + epoch
def get_timestamp():
return get_time_monotonic()-g_pool.timebase.value
def get_time():
return get_time_monotonic()+epoch
def get_now(self):
return uvc.get_time_monotonic()
def test_2_get_time_methods():
import uvc
t1 = uvc.get_time_monotonic() * 1000 # keep the result with optitrack
t2 = round((time.time() - boot_time) * 1e6)
print('t1:{}, t2:{}'.format(t1, t2))
def get_now(self):
return uvc.get_time_monotonic()
