Fix realsense processing block bug

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`.

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: