From 8059efc84d0c7d885152f42b78d7d29888d14277 Mon Sep 17 00:00:00 2001 From: Mathieu De Coster Date: Fri, 6 Dec 2024 12:01:18 +0100 Subject: [PATCH] Fix realsense processing block bug --- CHANGELOG.md | 1 + .../cameras/realsense/realsense.py | 14 +++++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8e88d25..9206cc9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -35,6 +35,7 @@ This project uses a [CalVer](https://calver.org/) versioning scheme with monthly ### Fixed - Fixed bug in `get_colored_point_cloud()` that removed some points see issue #25. - Fixed bug requiring unplug-and-plug of USB cable for Realsense: see issue #109. +- Fixed bug with Realsense cameras raising `RuntimeErrors` in RGB-depth alignment when CPU is busy. The camera will now try again once after 1 second. - Removed camera imports in `airo_camera_toolkit.cameras`: see issue #110. - Added `__init__.py` to `realsense` and `utils` in `airo_camera_toolkit.cameras`, fixing installs with pip and issue #113. - Fixed bug that returned a transposed resolution in `MultiprocessRGBReceiver`. diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/realsense/realsense.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/realsense/realsense.py index 53f0caf..0e876ba 100644 --- a/airo-camera-toolkit/airo_camera_toolkit/cameras/realsense/realsense.py +++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/realsense/realsense.py @@ -1,5 +1,6 @@ from __future__ import annotations +import time from typing import Any, Optional import numpy as np @@ -13,6 +14,7 @@ NumpyFloatImageType, NumpyIntImageType, ) +from loguru import logger class Realsense(RGBDCamera): @@ -115,7 +117,17 @@ def _grab_images(self) -> None: if not self._depth_enabled: return - aligned_frames = self.align_transform.process(self._composite_frame) + try: + aligned_frames = self.align_transform.process(self._composite_frame) + except RuntimeError as e: + # Sometimes, the realsense SDK throws an error withn aligning RGB and depth. + # This can happen if the CPU is busy: https://github.com/IntelRealSense/librealsense/issues/6628#issuecomment-647379900 + # A solution is to try again. Here, we only try again once; if the error occurs again, we raise it + # and let the user deal with it. + logger.error(f"Error while grabbing images:\n{e}.\nWill retry in 1 second.") + time.sleep(1) + aligned_frames = self.align_transform.process(self._composite_frame) + self._depth_frame = aligned_frames.get_depth_frame() if self.hole_filling_enabled: