Record each ZED camera's read-back exposure, gain and white balance per episode

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()
+        },
     )
 
 

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``

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})

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())

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:

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

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.

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}

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])