mirror of
https://github.com/huggingface/lerobot.git
synced 2026-05-24 21:19:53 +00:00
feat(cameras/realsense): expose async depth in metric meters
This commit is contained in:
@@ -532,7 +532,6 @@ class RealSenseCamera(Camera):
|
|||||||
self.latest_timestamp = None
|
self.latest_timestamp = None
|
||||||
self.new_frame_event.clear()
|
self.new_frame_event.clear()
|
||||||
|
|
||||||
# NOTE(Steven): Missing implementation for depth for now
|
|
||||||
@check_if_not_connected
|
@check_if_not_connected
|
||||||
def async_read(self, timeout_ms: float = 200) -> NDArray[Any]:
|
def async_read(self, timeout_ms: float = 200) -> NDArray[Any]:
|
||||||
"""
|
"""
|
||||||
@@ -575,7 +574,6 @@ class RealSenseCamera(Camera):
|
|||||||
|
|
||||||
return frame
|
return frame
|
||||||
|
|
||||||
# NOTE(Steven): Missing implementation for depth for now
|
|
||||||
@check_if_not_connected
|
@check_if_not_connected
|
||||||
def read_latest(self, max_age_ms: int = 500) -> NDArray[Any]:
|
def read_latest(self, max_age_ms: int = 500) -> NDArray[Any]:
|
||||||
"""Return the most recent (color) frame captured immediately (Peeking).
|
"""Return the most recent (color) frame captured immediately (Peeking).
|
||||||
@@ -611,6 +609,71 @@ class RealSenseCamera(Camera):
|
|||||||
|
|
||||||
return frame
|
return frame
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def async_read_depth(self, timeout_ms: float = 200) -> NDArray[Any]:
|
||||||
|
"""Read the latest depth frame asynchronously, in metric meters.
|
||||||
|
|
||||||
|
Mirrors :meth:`async_read` but returns the depth stream rather than the
|
||||||
|
color stream. Output is ``np.uint16`` of shape ``(H, W)``.
|
||||||
|
|
||||||
|
Raises:
|
||||||
|
DeviceNotConnectedError: If the camera is not connected.
|
||||||
|
RuntimeError: If ``use_depth`` is ``False`` for this camera, or if
|
||||||
|
the background read thread is not running.
|
||||||
|
TimeoutError: If no frame becomes available within ``timeout_ms``.
|
||||||
|
"""
|
||||||
|
if not self.use_depth:
|
||||||
|
raise RuntimeError(f"{self}: cannot read depth — camera was configured with use_depth=False.")
|
||||||
|
|
||||||
|
if self.thread is None or not self.thread.is_alive():
|
||||||
|
raise RuntimeError(f"{self} read thread is not running.")
|
||||||
|
|
||||||
|
if not self.new_frame_event.wait(timeout=timeout_ms / 1000.0):
|
||||||
|
raise TimeoutError(f"Timed out waiting for depth frame from camera {self} after {timeout_ms} ms.")
|
||||||
|
|
||||||
|
with self.frame_lock:
|
||||||
|
depth_frame = self.latest_depth_frame
|
||||||
|
self.new_frame_event.clear()
|
||||||
|
|
||||||
|
if depth_frame is None:
|
||||||
|
raise RuntimeError(f"Internal error: Event set but no depth frame available for {self}.")
|
||||||
|
|
||||||
|
return depth_frame
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def read_latest_depth(self, max_age_ms: int = 500) -> NDArray[Any]:
|
||||||
|
"""Return the most recent depth frame in metric meters (peeking).
|
||||||
|
|
||||||
|
Non-blocking counterpart of :meth:`read_latest` for the depth stream.
|
||||||
|
Output is ``np.float32`` of shape ``(H, W)`` in meters.
|
||||||
|
|
||||||
|
Raises:
|
||||||
|
DeviceNotConnectedError: If the camera is not connected.
|
||||||
|
RuntimeError: If ``use_depth`` is ``False`` for this camera, or if
|
||||||
|
no depth frame has been captured yet.
|
||||||
|
TimeoutError: If the latest depth frame is older than ``max_age_ms``.
|
||||||
|
"""
|
||||||
|
if not self.use_depth:
|
||||||
|
raise RuntimeError(f"{self}: cannot read depth — camera was configured with use_depth=False.")
|
||||||
|
|
||||||
|
if self.thread is None or not self.thread.is_alive():
|
||||||
|
raise RuntimeError(f"{self} read thread is not running.")
|
||||||
|
|
||||||
|
with self.frame_lock:
|
||||||
|
depth_frame = self.latest_depth_frame
|
||||||
|
timestamp = self.latest_timestamp
|
||||||
|
|
||||||
|
if depth_frame is None or timestamp is None:
|
||||||
|
raise RuntimeError(f"{self} has not captured any depth frames yet.")
|
||||||
|
|
||||||
|
age_ms = (time.perf_counter() - timestamp) * 1e3
|
||||||
|
if age_ms > max_age_ms:
|
||||||
|
raise TimeoutError(
|
||||||
|
f"{self} latest depth frame is too old: {age_ms:.1f} ms (max allowed: {max_age_ms} ms)."
|
||||||
|
)
|
||||||
|
|
||||||
|
return depth_frame
|
||||||
|
|
||||||
def disconnect(self) -> None:
|
def disconnect(self) -> None:
|
||||||
"""
|
"""
|
||||||
Disconnects from the camera, stops the pipeline, and cleans up resources.
|
Disconnects from the camera, stops the pipeline, and cleans up resources.
|
||||||
|
|||||||
Reference in New Issue
Block a user