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