fix(cameras): release device handle when connect() setup fails (#4187)

Co-authored-by: Ryan Rana <39924576+RyanRana@users.noreply.github.com>

src/lerobot/cameras/opencv/camera_opencv.py

@@ -120,14 +120,22 @@ class OpenCVCamera(Camera):
         self.rotation: int | None = get_cv2_rotation(config.rotation)
         self.backend: int = config.backend
 
-        if self.height and self.width:
-            self.capture_width, self.capture_height = self.width, self.height
-            if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
-                self.capture_width, self.capture_height = self.height, self.width
+        self.capture_width: int | None = None
+        self.capture_height: int | None = None
+        self._reset_connection_settings()
 
     def __str__(self) -> str:
         return f"{self.__class__.__name__}({self.index_or_path})"
 
+    def _reset_connection_settings(self) -> None:
+        """Restore settings that may have been auto-detected during a failed connection."""
+        self.fps = self.config.fps
+        self.width = self.config.width
+        self.height = self.config.height
+        self.capture_width, self.capture_height = self.width, self.height
+        if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+            self.capture_width, self.capture_height = self.height, self.width
+
     @property
     def is_connected(self) -> bool:
         """Checks if the camera is currently connected and opened."""
@@ -164,6 +172,7 @@ class OpenCVCamera(Camera):
                 f"Failed to open {self}.Run `lerobot-find-cameras opencv` to find available cameras."
             )
 
+        try:
             self._configure_capture_settings()
             self._start_read_thread()
 
@@ -175,6 +184,13 @@ class OpenCVCamera(Camera):
                 with self.frame_lock:
                     if self.latest_frame is None:
                         raise ConnectionError(f"{self} failed to capture frames during warmup.")
+        except BaseException:
+            try:
+                self._cleanup_resources()
+            except Exception:
+                logger.exception(f"Failed to fully clean up {self} after connect() failed.")
+            self._reset_connection_settings()
+            raise
 
         logger.info(f"{self} connected.")
 
@@ -312,6 +328,7 @@ class OpenCVCamera(Camera):
 
         for target in targets_to_scan:
             camera = cv2.VideoCapture(target)
+            try:
                 if camera.isOpened():
                     default_width = int(camera.get(cv2.CAP_PROP_FRAME_WIDTH))
                     default_height = int(camera.get(cv2.CAP_PROP_FRAME_HEIGHT))
@@ -321,7 +338,9 @@ class OpenCVCamera(Camera):
                     # Get FOURCC code and convert to string
                     default_fourcc_code = camera.get(cv2.CAP_PROP_FOURCC)
                     default_fourcc_code_int = int(default_fourcc_code)
-                default_fourcc = "".join([chr((default_fourcc_code_int >> 8 * i) & 0xFF) for i in range(4)])
+                    default_fourcc = "".join(
+                        [chr((default_fourcc_code_int >> 8 * i) & 0xFF) for i in range(4)]
+                    )
 
                     camera_info = {
                         "name": f"OpenCV Camera @ {target}",
@@ -338,6 +357,7 @@ class OpenCVCamera(Camera):
                     }
 
                     found_cameras_info.append(camera_info)
+            finally:
                 camera.release()
 
         return found_cameras_info
@@ -496,6 +516,26 @@ class OpenCVCamera(Camera):
             self.latest_timestamp = None
             self.new_frame_event.clear()
 
