From 23f6d5dabd079b5e55474e977bdb1d40ff7e771d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 28 Jul 2026 13:21:06 +0200 Subject: [PATCH] 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 | 126 +++++++++++------- .../cameras/realsense/camera_realsense.py | 88 +++++++----- tests/cameras/test_opencv.py | 69 +++++++++- tests/cameras/test_realsense.py | 27 ++++ 4 files changed, 230 insertions(+), 80 deletions(-) diff --git a/src/lerobot/cameras/opencv/camera_opencv.py b/src/lerobot/cameras/opencv/camera_opencv.py index e50d24c01..bdcc7e17d 100644 --- a/src/lerobot/cameras/opencv/camera_opencv.py +++ b/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,17 +172,25 @@ class OpenCVCamera(Camera): f"Failed to open {self}.Run `lerobot-find-cameras opencv` to find available cameras." ) - self._configure_capture_settings() - self._start_read_thread() + try: + self._configure_capture_settings() + self._start_read_thread() - if warmup and self.warmup_s > 0: - start_time = time.time() - while time.time() - start_time < self.warmup_s: - self.async_read(timeout_ms=self.warmup_s * 1000) - time.sleep(0.1) - with self.frame_lock: - if self.latest_frame is None: - raise ConnectionError(f"{self} failed to capture frames during warmup.") + if warmup and self.warmup_s > 0: + start_time = time.time() + while time.time() - start_time < self.warmup_s: + self.async_read(timeout_ms=self.warmup_s * 1000) + time.sleep(0.1) + 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,32 +328,36 @@ class OpenCVCamera(Camera): for target in targets_to_scan: camera = cv2.VideoCapture(target) - if camera.isOpened(): - default_width = int(camera.get(cv2.CAP_PROP_FRAME_WIDTH)) - default_height = int(camera.get(cv2.CAP_PROP_FRAME_HEIGHT)) - default_fps = camera.get(cv2.CAP_PROP_FPS) - default_format = camera.get(cv2.CAP_PROP_FORMAT) + try: + if camera.isOpened(): + default_width = int(camera.get(cv2.CAP_PROP_FRAME_WIDTH)) + default_height = int(camera.get(cv2.CAP_PROP_FRAME_HEIGHT)) + default_fps = camera.get(cv2.CAP_PROP_FPS) + default_format = camera.get(cv2.CAP_PROP_FORMAT) - # 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)]) + # 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)] + ) - camera_info = { - "name": f"OpenCV Camera @ {target}", - "type": "OpenCV", - "id": target, - "backend_api": camera.getBackendName(), - "default_stream_profile": { - "format": default_format, - "fourcc": default_fourcc, - "width": default_width, - "height": default_height, - "fps": default_fps, - }, - } + camera_info = { + "name": f"OpenCV Camera @ {target}", + "type": "OpenCV", + "id": target, + "backend_api": camera.getBackendName(), + "default_stream_profile": { + "format": default_format, + "fourcc": default_fourcc, + "width": default_width, + "height": default_height, + "fps": default_fps, + }, + } - found_cameras_info.append(camera_info) + 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.") diff --git a/src/lerobot/cameras/realsense/camera_realsense.py b/src/lerobot/cameras/realsense/camera_realsense.py index f873abbd4..bad22fcc0 100644 --- a/src/lerobot/cameras/realsense/camera_realsense.py +++ b/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,22 +199,30 @@ class RealSenseCamera(Camera): f"Failed to open {self}.Run `lerobot-find-cameras realsense` to find available cameras." ) from e - self._configure_capture_settings() - self._start_read_thread() + try: + self._configure_capture_settings() + self._start_read_thread() - # NOTE(Steven/Caroline): Enforcing at least one second of warmup as RS cameras need a bit of time before the first read. If we don't wait, the first read from the warmup will raise. - self.warmup_s = max(self.warmup_s, 1) + # NOTE(Steven/Caroline): Enforcing at least one second of warmup as RS cameras need a bit of time before the first read. If we don't wait, the first read from the warmup will raise. + self.warmup_s = max(self.warmup_s, 1) - warmup_read = self.async_read if self.use_rgb else self.async_read_depth - start_time = time.time() - while time.time() - start_time < self.warmup_s: - warmup_read(timeout_ms=self.warmup_s * 1000) - time.sleep(0.1) - with self.frame_lock: - if (self.use_rgb and self.latest_color_frame is None) or ( - self.use_depth and self.latest_depth_frame is None - ): - raise ConnectionError(f"{self} failed to capture frames during warmup.") + warmup_read = self.async_read if self.use_rgb else self.async_read_depth + start_time = time.time() + while time.time() - start_time < self.warmup_s: + warmup_read(timeout_ms=self.warmup_s * 1000) + time.sleep(0.1) + with self.frame_lock: + if (self.use_rgb and self.latest_color_frame is None) or ( + 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.") diff --git a/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index e2ae5c572..c8d609f36 100644 --- a/tests/cameras/test_opencv.py +++ b/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) diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 64cc272d5..789a07209 100644 --- a/tests/cameras/test_realsense.py +++ b/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: