diff --git a/src/lerobot/cameras/realsense/camera_realsense.py b/src/lerobot/cameras/realsense/camera_realsense.py index d58799217..25ea6d519 100644 --- a/src/lerobot/cameras/realsense/camera_realsense.py +++ b/src/lerobot/cameras/realsense/camera_realsense.py @@ -360,11 +360,33 @@ class RealSenseCamera(Camera): 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. Skipped entirely if no options are set. + + 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. """ config = self.config if config.exposure is None and config.gain is None and config.white_balance is None: @@ -375,29 +397,33 @@ class RealSenseCamera(Camera): if (config.exposure is not None or config.gain is not None) and color_sensor.supports( rs.option.enable_auto_exposure ): - color_sensor.set_option(rs.option.enable_auto_exposure, 0) + self._set_sensor_option(color_sensor, rs.option.enable_auto_exposure, 0, "auto-exposure") logger.info(f"{self} auto-exposure disabled.") if config.exposure is not None: if color_sensor.supports(rs.option.exposure): - color_sensor.set_option(rs.option.exposure, config.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.") if config.gain is not None: if color_sensor.supports(rs.option.gain): - color_sensor.set_option(rs.option.gain, config.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.") if config.white_balance is not None: if color_sensor.supports(rs.option.enable_auto_white_balance): - color_sensor.set_option(rs.option.enable_auto_white_balance, 0) + self._set_sensor_option( + 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): - color_sensor.set_option(rs.option.white_balance, config.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.") diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 1deb73f05..343c43157 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" @@ -202,6 +204,162 @@ 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) + + +def test_configure_sensor_options_warns_when_unsupported(caplog): + 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=set()) + _attach_mock_color_sensor(camera, sensor) + + with caplog.at_level("WARNING"): + camera._configure_sensor_options() + + 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(): + """white_balance=None should not touch auto white balance.""" + config = RealSenseCameraConfig(serial_number_or_name="042", exposure=120) + camera = RealSenseCamera(config) + + sensor = _make_mock_sensor( + "RGB Camera", + supported_options={rs.option.enable_auto_exposure, rs.option.exposure}, + ) + _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 (rs.option.exposure, 120) 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_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", [