diff --git a/docs/source/cameras.mdx b/docs/source/cameras.mdx index 02714d591..82d6c8355 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,15 @@ 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`. + +On the RealSense D405, the color stream is provided by the Stereo Module, so changing manual +exposure or gain also affects the depth stream. + diff --git a/src/lerobot/cameras/realsense/camera_realsense.py b/src/lerobot/cameras/realsense/camera_realsense.py index bad22fcc0..34ee4348f 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: @@ -131,6 +134,9 @@ class RealSenseCamera(Camera): self.use_rgb = config.use_rgb self.use_depth = config.use_depth self.warmup_s = config.warmup_s + self.exposure: int | None = config.exposure + self.gain: int | None = config.gain + self.white_balance: int | None = config.white_balance self.rs_pipeline: rs.pipeline | None = None self.rs_profile: rs.pipeline_profile | None = None @@ -181,7 +187,8 @@ class RealSenseCamera(Camera): Raises: DeviceAlreadyConnectedError: If the camera is already connected. - ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique). + 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. """ @@ -201,6 +208,7 @@ class RealSenseCamera(Camera): 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. @@ -356,6 +364,111 @@ class RealSenseCamera(Camera): self.new_frame_event.clear() return self._async_read(timeout_ms=10000, read_depth=read_depth) + def _get_color_sensor(self) -> "rs.sensor": + """Returns the sensor that controls the color stream. + + Most RealSense cameras expose "RGB Camera" for color. The D405 has no + separate RGB module — its color stream comes from "Stereo Module". + We try RGB Camera first, then fall back to Stereo Module. + """ + if self.rs_profile is None: + raise RuntimeError(f"{self}: rs_profile must be initialized before use.") + + device = self.rs_profile.get_device() + sensors = {s.get_info(rs.camera_info.name): s for s in device.query_sensors()} + + for name in ("RGB Camera", "Stereo Module"): + if name in sensors: + return sensors[name] + + available = list(sensors.keys()) + raise RuntimeError(f"{self}: no color sensor found. Available sensors: {available}") + + def _set_sensor_option(self, sensor: "rs.sensor", option: "rs.option", value: float, label: str) -> None: + """Sets a sensor option, re-raising range errors with actionable diagnostics.""" + try: + sensor.set_option(option, value) + except Exception as e: + range_info = "" + try: + option_range = sensor.get_option_range(option) + range_info = ( + f" (supported range: min={option_range.min}, max={option_range.max}, " + f"step={option_range.step}, default={option_range.default})" + ) + except Exception: + range_info = " (option range unavailable)" + raise ValueError( + f"{self}: failed to set {label} to {value}{range_info}. Original error: {e}" + ) from e + + def _configure_sensor_options(self) -> None: + """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. An omitted option is left unchanged, + and configuration is skipped entirely if all options are omitted. + + Raises: + 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. + """ + if self.exposure is None and self.gain is None and self.white_balance is None: + return + + color_sensor = self._get_color_sensor() + + requested_options = ( + (rs.option.exposure, self.exposure, "exposure"), + (rs.option.gain, self.gain, "gain"), + (rs.option.white_balance, self.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}." + ) + + manual_exposure_requested = self.exposure is not None or self.gain is not None + if manual_exposure_requested: + if color_sensor.supports(rs.option.enable_auto_exposure): + self._set_sensor_option(color_sensor, rs.option.enable_auto_exposure, 0, "auto-exposure") + logger.info(f"{self} auto-exposure disabled.") + else: + logger.warning( + f"{self} sensor does not support disabling auto-exposure; " + "applying manual exposure/gain directly." + ) + + if self.exposure is not None: + self._set_sensor_option(color_sensor, rs.option.exposure, self.exposure, "exposure") + logger.info(f"{self} exposure set to {self.exposure}.") + + if self.gain is not None: + self._set_sensor_option(color_sensor, rs.option.gain, self.gain, "gain") + logger.info(f"{self} gain set to {self.gain}.") + + if self.white_balance is not None: + if color_sensor.supports(rs.option.enable_auto_white_balance): + self._set_sensor_option( + color_sensor, rs.option.enable_auto_white_balance, 0, "auto white balance" + ) + logger.info(f"{self} auto white balance disabled.") + else: + logger.warning( + f"{self} sensor does not support disabling auto white balance; " + "applying manual white balance directly." + ) + self._set_sensor_option( + color_sensor, rs.option.white_balance, self.white_balance, "white balance" + ) + logger.info(f"{self} white balance set to {self.white_balance}.") + @check_if_not_connected def read_depth(self, timeout_ms: int = 200) -> NDArray[Any]: """ @@ -723,5 +836,4 @@ class RealSenseCamera(Camera): ) self._cleanup_resources() - logger.info(f"{self} disconnected.") diff --git a/src/lerobot/cameras/realsense/configuration_realsense.py b/src/lerobot/cameras/realsense/configuration_realsense.py index 018675195..c4f1c0e83 100644 --- a/src/lerobot/cameras/realsense/configuration_realsense.py +++ b/src/lerobot/cameras/realsense/configuration_realsense.py @@ -46,6 +46,17 @@ class RealSenseCameraConfig(CameraConfig): use_depth: Whether to enable depth stream. Defaults to False. rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation. 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 ranges are camera-model specific + and reported if the value is rejected. 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, which also freezes exposure at its current value + when no exposure is configured. Valid ranges are camera-model specific and + reported if the value is rejected. 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 ranges are + camera-model specific and reported if the value is rejected. Defaults to None + (leave unchanged). Note: - Either name or serial_number must be specified. @@ -61,6 +72,9 @@ class RealSenseCameraConfig(CameraConfig): use_depth: bool = False rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION warmup_s: int = 1 + exposure: int | None = None + gain: int | None = None + white_balance: int | None = None def __post_init__(self) -> None: self.color_mode = ColorMode(self.color_mode) @@ -69,6 +83,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 789a07209..e4ccef801 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -20,7 +20,7 @@ # ``` from pathlib import Path -from unittest.mock import patch +from unittest.mock import MagicMock, patch import numpy as np import pytest @@ -30,6 +30,8 @@ from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnected pytest.importorskip("pyrealsense2") +import pyrealsense2 as rs + from lerobot.cameras.realsense import RealSenseCamera, RealSenseCameraConfig TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras" @@ -61,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) @@ -83,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) @@ -255,6 +289,203 @@ def test_read_latest_too_old(): _ = camera.read_latest(max_age_ms=0) # immediately too old +def _make_mock_sensor(name: str, supported_options: set | None = None) -> MagicMock: + """Build a fake rs.sensor that reports a name and a configurable supported-options set.""" + supported = supported_options if supported_options is not None else set() + sensor = MagicMock() + sensor.get_info.return_value = name + sensor.supports.side_effect = lambda opt: opt in supported + return sensor + + +def _attach_mock_color_sensor(camera: RealSenseCamera, sensor: MagicMock) -> None: + """Wire camera.rs_profile so _get_color_sensor finds the given sensor.""" + profile = MagicMock() + device = MagicMock() + device.query_sensors.return_value = [sensor] + profile.get_device.return_value = device + camera.rs_profile = profile + + +def test_get_color_sensor_prefers_rgb_camera(): + config = RealSenseCameraConfig(serial_number_or_name="042") + camera = RealSenseCamera(config) + + rgb = _make_mock_sensor("RGB Camera") + stereo = _make_mock_sensor("Stereo Module") + profile = MagicMock() + device = MagicMock() + device.query_sensors.return_value = [stereo, rgb] + profile.get_device.return_value = device + camera.rs_profile = profile + + assert camera._get_color_sensor() is rgb + + +def test_get_color_sensor_falls_back_to_stereo_module(): + """D405 has no separate RGB module; color comes from Stereo Module.""" + config = RealSenseCameraConfig(serial_number_or_name="042") + camera = RealSenseCamera(config) + + stereo = _make_mock_sensor("Stereo Module") + _attach_mock_color_sensor(camera, stereo) + + assert camera._get_color_sensor() is stereo + + +def test_get_color_sensor_raises_with_available_sensors(): + config = RealSenseCameraConfig(serial_number_or_name="042") + camera = RealSenseCamera(config) + + other = _make_mock_sensor("Motion Module") + _attach_mock_color_sensor(camera, other) + + with pytest.raises(RuntimeError, match="Motion Module"): + camera._get_color_sensor() + + +def test_configure_sensor_options_skipped_when_none(): + config = RealSenseCameraConfig(serial_number_or_name="042") + camera = RealSenseCamera(config) + + with patch.object(RealSenseCamera, "_get_color_sensor") as mock_get: + camera._configure_sensor_options() + mock_get.assert_not_called() + + +def test_configure_sensor_options_applies_all_values(): + config = RealSenseCameraConfig(serial_number_or_name="042", exposure=120, gain=64, white_balance=4600) + camera = RealSenseCamera(config) + + sensor = _make_mock_sensor( + "RGB Camera", + supported_options={ + rs.option.enable_auto_exposure, + rs.option.exposure, + rs.option.gain, + rs.option.enable_auto_white_balance, + rs.option.white_balance, + }, + ) + _attach_mock_color_sensor(camera, sensor) + + camera._configure_sensor_options() + + sensor.set_option.assert_any_call(rs.option.enable_auto_exposure, 0) + sensor.set_option.assert_any_call(rs.option.exposure, 120) + sensor.set_option.assert_any_call(rs.option.gain, 64) + sensor.set_option.assert_any_call(rs.option.enable_auto_white_balance, 0) + sensor.set_option.assert_any_call(rs.option.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 pytest.raises(ValueError, match=label): + camera._configure_sensor_options() + + sensor.supports.assert_any_call(option) + sensor.set_option.assert_not_called() + + +@pytest.mark.parametrize( + ("config_field", "option", "value"), + [ + ("exposure", rs.option.exposure, 120), + ("gain", rs.option.gain, 64), + ], +) +def test_configure_sensor_options_exposure_or_gain_disables_auto_exposure(config_field, option, value): + """white_balance=None should not touch auto white balance.""" + config = RealSenseCameraConfig(serial_number_or_name="042", **{config_field: value}) + camera = RealSenseCamera(config) + + sensor = _make_mock_sensor( + "RGB Camera", + supported_options={rs.option.enable_auto_exposure, option}, + ) + _attach_mock_color_sensor(camera, sensor) + + camera._configure_sensor_options() + + calls = [call.args for call in sensor.set_option.call_args_list] + assert (rs.option.enable_auto_exposure, 0) in calls + assert (option, value) in calls + for opt, _ in calls: + assert opt != rs.option.enable_auto_white_balance + assert opt != rs.option.white_balance + + +def test_configure_sensor_options_warns_when_auto_exposure_control_is_unsupported(caplog): + config = RealSenseCameraConfig(serial_number_or_name="042", exposure=120) + camera = RealSenseCamera(config) + + sensor = _make_mock_sensor("RGB Camera", supported_options={rs.option.exposure}) + _attach_mock_color_sensor(camera, sensor) + + with caplog.at_level("WARNING"): + camera._configure_sensor_options() + + sensor.set_option.assert_called_once_with(rs.option.exposure, 120) + assert "does not support disabling auto-exposure" in caplog.text + + +def test_configure_sensor_options_warns_when_auto_white_balance_control_is_unsupported(caplog): + config = RealSenseCameraConfig(serial_number_or_name="042", white_balance=4600) + camera = RealSenseCamera(config) + + sensor = _make_mock_sensor("RGB Camera", supported_options={rs.option.white_balance}) + _attach_mock_color_sensor(camera, sensor) + + with caplog.at_level("WARNING"): + camera._configure_sensor_options() + + sensor.set_option.assert_called_once_with(rs.option.white_balance, 4600) + assert "does not support disabling auto white balance" in caplog.text + + +def test_configure_sensor_options_out_of_range_raises_value_error(): + """set_option errors should be re-raised as ValueError with range diagnostics.""" + config = RealSenseCameraConfig(serial_number_or_name="042", exposure=999999) + camera = RealSenseCamera(config) + + sensor = _make_mock_sensor( + "RGB Camera", + supported_options={rs.option.enable_auto_exposure, rs.option.exposure}, + ) + + def fake_set_option(option, value): + if option == rs.option.exposure: + raise RuntimeError("value out of range") + + sensor.set_option.side_effect = fake_set_option + + option_range = MagicMock(min=1, max=10000, step=1, default=156) + sensor.get_option_range.return_value = option_range + + _attach_mock_color_sensor(camera, sensor) + + with pytest.raises(ValueError, match="exposure") as exc_info: + camera._configure_sensor_options() + + msg = str(exc_info.value) + assert "999999" in msg + assert "min=1" in msg + assert "max=10000" in msg + + @pytest.mark.parametrize( "rotation", [