+    def _cleanup_resources(self) -> None:
+        """Stop background reads and release the capture, including after partial setup."""
+        read_thread = self.thread
+        videocapture = self.videocapture
+
+        try:
+            self._stop_read_thread()
+        finally:
+            self.videocapture = None
+            try:
+                if videocapture is not None:
+                    videocapture.release()
+            finally:
+                # Releasing the device may unblock a hardware read that outlived
+                # the first bounded join in _stop_read_thread().
+                if read_thread is not None and read_thread.is_alive():
+                    read_thread.join(timeout=2.0)
+                    if read_thread.is_alive():  # pragma: no cover
+                        logger.warning(f"{self} read thread remained alive after releasing the capture.")
+
     @check_if_not_connected
     def async_read(self, timeout_ms: float = 200) -> NDArray[Any]:
         """
@@ -586,16 +626,6 @@ class OpenCVCamera(Camera):
         if not self.is_connected and self.thread is None:
             raise DeviceNotConnectedError(f"{self} not connected.")
 
-        if self.thread is not None:
-            self._stop_read_thread()
-
-        if self.videocapture is not None:
-            self.videocapture.release()
-            self.videocapture = None
-
-        with self.frame_lock:
-            self.latest_frame = None
-            self.latest_timestamp = None
-            self.new_frame_event.clear()
+        self._cleanup_resources()
 
         logger.info(f"{self} disconnected.")

src/lerobot/cameras/realsense/camera_realsense.py

@@ -145,14 +145,23 @@ class RealSenseCamera(Camera):
 
         self.rotation: int | None = get_cv2_rotation(config.rotation)
 
-        if self.height and self.width:
-            self.capture_width, self.capture_height = self.width, self.height
-            if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
-                self.capture_width, self.capture_height = self.height, self.width
+        self.capture_width: int | None = None
+        self.capture_height: int | None = None
+        self._reset_connection_settings()
 
     def __str__(self) -> str:
         return f"{self.__class__.__name__}({self.serial_number})"
 
+    def _reset_connection_settings(self) -> None:
+        """Restore settings that may have been auto-detected during a failed connection."""
+        self.fps = self.config.fps
+        self.width = self.config.width
+        self.height = self.config.height
+        self.warmup_s = self.config.warmup_s
+        self.capture_width, self.capture_height = self.width, self.height
+        if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+            self.capture_width, self.capture_height = self.height, self.width
+
     @property
     def is_connected(self) -> bool:
         """Checks if the camera pipeline is started and streams are active."""
@@ -190,6 +199,7 @@ class RealSenseCamera(Camera):
                 f"Failed to open {self}.Run `lerobot-find-cameras realsense` to find available cameras."
             ) from e
 
+        try:
             self._configure_capture_settings()
             self._start_read_thread()
 
@@ -206,6 +216,13 @@ class RealSenseCamera(Camera):
                     self.use_depth and self.latest_depth_frame is None
                 ):
                     raise ConnectionError(f"{self} failed to capture frames during warmup.")
+        except BaseException:
+            try:
+                self._cleanup_resources()
+            except Exception:
+                logger.exception(f"Failed to fully clean up {self} after connect() failed.")
+            self._reset_connection_settings()
+            raise
 
         logger.info(f"{self} connected.")
 
@@ -541,6 +558,27 @@ class RealSenseCamera(Camera):
             self.latest_timestamp = None
             self.new_frame_event.clear()
 
+    def _cleanup_resources(self) -> None:
+        """Stop background reads and stop the pipeline, including after partial setup."""
+        read_thread = self.thread
+        rs_pipeline = self.rs_pipeline
+
+        try:
+            self._stop_read_thread()
+        finally:
+            self.rs_pipeline = None
+            self.rs_profile = None
+            try:
+                if rs_pipeline is not None:
+                    rs_pipeline.stop()
+            finally:
+                # Stopping the pipeline may unblock a hardware read that outlived
+                # the first bounded join in _stop_read_thread().
+                if read_thread is not None and read_thread.is_alive():
+                    read_thread.join(timeout=2.0)
+                    if read_thread.is_alive():  # pragma: no cover
+                        logger.warning(f"{self} read thread remained alive after stopping the pipeline.")
+
     def _async_read(self, timeout_ms: float, read_depth: bool = False) -> NDArray[Any]:
         """Shared helper for :meth:`async_read`/:meth:`async_read_depth`: return the latest buffered frame."""
         if self.thread is None or not self.thread.is_alive():
@@ -684,18 +722,6 @@ class RealSenseCamera(Camera):
                 f"Attempted to disconnect {self}, but it appears already disconnected."
             )
 
-        if self.thread is not None:
-            self._stop_read_thread()
-
-        if self.rs_pipeline is not None:
-            self.rs_pipeline.stop()
-            self.rs_pipeline = None
-            self.rs_profile = None
-
-        with self.frame_lock:
-            self.latest_color_frame = None
-            self.latest_depth_frame = None
-            self.latest_timestamp = None
-            self.new_frame_event.clear()
+        self._cleanup_resources()
 
         logger.info(f"{self} disconnected.")

tests/cameras/test_opencv.py

@@ -20,7 +20,7 @@
 # ```
 
 from pathlib import Path
-from unittest.mock import patch
+from unittest.mock import MagicMock, patch
 
 import cv2
 import numpy as np
@@ -123,6 +123,73 @@ def test_invalid_width_connect():
         camera.connect(warmup=False)
 
 
+def test_connect_cleans_up_after_settings_failure_and_allows_retry():
+    config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH, warmup_s=0)
+    camera = OpenCVCamera(config)
+    opened_captures = []
+
+    def fail_settings():
+        opened_captures.append(camera.videocapture)
+        raise RuntimeError("settings failed")
+
+    with (
+        patch.object(camera, "_configure_capture_settings", side_effect=fail_settings),
+        pytest.raises(RuntimeError, match="settings failed"),
+    ):
+        camera.connect(warmup=False)
+
+    assert camera.videocapture is None
+    assert camera.thread is None
+    assert not camera.is_connected
+    assert opened_captures[0] is not None
+    assert not opened_captures[0].isOpened()
+
+    camera.connect(warmup=False)
+    assert camera.is_connected
+    camera.disconnect()
+
+
+def test_connect_cleans_up_after_warmup_failure_and_allows_retry():
+    config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH, warmup_s=1)
+    camera = OpenCVCamera(config)
+    read_threads = []
+
+    def fail_warmup(*_args, **_kwargs):
+        read_threads.append(camera.thread)
+        raise TimeoutError("no frame")
+
+    with (
+        patch.object(camera, "async_read", side_effect=fail_warmup),
+        pytest.raises(TimeoutError, match="no frame"),
+    ):
+        camera.connect()
+
+    assert camera.videocapture is None
+    assert camera.thread is None
+    assert not camera.is_connected
+    assert read_threads[0] is not None
+    assert not read_threads[0].is_alive()
+
+    camera.connect(warmup=False)
+    assert camera.is_connected
+    camera.disconnect()
+
+
+def test_find_cameras_releases_unopened_handles():
+    module_path = OpenCVCamera.__module__
+    unopened_capture = MagicMock()
+    unopened_capture.isOpened.return_value = False
+
+    with (
+        patch(f"{module_path}.platform.system", return_value="Darwin"),
+        patch(f"{module_path}.MAX_OPENCV_INDEX", 1),
+        patch(f"{module_path}.cv2.VideoCapture", return_value=unopened_capture),
+    ):
+        assert OpenCVCamera.find_cameras() == []
+
+    unopened_capture.release.assert_called_once_with()
+
+
 @pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=TEST_IMAGE_SIZES)
 def test_read(index_or_path):
     config = OpenCVCameraConfig(index_or_path=index_or_path, warmup_s=0)

tests/cameras/test_realsense.py

@@ -91,6 +91,33 @@ def test_invalid_width_connect():
         camera.connect(warmup=False)
 
 
+def test_connect_cleans_up_after_warmup_failure_and_allows_retry():
+    config = RealSenseCameraConfig(serial_number_or_name="042", width=640, height=480, fps=30)
+    camera = RealSenseCamera(config)
+    read_threads = []
+
+    def fail_warmup(*_args, **_kwargs):
+        read_threads.append(camera.thread)
+        raise TimeoutError("no frame")
+
+    with (
+        patch.object(camera, "async_read", side_effect=fail_warmup),
+        pytest.raises(TimeoutError, match="no frame"),
+    ):
+        camera.connect()
+
+    assert camera.rs_pipeline is None
+    assert camera.rs_profile is None
+    assert camera.thread is None
+    assert not camera.is_connected
+    assert read_threads[0] is not None
+    assert not read_threads[0].is_alive()
+
+    camera.connect(warmup=False)
+    assert camera.is_connected
+    camera.disconnect()
+
+
 def test_read():
     config = RealSenseCameraConfig(serial_number_or_name="042", width=640, height=480, fps=30, warmup_s=0)
     with RealSenseCamera(config) as camera: