refactor(cameras): add read_depth() for realsense + new compressed bag
This commit is contained in:
6
.gitignore
vendored
6
.gitignore
vendored
@@ -171,3 +171,9 @@ dmypy.json
|
|||||||
|
|
||||||
# Cython debug symbols
|
# Cython debug symbols
|
||||||
cython_debug/
|
cython_debug/
|
||||||
|
|
||||||
|
# realsense data-recording format
|
||||||
|
*.bag
|
||||||
|
|
||||||
|
# opencv test images
|
||||||
|
fakecam*
|
||||||
|
|||||||
@@ -27,7 +27,7 @@ repos:
|
|||||||
- repo: https://github.com/pre-commit/pre-commit-hooks
|
- repo: https://github.com/pre-commit/pre-commit-hooks
|
||||||
rev: v5.0.0
|
rev: v5.0.0
|
||||||
hooks:
|
hooks:
|
||||||
#- id: check-added-large-files
|
- id: check-added-large-files
|
||||||
- id: debug-statements
|
- id: debug-statements
|
||||||
- id: check-merge-conflict
|
- id: check-merge-conflict
|
||||||
- id: check-case-conflict
|
- id: check-case-conflict
|
||||||
|
|||||||
@@ -399,33 +399,75 @@ class RealSenseCamera(Camera):
|
|||||||
)
|
)
|
||||||
logger.debug(f"Capture height set to {actual_height} for {self}.")
|
logger.debug(f"Capture height set to {actual_height} for {self}.")
|
||||||
|
|
||||||
def read(
|
def read_depth(self, timeout_ms: int = 5000) -> np.ndarray:
|
||||||
self, color_mode: ColorMode | None = None, timeout_ms: int = 5000
|
|
||||||
) -> np.ndarray | Tuple[np.ndarray, np.ndarray]:
|
|
||||||
"""
|
"""
|
||||||
Reads a single frame (color and optionally depth) synchronously from the camera.
|
Reads a single frame (depth) synchronously from the camera.
|
||||||
|
|
||||||
This is a blocking call. It waits for a coherent set of frames (color, depth if enabled)
|
This is a blocking call. It waits for a coherent set of frames (depth)
|
||||||
from the camera hardware via the RealSense pipeline.
|
from the camera hardware via the RealSense pipeline.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms.
|
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms.
|
||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
Union[np.ndarray, Tuple[np.ndarray, np.ndarray]]:
|
np.ndarray: The depth map as a NumPy array (height, width)
|
||||||
- If `use_depth` is False: The captured color frame as a NumPy array
|
of type `np.uint16` (raw depth values in millimeters) and rotation.
|
||||||
|
|
||||||
|
Raises:
|
||||||
|
DeviceNotConnectedError: If the camera is not connected.
|
||||||
|
RuntimeError: If reading frames from the pipeline fails or frames are invalid.
|
||||||
|
"""
|
||||||
|
|
||||||
|
if not self.is_connected:
|
||||||
|
raise DeviceNotConnectedError(f"{self} is not connected.")
|
||||||
|
|
||||||
|
if not self.use_depth:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"Failed to capture depth frame from {self}. '.read_depth()'. Depth stream is not enabled."
|
||||||
|
)
|
||||||
|
|
||||||
|
start_time = time.perf_counter()
|
||||||
|
|
||||||
|
ret, frame = self.rs_pipeline.try_wait_for_frames(
|
||||||
|
timeout_ms=timeout_ms
|
||||||
|
) # NOTE(Steven): This read has a timeout
|
||||||
|
|
||||||
|
if not ret or frame is None:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"Failed to capture frame from {self}. '.read_depth()' returned status={ret} and frame is None."
|
||||||
|
)
|
||||||
|
|
||||||
|
depth_frame = frame.get_depth_frame()
|
||||||
|
depth_map = np.asanyarray(depth_frame.get_data())
|
||||||
|
|
||||||
|
depth_map_processed = self._postprocess_image(depth_map)
|
||||||
|
|
||||||
|
read_duration_ms = (time.perf_counter() - start_time) * 1e3
|
||||||
|
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
|
||||||
|
|
||||||
|
self.logs["timestamp_utc"] = capture_timestamp_utc()
|
||||||
|
return depth_map_processed
|
||||||
|
|
||||||
|
def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 5000) -> np.ndarray:
|
||||||
|
"""
|
||||||
|
Reads a single frame (color) synchronously from the camera.
|
||||||
|
|
||||||
|
This is a blocking call. It waits for a coherent set of frames (color)
|
||||||
|
from the camera hardware via the RealSense pipeline.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
np.ndarray: The captured color frame as a NumPy array
|
||||||
(height, width, channels), processed according to `color_mode` and rotation.
|
(height, width, channels), processed according to `color_mode` and rotation.
|
||||||
- If `use_depth` is True: A tuple containing:
|
|
||||||
- color_image (np.ndarray): The processed color frame.
|
|
||||||
- depth_map (np.ndarray): The depth map as a NumPy array (height, width)
|
|
||||||
of type `np.uint16` (raw depth values in millimeters, before rotation).
|
|
||||||
Depth map is NOT rotated by this method.
|
|
||||||
|
|
||||||
Raises:
|
Raises:
|
||||||
DeviceNotConnectedError: If the camera is not connected.
|
DeviceNotConnectedError: If the camera is not connected.
|
||||||
RuntimeError: If reading frames from the pipeline fails or frames are invalid.
|
RuntimeError: If reading frames from the pipeline fails or frames are invalid.
|
||||||
ValueError: If an invalid `color_mode` is requested.
|
ValueError: If an invalid `color_mode` is requested.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
if not self.is_connected:
|
if not self.is_connected:
|
||||||
raise DeviceNotConnectedError(f"{self} is not connected.")
|
raise DeviceNotConnectedError(f"{self} is not connected.")
|
||||||
|
|
||||||
@@ -445,23 +487,11 @@ class RealSenseCamera(Camera):
|
|||||||
|
|
||||||
color_image_processed = self._postprocess_image(color_image_raw, color_mode)
|
color_image_processed = self._postprocess_image(color_image_raw, color_mode)
|
||||||
|
|
||||||
if self.use_depth:
|
read_duration_ms = (time.perf_counter() - start_time) * 1e3
|
||||||
depth_frame = frame.get_depth_frame()
|
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
|
||||||
depth_map = np.asanyarray(depth_frame.get_data())
|
|
||||||
|
|
||||||
depth_map_processed = self._postprocess_image(depth_map)
|
self.logs["timestamp_utc"] = capture_timestamp_utc()
|
||||||
|
return color_image_processed
|
||||||
read_duration_ms = (time.perf_counter() - start_time) * 1e3
|
|
||||||
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
|
|
||||||
|
|
||||||
self.logs["timestamp_utc"] = capture_timestamp_utc()
|
|
||||||
return color_image_processed, depth_map_processed
|
|
||||||
else:
|
|
||||||
read_duration_ms = (time.perf_counter() - start_time) * 1e3
|
|
||||||
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
|
|
||||||
|
|
||||||
self.logs["timestamp_utc"] = capture_timestamp_utc()
|
|
||||||
return color_image_processed
|
|
||||||
|
|
||||||
def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
|
def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
|
||||||
"""
|
"""
|
||||||
@@ -549,6 +579,7 @@ class RealSenseCamera(Camera):
|
|||||||
self.thread.start()
|
self.thread.start()
|
||||||
logger.debug(f"Read thread started for {self}.")
|
logger.debug(f"Read thread started for {self}.")
|
||||||
|
|
||||||
|
# NOTE(Steven): Missing implementation for depth
|
||||||
def async_read(self, timeout_ms: float = 2000) -> Union[np.ndarray, Tuple[np.ndarray, np.ndarray]]:
|
def async_read(self, timeout_ms: float = 2000) -> Union[np.ndarray, Tuple[np.ndarray, np.ndarray]]:
|
||||||
"""
|
"""
|
||||||
Reads the latest available frame data (color or color+depth) asynchronously.
|
Reads the latest available frame data (color or color+depth) asynchronously.
|
||||||
|
|||||||
Binary file not shown.
Reference in New Issue
Block a user