From 963978071c2f81c3a082b520e69d564c42b7a56d Mon Sep 17 00:00:00 2001 From: Vladimir Yakunin Date: Fri, 11 Sep 2026 21:09:27 +0000 Subject: [PATCH] Record each ZED camera's read-back exposure, gain and white balance per episode The frames of an episode are kept, so any pixel statistic can be computed later. The exposure time, the gain and the white-balance temperature that the camera's automatic control chose are lost when the episode ends. This change records them. `SLCamera` reads the values back from the SDK once per `state_period_sec` (default 1 s) and emits them on a new `state` signal. The values come from `get_camera_settings`, so they are what the camera runs at, not the configured set points. The two `auto_*` flags say whether the values beside them are the camera's own choice. `Embodiment` gains `recorded`: signals the recorder writes and the policy never reads. The DROID embodiment lists each camera's state there, so an episode carries `camera_state..exposure`, `.gain`, `.white_balance_temperature`, `.auto_exposure` and `.auto_white_balance` as scalar signals. The recorder's opening turn writes the value each camera holds at episode start; the samples during the episode follow at the period. Ticket: Positronic-Robotics/internal#1298 #refs --- positronic/cfg/embodiment.py | 3 ++ positronic/dataset/serializers.py | 5 +++ positronic/drivers/camera/tests/conftest.py | 36 +++++++++++++++++ positronic/drivers/camera/tests/test_zed.py | 45 +++++++++++++++++++++ positronic/drivers/camera/zed.py | 35 ++++++++++++++++ positronic/eval/__init__.py | 3 ++ positronic/keys.py | 10 +++++ positronic/policy/tests/test_harness.py | 25 ++++++++++++ positronic/wire.py | 7 +++- 9 files changed, 167 insertions(+), 2 deletions(-) create mode 100644 positronic/drivers/camera/tests/conftest.py create mode 100644 positronic/drivers/camera/tests/test_zed.py diff --git a/positronic/cfg/embodiment.py b/positronic/cfg/embodiment.py index 4dacebe9f..d5a10ae29 100644 --- a/positronic/cfg/embodiment.py +++ b/positronic/cfg/embodiment.py @@ -34,6 +34,9 @@ def droid(robot_arm, gripper, cameras): meta_source=robot_arm.robot_meta, control_systems=(*cameras.values(), robot_arm, gripper), simulated=False, + recorded={ + keys.camera_state(name): Observation(cam.state, Serializers.camera_state) for name, cam in cameras.items() + }, ) diff --git a/positronic/dataset/serializers.py b/positronic/dataset/serializers.py index 7ab418677..d24e98b12 100644 --- a/positronic/dataset/serializers.py +++ b/positronic/dataset/serializers.py @@ -141,6 +141,11 @@ def camera_images(data: pimm.shared_memory.NumpySMAdapter) -> np.ndarray: """Extract array from NumpySMAdapter for storage.""" return data.array + @staticmethod + def camera_state(data: dict[str, int]) -> dict[str, int]: + """Record each read-back camera setting as its own scalar signal: ``camera_state.wrist.exposure``.""" + return {f'.{name}': value for name, value in data.items()} + def expand_suffixed(name: str, value: Any) -> Iterator[tuple[str, Any]]: """Unfold a value into ``(full_name, value)`` pairs: a dict expands into ``name + suffix`` diff --git a/positronic/drivers/camera/tests/conftest.py b/positronic/drivers/camera/tests/conftest.py new file mode 100644 index 000000000..8edc90901 --- /dev/null +++ b/positronic/drivers/camera/tests/conftest.py @@ -0,0 +1,36 @@ +"""Stand-in for the ZED vendor package. + +``pyzed`` installs with the ZED SDK and nowhere else, so the driver cannot be imported from a default sync. +The tests read the driver's state against their own fake camera and never call into the vendor, so a module +carrying the names the driver binds at import is enough. Installed here, before any test module imports the +driver, and only when the real package is absent. +""" + +import importlib.util +import sys +import types +from enum import Enum + +PACKAGE = 'pyzed' +SL = f'{PACKAGE}.sl' + + +class ErrorCode(Enum): + SUCCESS = 0 + FAILURE = 1 + + +class VideoSettings(Enum): + EXPOSURE = 0 + GAIN = 1 + WHITEBALANCE_TEMPERATURE = 2 + AEC_AGC = 3 + WHITEBALANCE_AUTO = 4 + + +if importlib.util.find_spec(PACKAGE) is None: + sl = types.ModuleType(SL) + sl.__dict__.update(ERROR_CODE=ErrorCode, VIDEO_SETTINGS=VideoSettings) + package = types.ModuleType(PACKAGE) + package.__dict__.update(sl=sl) + sys.modules.update({PACKAGE: package, SL: sl}) diff --git a/positronic/drivers/camera/tests/test_zed.py b/positronic/drivers/camera/tests/test_zed.py new file mode 100644 index 000000000..1d9c26fbf --- /dev/null +++ b/positronic/drivers/camera/tests/test_zed.py @@ -0,0 +1,45 @@ +from positronic.drivers.camera import zed +from positronic.drivers.camera.zed import CAMERA_STATE_SETTINGS, read_camera_state + +# The SDK namespace the driver bound at import: the vendor's when installed, the conftest's stand-in otherwise. +sl = zed.sl + + +class FakeCamera: + """Answers each setting with the value it holds, and refuses the ones it does not.""" + + def __init__(self, values: dict): + self._values = values + + def get_camera_settings(self, setting) -> tuple: + if setting in self._values: + return sl.ERROR_CODE.SUCCESS, self._values[setting] + return sl.ERROR_CODE.FAILURE, -1 + + +def test_read_camera_state_reports_every_setting_the_camera_answers(): + camera = FakeCamera({ + sl.VIDEO_SETTINGS.EXPOSURE: 45, + sl.VIDEO_SETTINGS.GAIN: 12, + sl.VIDEO_SETTINGS.WHITEBALANCE_TEMPERATURE: 4700, + sl.VIDEO_SETTINGS.AEC_AGC: 1, + sl.VIDEO_SETTINGS.WHITEBALANCE_AUTO: 1, + }) + + assert read_camera_state(camera) == { + 'exposure': 45, + 'gain': 12, + 'white_balance_temperature': 4700, + 'auto_exposure': 1, + 'auto_white_balance': 1, + } + + +def test_read_camera_state_leaves_out_a_setting_the_camera_refuses(): + camera = FakeCamera({sl.VIDEO_SETTINGS.EXPOSURE: 45}) + + assert read_camera_state(camera) == {'exposure': 45} + + +def test_every_recorded_setting_names_a_video_setting(): + assert all(hasattr(sl.VIDEO_SETTINGS, sdk_name) for sdk_name in CAMERA_STATE_SETTINGS.values()) diff --git a/positronic/drivers/camera/zed.py b/positronic/drivers/camera/zed.py index 4057736c7..9855efc2b 100644 --- a/positronic/drivers/camera/zed.py +++ b/positronic/drivers/camera/zed.py @@ -14,6 +14,27 @@ logger = logging.getLogger(__name__) +# The settings a camera's automatic control moves, keyed by the name each records under, valued by the SDK's +# ``VIDEO_SETTINGS`` member. The two ``auto_*`` flags say whether the values beside them are the sensor's own +# choice or a set point. +CAMERA_STATE_SETTINGS = { + 'exposure': 'EXPOSURE', + 'gain': 'GAIN', + 'white_balance_temperature': 'WHITEBALANCE_TEMPERATURE', + 'auto_exposure': 'AEC_AGC', + 'auto_white_balance': 'WHITEBALANCE_AUTO', +} + + +def read_camera_state(zed) -> dict[str, int]: + """What the camera reports for each of ``CAMERA_STATE_SETTINGS`` now. A setting the SDK refuses is left out.""" + state = {} + for name, sdk_name in CAMERA_STATE_SETTINGS.items(): + error_code, value = zed.get_camera_settings(getattr(sl.VIDEO_SETTINGS, sdk_name)) + if error_code == sl.ERROR_CODE.SUCCESS: + state[name] = int(value) + return state + class SLCamera(pimm.ControlSystem): def __init__( @@ -30,6 +51,7 @@ def __init__( max_recovery_time_sec: float = 10, image_enhancement: bool = False, mono: bool = False, + state_period_sec: float = 1.0, ): """ StereoLabs camera driver. @@ -46,6 +68,10 @@ def __init__( max_recovery_time_sec: (float) Maximum time to wait for camera recovery. If exceeded, will stop the camera. mono: (bool) Open a single-sensor camera (e.g. ZED X One) via ``sl.CameraOne``. Mono cameras support only ``view='left'``, ``depth_mode='none'`` and no image enhancement. + state_period_sec: (float) How often ``state`` reports the exposure, gain and white balance the + camera runs at. Automatic control moves them as the scene changes, so one reading per + episode is not enough; each reading is a control request to the camera, so once a frame + is too many. """ super().__init__() # IMPORTANT: This control system may be spawned under multiprocessing "spawn". @@ -59,6 +85,7 @@ def __init__( self._image_enhancement = image_enhancement self._depth_mask_requested = depth_mask self._mono = mono + self._state_period_sec = state_period_sec self.max_depth = max_depth self.max_recovery_time_sec = max_recovery_time_sec @@ -74,6 +101,9 @@ def __init__( self.depth_mask: pimm.SignalEmitter = pimm.ControlSystemEmitter(self) self._depth_mask_adapter = None # Lazy init + # The exposure, gain and white balance the camera runs at, read back from it every ``state_period_sec``. + self.state: pimm.SignalEmitter[dict[str, int]] = pimm.ControlSystemEmitter(self) + @staticmethod def _open_under_device_lock(zed, init_params) -> Iterator[pimm.Sleep]: """Open the camera, retrying: the lock binds only openers that take it, so an open can still lose the bus.""" @@ -132,6 +162,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p yield from self._open_under_device_lock(zed, init_params) self.recovery_start_time = None + next_state_read = clock.now() while not should_stop.value: result = zed.grab() @@ -149,6 +180,10 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p logger.info(f'Camera recovered after {clock.now() - self.recovery_start_time:.2f} seconds') self.recovery_start_time = None + if clock.now() >= next_state_read: + self.state.emit(read_camera_state(zed)) + next_state_read = clock.now() + self._state_period_sec + image = sl.Mat() ts_s = zed.get_timestamp(TIME_REF_IMAGE).get_nanoseconds() / 1e9 if zed.retrieve_image(image, view) == SUCCESS: diff --git a/positronic/eval/__init__.py b/positronic/eval/__init__.py index 4e8f2872a..26a9431c2 100644 --- a/positronic/eval/__init__.py +++ b/positronic/eval/__init__.py @@ -60,6 +60,9 @@ class Embodiment: meta_source: pimm.ControlSystemEmitter | None control_systems: tuple[pimm.ControlSystem, ...] = () simulated: bool = False + # What a device reports about itself during an episode, recorded beside the observations and never fed + # to the policy: a camera's read-back exposure, gain and white balance. + recorded: dict[str, Observation] = field(default_factory=dict) @dataclass diff --git a/positronic/keys.py b/positronic/keys.py index d03c2d5f9..ca916748f 100644 --- a/positronic/keys.py +++ b/positronic/keys.py @@ -46,6 +46,16 @@ def is_robot_command(name: str) -> bool: WRIST_IMAGE = f'{IMAGE_PREFIX}wrist' EXTERIOR_IMAGE = f'{IMAGE_PREFIX}exterior' EXTERIOR_IMAGE_2 = f'{IMAGE_PREFIX}exterior_2' +# What a camera's own control chose, read back from the sensor and recorded beside its frames: the exposure, +# the gain and the white-balance temperature an automatic mode settled on. The config says "auto"; this says +# what auto chose, and the frames cannot reproduce it once the episode ends. +CAMERA_STATE_PREFIX = 'camera_state.' + + +def camera_state(image_key: str) -> str: + """The signal a camera's read-back state records under: ``image.wrist`` records as ``camera_state.wrist``.""" + return f'{CAMERA_STATE_PREFIX}{image_key.removeprefix(IMAGE_PREFIX)}' + # The harness stamps each observation with the control clock's time (``OBS_TIME_NS``) and the wall # clock's (``WALL_TIME_NS``); recording timelines and action scheduling read time back off them. diff --git a/positronic/policy/tests/test_harness.py b/positronic/policy/tests/test_harness.py index b05bed4c8..7c140ff4d 100644 --- a/positronic/policy/tests/test_harness.py +++ b/positronic/policy/tests/test_harness.py @@ -2536,3 +2536,28 @@ def test_finishing_discards_a_call_that_is_still_in_flight(world): drive_scheduler(world.start([harness, driver, _Pacer()]), steps=2000) assert not _emitted_commands(cmd_recorder) + + +def test_a_recorded_signal_reaches_the_recorder_and_not_the_policy(world, tmp_path): + """A device read-back the embodiment lists under ``recorded`` is a recorder input under its own name, expanded + as its serializer says, and no observation the harness reads.""" + device = _FrameIndexDevice() + embodiment = Embodiment( + descriptor='', + observations={'frame': Observation(device.state, None)}, + commands={keys.ROBOT_COMMAND: Command(device.cmd, None)}, + prepare_handlers={}, + static_meta={}, + meta_source=device.meta, + control_systems=(device,), + recorded={keys.camera_state(CAM): Observation(device.state, Serializers.camera_state)}, + ) + harness = Harness(embodiment) + + ds_agent = wire.wire_embodiment(world, harness, embodiment) + + assert ds_agent is not None + assert keys.camera_state(CAM) == 'camera_state.cam' + assert set(ds_agent.inputs) == {'frame', keys.ROBOT_COMMAND, 'camera_state.cam'} + assert set(harness.observations) == {'frame'} + assert Serializers.camera_state({'exposure': 45, 'gain': 12}) == {'.exposure': 45, '.gain': 12} diff --git a/positronic/wire.py b/positronic/wire.py index d49225f94..3df060d37 100644 --- a/positronic/wire.py +++ b/positronic/wire.py @@ -71,8 +71,8 @@ def wire( # noqa: C901 def _recorder( world: pimm.World, harness: Harness, embodiment: Embodiment, time_mode: TimeMode, privileged: dict[str, Observation] ) -> DsWriterAgent: - """An embodiment's observations, command chunks and privileged ground-truth, recorded into the dataset - each episode names.""" + """An embodiment's observations, command chunks, device read-backs and privileged ground-truth, recorded + into the dataset each episode names.""" ds_agent = DsWriterAgent( LocalDatasetWriter, time_mode=time_mode, @@ -87,6 +87,9 @@ def _recorder( for name, cmd in embodiment.commands.items(): ds_agent.add_signal(name, cmd.serializer) world.connect(harness.commands[name], ds_agent.inputs[name]) + for name, obs in embodiment.recorded.items(): + ds_agent.add_signal(name, obs.serializer) + world.connect(obs.source, ds_agent.inputs[name]) for name, priv in privileged.items(): ds_agent.add_signal(name, priv.serializer) world.connect(priv.source, ds_agent.inputs[name])