admin/lerobot
1
0
Fork 0
mirror of https://github.com/huggingface/lerobot.git synced 2026-08-08 17:39:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

fix(cameras): D405 RealSense connection timeout on startup (#3894)

Browse Source 
* fix(cameras): add color_format config and auto-recovery for RealSense D405

The D405 delivers color from its stereo depth module, not a dedicated
RGB sensor. The driver previously hardcoded rs.format.rgb8, causing
silent frame capture failure on D405.

Changes:
- Add color_format field to RealSenseCameraConfig (default rgb8, D405
  users set bgr8), validated against whitelist
- Use configured format in _configure_rs_pipeline_config
- Fix _postprocess_image to handle both rgb8 and bgr8 source formats
- Add _hardware_reset auto-recovery: if warmup times out, perform USB
  hardware reset and retry once (common D405 recovery path)
- Fix thread race in _read_loop (local ref to stop_event)

Continuation of #3164 (closed due to deleted fork).

Tested on Intel RealSense D405 at 1280x720@30fps with color_format=bgr8.

* style: fix ruff lint B904 and format

* refactor(cameras): clarify RealSense connection retry

* refactor(cameras): address review, retry RealSense connect before hardware reset

Drop color_format (device-side streaming state was the actual cause), keep _open_pipeline attempt-agnostic, catch only retry-worthy errors, reset only as last resort, guard read loop against late frame publication after stop.

* refactor(cameras): restore BaseException teardown, shorten stop-check comment

* test(cameras): expect ConnectionError after retries are exhausted
This commit is contained in:
Yuxian LI
2026-08-03 03:54:08 +08:00
committed by GitHub
parent adccdea1cf
commit bad0260a46
2 changed files with 272 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lerobot/cameras/realsense/camera_realsense.py
+112 -37
View File
@@ -109,6 +109,11 @@ class RealSenseCamera(Camera):
```
"""
# Maximum number of warmup attempts made by connect(). A failed attempt is first
# retried with a plain pipeline stop/start, which is usually enough to recover the
# stream; a USB hardware reset is performed before the final attempt as a last resort.
_MAX_CONNECT_ATTEMPTS = 3
def __init__(self, config: RealSenseCameraConfig):
"""
Initializes the RealSenseCamera instance.
@@ -173,6 +178,76 @@ class RealSenseCamera(Camera):
"""Checks if the camera pipeline is started and streams are active."""
return self.rs_pipeline is not None and self.rs_profile is not None
def _hardware_reset(self, wait_s: float = 5.0) -> None:
"""Issue a USB hardware reset to recover an unresponsive device (common on D405)."""
context = rs.context()
for device in context.query_devices():
if device.get_info(rs.camera_info.serial_number) == self.serial_number:
logger.info(f"{self} performing hardware reset.")
device.hardware_reset()
time.sleep(wait_s)
return
logger.warning(f"{self} device not found for hardware reset, skipping.")
def _open_pipeline(self) -> None:
"""Initializes the RealSense pipeline, starts it, and starts the background read thread.
Raises:
ValueError: If the configuration is invalid, a requested sensor option is unsupported,
or a requested sensor value is invalid.
ConnectionError: If the camera is found but fails to start the pipeline or no RealSense devices are detected at all.
RuntimeError: If the pipeline starts but fails to apply requested settings.
"""
rs_pipeline = rs.pipeline()
rs_config = rs.config()
self._configure_rs_pipeline_config(rs_config)
try:
rs_profile = rs_pipeline.start(rs_config)
except RuntimeError as e:
raise ConnectionError(
f"Failed to open {self}.Run `lerobot-find-cameras realsense` to find available cameras."
) from e
self.rs_pipeline = rs_pipeline
self.rs_profile = rs_profile
try:
self._configure_capture_settings()
self._configure_sensor_options()
self._start_read_thread()
except BaseException:
self._release_after_failed_setup()
raise
def _run_warmup(self) -> None:
"""Blocks until at least one valid frame has been captured by the background thread.
Raises:
ConnectionError: If no frame arrives before ``warmup_s`` elapses.
"""
# 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.")
def _release_after_failed_setup(self) -> None:
"""Releases the device handle and restores auto-detected settings after a failed attempt."""
try:
self._cleanup_resources()
except Exception:
logger.exception(f"Failed to fully clean up {self} after connect() failed.")
self._reset_connection_settings()
@check_if_already_connected
def connect(self, warmup: bool = True) -> None:
"""
@@ -181,58 +256,48 @@ class RealSenseCamera(Camera):
Initializes the RealSense pipeline, configures the required streams (color
and optionally depth), starts the pipeline, and validates the actual stream settings.
If the pipeline starts but no frames arrive during warmup, retries up to
``_MAX_CONNECT_ATTEMPTS`` times, performing a USB hardware reset before the
final attempt.
Args:
warmup (bool): If True, waits at connect() time until at least one valid frame
has been captured by the background thread. Defaults to True.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the configuration is invalid, a requested sensor option is unsupported,
or a requested sensor value is invalid.
ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique).
ConnectionError: If the camera is found but fails to start the pipeline or no RealSense devices are detected at all.
RuntimeError: If the pipeline starts but fails to apply requested settings.
"""
self.rs_pipeline = rs.pipeline()
rs_config = rs.config()
self._configure_rs_pipeline_config(rs_config)
last_error: Exception | None = None
try:
self.rs_profile = self.rs_pipeline.start(rs_config)
except RuntimeError as e:
self.rs_profile = None
self.rs_pipeline = None
raise ConnectionError(
f"Failed to open {self}.Run `lerobot-find-cameras realsense` to find available cameras."
) from e
for attempt in range(1, self._MAX_CONNECT_ATTEMPTS + 1):
if attempt == self._MAX_CONNECT_ATTEMPTS:
self._hardware_reset()
try:
self._configure_capture_settings()
self._configure_sensor_options()
self._start_read_thread()
self._open_pipeline()
# 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.")
except BaseException:
connected = False
try:
self._cleanup_resources()
except Exception:
logger.exception(f"Failed to fully clean up {self} after connect() failed.")
self._reset_connection_settings()
raise
self._run_warmup()
connected = True
except (TimeoutError, ConnectionError) as e:
last_error = e
finally:
if not connected:
self._release_after_failed_setup()
logger.info(f"{self} connected.")
if connected:
logger.info(f"{self} connected.")
return
logger.warning(f"{self} warmup failed (attempt {attempt}/{self._MAX_CONNECT_ATTEMPTS}).")
raise ConnectionError(
f"{self} failed to capture frames after {self._MAX_CONNECT_ATTEMPTS} attempts."
) from last_error
@staticmethod
def find_cameras() -> list[dict[str, Any]]:
@@ -629,6 +694,9 @@ class RealSenseCamera(Camera):
capture_time = time.perf_counter()
with self.frame_lock:
# Under the lock, so a late frame cannot resurrect the buffer _stop_read_thread() cleared.
if stop_event.is_set():
break
if self.use_rgb:
self.latest_color_frame = processed_color_frame
if self.use_depth:
@@ -839,4 +907,11 @@ class RealSenseCamera(Camera):
)
self._cleanup_resources()
with self.frame_lock:
self.latest_color_frame = None
self.latest_depth_frame = None
self.latest_timestamp = None
self.new_frame_event.clear()
logger.info(f"{self} disconnected.")
tests/cameras/test_realsense.py
+160 -1
View File
@@ -20,6 +20,7 @@
# ```
from pathlib import Path
from threading import Event
from unittest.mock import MagicMock, patch
import numpy as np
@@ -134,9 +135,11 @@ def test_connect_cleans_up_after_warmup_failure_and_allows_retry():
read_threads.append(camera.thread)
raise TimeoutError("no frame")
# Every attempt times out, so connect() exhausts its retries and reports the failure.
with (
patch.object(camera, "async_read", side_effect=fail_warmup),
pytest.raises(TimeoutError, match="no frame"),
patch.object(camera, "_hardware_reset"),
pytest.raises(ConnectionError, match="failed to capture frames"),
):
camera.connect()
@@ -511,3 +514,159 @@ def test_rotation(rotation):
assert camera.width == 640
assert camera.height == 480
assert img.shape[:2] == (480, 640)
# --- connect() retry/state-machine tests ---
def test_connect_open_failure_propagates(patch_realsense):
"""A pipeline that cannot be opened at all fails immediately, with no hardware reset."""
patch_realsense.side_effect = mock_rs_config_enable_device_bad_file
config = RealSenseCameraConfig(serial_number_or_name="042")
camera = RealSenseCamera(config)
with (
patch.object(camera, "_hardware_reset") as mock_reset,
pytest.raises(ConnectionError),
):
camera.connect(warmup=False)
mock_reset.assert_not_called()
@pytest.mark.parametrize(
"warmup_error",
[ConnectionError("no frames"), TimeoutError("timed out")],
ids=["connection_error", "timeout_error"],
)
def test_connect_retries_without_reset_first(patch_realsense, warmup_error):
"""A failed warmup tears down and is first retried with a plain stop/start cycle.
Both failure types are covered: warmup itself raises ConnectionError, while a
stalled read surfaces as TimeoutError from async_read.
"""
config = RealSenseCameraConfig(serial_number_or_name="042", warmup_s=0)
camera = RealSenseCamera(config)
with (
patch.object(camera, "_run_warmup", side_effect=[warmup_error, None]) as mock_warmup,
patch.object(camera, "_hardware_reset") as mock_reset,
):
camera.connect(warmup=False)
assert mock_warmup.call_count == 2
mock_reset.assert_not_called()
assert camera.is_connected
camera.disconnect()
def test_connect_resets_before_final_attempt(patch_realsense):
"""When plain retries keep failing, the device is reset before the final attempt."""
config = RealSenseCameraConfig(serial_number_or_name="042", warmup_s=0)
camera = RealSenseCamera(config)
real_release = camera._release_after_failed_setup
calls = []
def tracked_release():
calls.append("teardown")
real_release()
failures = [ConnectionError("no frames")] * (RealSenseCamera._MAX_CONNECT_ATTEMPTS - 1)
with (
patch.object(camera, "_run_warmup", side_effect=[*failures, None]) as mock_warmup,
patch.object(camera, "_release_after_failed_setup", side_effect=tracked_release),
patch.object(camera, "_hardware_reset", side_effect=lambda: calls.append("reset")),
):
camera.connect(warmup=False)
assert mock_warmup.call_count == RealSenseCamera._MAX_CONNECT_ATTEMPTS
# every failed attempt is torn down, and the reset happens after the last teardown
assert calls == ["teardown"] * len(failures) + ["reset"]
assert camera.is_connected
camera.disconnect()
def test_connect_exhausts_attempts_and_cleans_up(patch_realsense):
"""When every attempt fails, connect() raises and leaves the camera fully torn down."""
config = RealSenseCameraConfig(serial_number_or_name="042", warmup_s=0)
camera = RealSenseCamera(config)
max_attempts = RealSenseCamera._MAX_CONNECT_ATTEMPTS
with (
patch.object(camera, "_run_warmup", side_effect=ConnectionError("no frames")) as mock_warmup,
patch.object(camera, "_hardware_reset") as mock_reset,
pytest.raises(ConnectionError, match=f"after {max_attempts} attempts"),
):
camera.connect(warmup=False)
assert mock_warmup.call_count == max_attempts
# the hardware reset is a last resort, used only before the final attempt
assert mock_reset.call_count == 1
assert not camera.is_connected
assert camera.thread is None
assert camera.rs_pipeline is None
def test_connect_setup_failure_after_start_tears_down_and_is_not_retried(patch_realsense):
"""A failure in _configure_capture_settings/_start_read_thread after a successful pipeline
start must tear down and propagate unchanged, not be retried."""
config = RealSenseCameraConfig(serial_number_or_name="042", warmup_s=0)
camera = RealSenseCamera(config)
with (
patch.object(camera, "_configure_capture_settings", side_effect=RuntimeError("boom")),
patch.object(camera, "_hardware_reset") as mock_reset,
pytest.raises(RuntimeError, match="boom"),
):
camera.connect(warmup=False)
mock_reset.assert_not_called()
assert not camera.is_connected
assert camera.rs_pipeline is None
def test_connect_unexpected_warmup_exception_tears_down_and_propagates(patch_realsense):
"""An unexpected (non-Timeout/Connection) exception from warmup tears down and propagates
unchanged, not treated as retry-worthy."""
config = RealSenseCameraConfig(serial_number_or_name="042", warmup_s=0)
camera = RealSenseCamera(config)
with (
patch.object(camera, "_run_warmup", side_effect=ValueError("unexpected")),
patch.object(camera, "_hardware_reset") as mock_reset,
pytest.raises(ValueError, match="unexpected"),
):
camera.connect(warmup=False)
mock_reset.assert_not_called()
assert not camera.is_connected
assert camera.thread is None
assert camera.rs_pipeline is None
def test_read_loop_does_not_publish_after_stop_requested():
"""A read landing after a stop was requested must not repopulate the frame buffer.
`_stop_read_thread` gives up joining after 2s while a hardware read can block for up
to 10s, so a late frame would otherwise resurrect the buffer that was just cleared and
be seen as a fresh frame by the next connect attempt.
"""
config = RealSenseCameraConfig(serial_number_or_name="042", warmup_s=0)
camera = RealSenseCamera(config)
camera.stop_event = Event()
def read_then_request_stop():
# the stop lands while this read is in flight
camera.stop_event.set()
return MagicMock()
with (
patch.object(camera, "_read_from_hardware", side_effect=read_then_request_stop),
patch.object(camera, "_postprocess_image", return_value=np.zeros((480, 640, 3), np.uint8)),
):
camera._read_loop()
assert camera.latest_color_frame is None
assert camera.latest_timestamp is None
assert not camera.new_frame_event.is_set()