diff --git a/docs/source/cameras.mdx b/docs/source/cameras.mdx index 02714d591..e5a1f9032 100644 --- a/docs/source/cameras.mdx +++ b/docs/source/cameras.mdx @@ -136,6 +136,10 @@ config = RealSenseCameraConfig( height=480, color_mode=ColorMode.RGB, use_depth=True, + # Optional fixed color controls. Omit them to leave the current sensor settings unchanged. + exposure=120, + gain=64, + white_balance=4600, rotation=Cv2Rotation.NO_ROTATION ) @@ -154,6 +158,12 @@ finally: ``` +Manual color controls disable the corresponding automatic exposure or white-balance mode. Their +supported ranges vary by camera model; an invalid value raises an error at connection time that +includes the range reported by the sensor. Requesting an unsupported control also raises an error. +Omitted controls leave the sensor's existing automatic or manual setting unchanged. These options +require `use_rgb=True`. + diff --git a/src/lerobot/cameras/realsense/camera_realsense.py b/src/lerobot/cameras/realsense/camera_realsense.py index 25ea6d519..a701a7450 100644 --- a/src/lerobot/cameras/realsense/camera_realsense.py +++ b/src/lerobot/cameras/realsense/camera_realsense.py @@ -121,6 +121,9 @@ class RealSenseCamera(Camera): self.config = config + self.width: int | None = config.width + self.height: int | None = config.height + if config.serial_number_or_name.isdigit(): self.serial_number = config.serial_number_or_name else: @@ -190,23 +193,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._configure_sensor_options() - self._start_read_thread() + try: + self._configure_capture_settings() + self._configure_sensor_options() + 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 Exception: + try: + self._cleanup_connection() + except Exception: + logger.exception(f"Failed to clean up {self} after a connection error.") + raise logger.info(f"{self} connected.") @@ -382,11 +392,13 @@ class RealSenseCamera(Camera): """Applies manual sensor options (exposure, gain, white balance) to the color sensor. When exposure or gain is set, auto-exposure is disabled first. When white_balance - is set, auto white balance is disabled first. Skipped entirely if no options are set. + is set, auto white balance is disabled first. An omitted option is left unchanged, + and configuration is skipped entirely if all options are omitted. Raises: - ValueError: If a requested value is outside the sensor's supported range. The - error message includes the option name, requested value, and supported range. + ValueError: If the sensor does not support a requested option or a requested + value is invalid. Invalid-value errors include the option name, requested + value, and supported range when available. """ config = self.config if config.exposure is None and config.gain is None and config.white_balance is None: @@ -394,6 +406,21 @@ class RealSenseCamera(Camera): color_sensor = self._get_color_sensor() + requested_options = ( + (rs.option.exposure, config.exposure, "exposure"), + (rs.option.gain, config.gain, "gain"), + (rs.option.white_balance, config.white_balance, "white balance"), + ) + unsupported_options = [ + label + for option, value, label in requested_options + if value is not None and not color_sensor.supports(option) + ] + if unsupported_options: + raise ValueError( + f"{self}: color sensor does not support requested manual options: {unsupported_options}." + ) + if (config.exposure is not None or config.gain is not None) and color_sensor.supports( rs.option.enable_auto_exposure ): @@ -401,18 +428,12 @@ class RealSenseCamera(Camera): logger.info(f"{self} auto-exposure disabled.") if config.exposure is not None: - if color_sensor.supports(rs.option.exposure): - self._set_sensor_option(color_sensor, rs.option.exposure, config.exposure, "exposure") - logger.info(f"{self} exposure set to {config.exposure}.") - else: - logger.warning(f"{self} sensor does not support manual exposure.") + self._set_sensor_option(color_sensor, rs.option.exposure, config.exposure, "exposure") + logger.info(f"{self} exposure set to {config.exposure}.") if config.gain is not None: - if color_sensor.supports(rs.option.gain): - self._set_sensor_option(color_sensor, rs.option.gain, config.gain, "gain") - logger.info(f"{self} gain set to {config.gain}.") - else: - logger.warning(f"{self} sensor does not support manual gain.") + self._set_sensor_option(color_sensor, rs.option.gain, config.gain, "gain") + logger.info(f"{self} gain set to {config.gain}.") if config.white_balance is not None: if color_sensor.supports(rs.option.enable_auto_white_balance): @@ -420,13 +441,10 @@ class RealSenseCamera(Camera): color_sensor, rs.option.enable_auto_white_balance, 0, "auto white balance" ) logger.info(f"{self} auto white balance disabled.") - if color_sensor.supports(rs.option.white_balance): - self._set_sensor_option( - color_sensor, rs.option.white_balance, config.white_balance, "white balance" - ) - logger.info(f"{self} white balance set to {config.white_balance}.") - else: - logger.warning(f"{self} sensor does not support manual white balance.") + self._set_sensor_option( + color_sensor, rs.option.white_balance, config.white_balance, "white balance" + ) + logger.info(f"{self} white balance set to {config.white_balance}.") @check_if_not_connected def read_depth(self, timeout_ms: int = 200) -> NDArray[Any]: @@ -773,13 +791,17 @@ class RealSenseCamera(Camera): f"Attempted to disconnect {self}, but it appears already disconnected." ) + self._cleanup_connection() + logger.info(f"{self} disconnected.") + + def _cleanup_connection(self) -> None: + """Release connection resources and reset state after disconnect or failed connect.""" 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 + pipeline = self.rs_pipeline + self.rs_pipeline = None + self.rs_profile = None with self.frame_lock: self.latest_color_frame = None @@ -787,4 +809,5 @@ class RealSenseCamera(Camera): self.latest_timestamp = None self.new_frame_event.clear() - logger.info(f"{self} disconnected.") + if pipeline is not None: + pipeline.stop() diff --git a/src/lerobot/cameras/realsense/configuration_realsense.py b/src/lerobot/cameras/realsense/configuration_realsense.py index b4cf4d9e0..e2f5b6b76 100644 --- a/src/lerobot/cameras/realsense/configuration_realsense.py +++ b/src/lerobot/cameras/realsense/configuration_realsense.py @@ -48,13 +48,14 @@ class RealSenseCameraConfig(CameraConfig): warmup_s: Time reading frames before returning from connect (in seconds) exposure: Manual exposure value for the color sensor. When set, auto-exposure is disabled and this fixed value is used. Valid range depends on the camera model - (e.g., 1-10000 for D400 series). Defaults to None (auto-exposure). + (e.g., 1-10000 for D400 series). Defaults to None (leave unchanged). gain: Manual gain value for the color sensor. When set, auto-exposure is disabled and this fixed gain is used. Valid range depends on the camera model - (e.g., 16-248 for D400 series). Defaults to None (auto). + (e.g., 16-248 for D400 series). Defaults to None (leave unchanged). white_balance: Manual white balance value for the color sensor. When set, auto white balance is disabled and this fixed value is used. Valid range depends on - the camera model (e.g., 2800-6500 for D400 series). Defaults to None (auto). + the camera model (e.g., 2800-6500 for D400 series). Defaults to None + (leave unchanged). Note: - Either name or serial_number must be specified. @@ -81,6 +82,18 @@ class RealSenseCameraConfig(CameraConfig): if not self.use_rgb and not self.use_depth: raise ValueError("At least one of `use_rgb` or `use_depth` must be enabled.") + manual_color_options = { + "exposure": self.exposure, + "gain": self.gain, + "white_balance": self.white_balance, + } + configured_color_options = [name for name, value in manual_color_options.items() if value is not None] + if configured_color_options and not self.use_rgb: + raise ValueError( + "Manual color sensor options require `use_rgb=True`. " + f"Configured options: {configured_color_options}." + ) + values = (self.fps, self.width, self.height) if any(v is not None for v in values) and any(v is None for v in values): raise ValueError( diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 343c43157..ffcc2402d 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -63,6 +63,17 @@ def test_abc_implementation(): _ = RealSenseCamera(config) +@pytest.mark.parametrize("option", ["exposure", "gain", "white_balance"]) +def test_manual_color_option_requires_rgb(option): + with pytest.raises(ValueError, match="use_rgb=True"): + RealSenseCameraConfig( + serial_number_or_name="042", + use_rgb=False, + use_depth=True, + **{option: 100}, + ) + + def test_connect(): config = RealSenseCameraConfig(serial_number_or_name="042", warmup_s=0) @@ -85,6 +96,27 @@ def test_connect_invalid_camera_path(patch_realsense): camera.connect(warmup=False) +def test_connect_cleans_up_when_sensor_configuration_fails(): + config = RealSenseCameraConfig(serial_number_or_name="042", exposure=120) + camera = RealSenseCamera(config) + pipeline = MagicMock() + pipeline.start.return_value = MagicMock() + + with ( + patch("lerobot.cameras.realsense.camera_realsense.rs.pipeline", return_value=pipeline), + patch.object(camera, "_configure_rs_pipeline_config"), + patch.object(camera, "_configure_capture_settings"), + patch.object(camera, "_configure_sensor_options", side_effect=ValueError("invalid exposure")), + pytest.raises(ValueError, match="invalid exposure"), + ): + camera.connect(warmup=False) + + pipeline.stop.assert_called_once_with() + assert camera.rs_pipeline is None + assert camera.rs_profile is None + assert not camera.is_connected + + def test_invalid_width_connect(): config = RealSenseCameraConfig(serial_number_or_name="042", width=99999, height=480, fps=30) camera = RealSenseCamera(config) @@ -293,20 +325,26 @@ def test_configure_sensor_options_applies_all_values(): sensor.set_option.assert_any_call(rs.option.white_balance, 4600) -def test_configure_sensor_options_warns_when_unsupported(caplog): - config = RealSenseCameraConfig(serial_number_or_name="042", exposure=120, gain=64, white_balance=4600) +@pytest.mark.parametrize( + ("config_field", "option", "label"), + [ + ("exposure", rs.option.exposure, "exposure"), + ("gain", rs.option.gain, "gain"), + ("white_balance", rs.option.white_balance, "white balance"), + ], +) +def test_configure_sensor_options_raises_when_requested_option_is_unsupported(config_field, option, label): + config = RealSenseCameraConfig(serial_number_or_name="042", **{config_field: 100}) camera = RealSenseCamera(config) sensor = _make_mock_sensor("RGB Camera", supported_options=set()) _attach_mock_color_sensor(camera, sensor) - with caplog.at_level("WARNING"): + with pytest.raises(ValueError, match=label): camera._configure_sensor_options() + sensor.supports.assert_any_call(option) sensor.set_option.assert_not_called() - assert "does not support manual exposure" in caplog.text - assert "does not support manual gain" in caplog.text - assert "does not support manual white balance" in caplog.text def test_configure_sensor_options_only_exposure_disables_auto_exposure():