From 5f05eb7ad841052a7f226ee937709e7282bb5098 Mon Sep 17 00:00:00 2001 From: Ernie Chu Date: Mon, 2 Nov 2026 17:41:59 -0500 Subject: [PATCH] openpilot: the hello carries the cameras' calibration Once calibrationd has saved a calibrated CalibrationParams, the link's hello sends the wide road camera's rotation from the device and the camera's height (`camera`: wide_from_device_euler, height), read once a link, for server-side code that needs to place what the cameras saw. An optional adapter method, camera_calibration(), reads it; Stock's works for upstream's liveCalibration and a fork's extrinsicsCalibration. A server that predates the key ignores it. Co-Authored-By: Claude Opus 5.5 --- docs/link-protocol.md | 6 ++++++ jetlink/client.py | 7 +++++-- jetlink/openpilot/interface.py | 10 +++++++++- jetlink/openpilot/link.py | 5 ++++- jetlink/openpilot/stock.py | 19 +++++++++++++++++++ tests/openpilot/test_interface.py | 3 ++- tests/openpilot/test_link.py | 7 +++++++ tests/openpilot/test_stock.py | 27 +++++++++++++++++++++++++++ tests/test_client.py | 16 ++++++++++++++++ 9 files changed, 95 insertions(+), 5 deletions(-) diff --git a/docs/link-protocol.md b/docs/link-protocol.md index 6c5b704c..f6a43d11 100644 --- a/docs/link-protocol.md +++ b/docs/link-protocol.md @@ -29,6 +29,12 @@ bench tools). There is one version: update the comma and Jetlink together. `fetches` says whether the server downloads a model itself when asked: a default route and no download failed for want of the internet in the last 10 minutes. +- **Cameras.** Once the comma is calibrated, its hello also carries what + calibrationd saved of its cameras (`camera`: `wide_from_device_euler`, the + wide road camera's roll, pitch and yaw from the device, and `height`, the + camera's height above the road), read once a link, for whatever on the + server needs to place what the cameras saw. A server that predates it + ignores the key. - **Session.** The comma says hello, asks for its model's engine (with `fetch` when it has not got the model either: the server then downloads it, state `downloading`, stage `download`, and builds it; a server that cannot diff --git a/jetlink/client.py b/jetlink/client.py index 35ea3f93..a202d293 100644 --- a/jetlink/client.py +++ b/jetlink/client.py @@ -224,17 +224,20 @@ def _expect(self, msg_type: int, seq: int, timeout: float | None) -> Message: # -- handshake ------------------------------------------------------------ - def hello(self, timeout: float = 5.0, pick: dict | None = None) -> dict: + def hello(self, timeout: float = 5.0, pick: dict | None = None, camera: dict | None = None) -> dict: """Introduce this client. The server starts its session over on a hello, so this is also how a new owner of the gadget takes over one the server never saw end; see Session.greet in JetlinkServer. `pick` is the comma's own big-model pick with its age and its date by the comma's clock, {ref, sha256, nbytes, name, age_s, at}; the answer's `pick` is the newer of it - and the server's (PickStore).""" + and the server's (PickStore). `camera` is what the comma's calibration + says of its cameras, {wide_from_device_euler: [roll, pitch, yaw], height}.""" seq = self._next_seq() client = {'nonce': self.nonce, 'name': self.name} if pick is not None: client['pick'] = pick + if camera: + client['camera'] = camera try: link = self.t.link_info() except Exception: diff --git a/jetlink/openpilot/interface.py b/jetlink/openpilot/interface.py index f0fa5e13..8c23f2cb 100644 --- a/jetlink/openpilot/interface.py +++ b/jetlink/openpilot/interface.py @@ -160,6 +160,14 @@ def small_reset(self, small: Any) -> Callable[[], None] | None: frame thread at a hand-back, so it zeroes the history in place and allocates and compiles nothing; warp.reset_queues makes one.""" + def camera_calibration(self) -> dict | None: + """Optional (OPTIONAL): what calibrationd saved of the cameras + (CalibrationParams), {wide_from_device_euler: [roll, pitch, yaw] of the + wide road camera from the device in radians, height: the camera's height + above the road in metres}, or None before it is calibrated. Read once a + link; the hello carries it, for the server to place what the cameras saw. + Never raises.""" + def model_warp(self, small: Any, cam_w: int, cam_h: int, model_w: int, model_h: int) -> FrameWarp | None: """Optional (OPTIONAL): the warp the link sends frames through, warmed, or None for openpilot's driving warp pickle at warp_path (warp.Warp). For @@ -237,7 +245,7 @@ def wait(self) -> None: # the protocols, where a name would be a member itself; a type checker still # asks for these, as typing has no optional member OPTIONAL: dict[type, tuple[str, ...]] = { - ModelSide: ('small_reset', 'model_warp'), + ModelSide: ('small_reset', 'model_warp', 'camera_calibration'), SmallModel: ('desire_key', 'numpy_inputs', 'lat_delay', 'PLANPLUS_CONTROL'), JoinedModel: ('get_action_from_model', 'constants', 'LAT_SMOOTH_SECONDS', 'LONG_SMOOTH_SECONDS', 'lat_delay'), } diff --git a/jetlink/openpilot/link.py b/jetlink/openpilot/link.py index ece06087..b72d3889 100644 --- a/jetlink/openpilot/link.py +++ b/jetlink/openpilot/link.py @@ -358,7 +358,10 @@ def open_link(parts, link: Link, should_stop=None): client = connect_patiently(link) try: sent = parts.models.hello_pick() - hello = parts.models.heard(client.hello(timeout=10.0, pick=sent), sent) + # the cameras' calibration, where the adapter knows it, for the server + calibration = getattr(parts.op, 'camera_calibration', lambda: None)() + camera = {'camera': calibration} if calibration else {} + hello = parts.models.heard(client.hello(timeout=10.0, pick=sent, **camera), sent) parts.log.warning("jetlink: %s trt %s, engine %s, loaded %s", hello.get('device'), hello.get('trt_version'), hello.get('engine_state'), str(hello.get('loaded'))[:16]) diff --git a/jetlink/openpilot/stock.py b/jetlink/openpilot/stock.py index 776fdfb6..c60b9f4e 100644 --- a/jetlink/openpilot/stock.py +++ b/jetlink/openpilot/stock.py @@ -145,6 +145,25 @@ def chestnut_present(self) -> bool: from jetlink.comma.port import chestnut_attached return chestnut_attached(self.CHESTNUT_IDS) + def camera_calibration(self) -> dict | None: + # calibrationd's saved calibration: liveCalibration upstream, + # extrinsicsCalibration in a fork that renamed it + try: + from openpilot.cereal import log + raw = self._params().get('CalibrationParams') + if not raw: + return None + with log.Event.from_bytes(raw) as msg: + cal = getattr(msg, msg.which()) + if str(cal.calStatus) != 'calibrated': + return None + euler, height = [float(v) for v in cal.wideFromDeviceEuler], [float(v) for v in cal.height] + if len(euler) != 3 or len(height) != 1 or not height[0] > 0: + return None + return {'wide_from_device_euler': euler, 'height': height[0]} + except Exception: + return None + def camera(self) -> tuple[int, int, int, int]: # modeld takes the size from camerad's buffer; a comma four's os04c10 is # the 1344x760 one, every other device's the 1928x1208 diff --git a/tests/openpilot/test_interface.py b/tests/openpilot/test_interface.py index 2940fe58..57b0df01 100644 --- a/tests/openpilot/test_interface.py +++ b/tests/openpilot/test_interface.py @@ -47,6 +47,7 @@ 'event': '(name: str, **fields: Any) -> None', 'small_reset': '(small: Any) -> Callable[[], None] | None', 'model_warp': '(small: Any, cam_w: int, cam_h: int, model_w: int, model_h: int) -> FrameWarp | None', + 'camera_calibration': '() -> dict | None', } SIDES = {'StatusSide': READERS, 'WorkerSide': WORKER, 'ModelSide': MODELD, 'Openpilot': MODELD} # what modeld hands attach, and what it reads off and writes onto the result @@ -59,7 +60,7 @@ WARP = {'output': None, 'start': '(frame: int, big_frame: int, tfm: Any, big_tfm: Any) -> None', 'wait': '() -> None'} MODELS = {'SmallModel': SMALL, 'JoinedModel': JOINED, 'FrameWarp': WARP} # the members an implementation may lack: jetlink reads them with getattr and a fallback -OPTIONAL = {'ModelSide': {'small_reset', 'model_warp'}, 'Openpilot': {'small_reset', 'model_warp'}, +OPTIONAL = {'ModelSide': {'small_reset', 'model_warp', 'camera_calibration'}, 'Openpilot': {'small_reset', 'model_warp', 'camera_calibration'}, 'SmallModel': {'desire_key', 'numpy_inputs', 'lat_delay', 'PLANPLUS_CONTROL'}, 'JoinedModel': {'get_action_from_model', 'constants', 'LAT_SMOOTH_SECONDS', 'LONG_SMOOTH_SECONDS', 'lat_delay'}} diff --git a/tests/openpilot/test_link.py b/tests/openpilot/test_link.py index 9b69f91e..7319734a 100644 --- a/tests/openpilot/test_link.py +++ b/tests/openpilot/test_link.py @@ -314,6 +314,13 @@ def test_the_hello_carries_our_pick(self): link.open_link(self.parts, self.link) self.client.hello.assert_called_once_with(timeout=10.0, pick={'ref': 'f' * 40, 'age_s': 5.0}) + def test_the_hello_carries_the_cameras_calibration_where_the_adapter_knows_it(self): + self.patch(self.parts.models, 'hello_pick', return_value=None) + calibration = {'wide_from_device_euler': [0.0, 0.0217, -0.0161], 'height': 1.374} + self.parts.op.camera_calibration = lambda: calibration + link.open_link(self.parts, self.link) + self.client.hello.assert_called_once_with(timeout=10.0, pick=None, camera=calibration) + def test_a_newer_pick_on_the_server_is_what_drives_and_is_ours_now(self): # picked on the Jetson's web page while parked: it drives at the next join theirs = {'name': 'Cinque Terre V3', 'ref': 'e' * 40, 'oid': 'e' * 64, 'size': 99} diff --git a/tests/openpilot/test_stock.py b/tests/openpilot/test_stock.py index c683e532..1318a40a 100644 --- a/tests/openpilot/test_stock.py +++ b/tests/openpilot/test_stock.py @@ -69,6 +69,16 @@ def remove(self, key): self.world.params.pop(key, None) +class FakeEvent: + """cereal's log.Event over JSON: {which: {field: value}}, as calibrationd saves one in CalibrationParams.""" + + @staticmethod + def from_bytes(raw): + which, fields = next(iter(json.loads(raw).items())) + msg = SimpleNamespace(which=lambda: which, **{which: SimpleNamespace(**fields)}) + return mock.MagicMock(__enter__=lambda self: msg, __exit__=lambda *a: False) + + class FakeSubMaster(testing.FakeSubMaster): """The kit's, over the world's messages, failing as the world says.""" @@ -150,6 +160,7 @@ def parse_outputs(self, outs): ('openpilot.selfdrive.modeld.parse_model_outputs', 'Parser'): Parser, ('openpilot.system.camerad.cameras.nv12_info', 'get_nv12_info'): nv12_info, ('openpilot.cereal', 'messaging'): None, # the module below + ('openpilot.cereal', 'log'): SimpleNamespace(Event=FakeEvent), } def modules(self) -> dict[str, ModuleType]: @@ -270,6 +281,22 @@ def test_params_dir_without_home_is_the_c_plus_pluses(self): class TestDevice(StockTest): + def test_camera_calibration_is_calibrationds_saved_one(self): + self.world.declared.add('CalibrationParams') + assert self.op.camera_calibration() is None, 'not yet calibrated: nothing saved' + for which in ('liveCalibration', 'extrinsicsCalibration'): # upstream's name, and a fork's + self.world.params['CalibrationParams'] = json.dumps( + {which: {'calStatus': 'calibrated', 'wideFromDeviceEuler': [0.0, 0.0217, -0.0161], 'height': [1.374]}}).encode() + assert self.op.camera_calibration() == {'wide_from_device_euler': [0.0, 0.0217, -0.0161], 'height': 1.374} + self.world.params['CalibrationParams'] = json.dumps( + {'liveCalibration': {'calStatus': 'uncalibrated', 'wideFromDeviceEuler': [0.0, 0.0, 0.0], 'height': [1.22]}}).encode() + assert self.op.camera_calibration() is None, 'only a calibrated one' + self.world.params['CalibrationParams'] = json.dumps( + {'liveCalibration': {'calStatus': 'calibrated', 'wideFromDeviceEuler': [0.0, 0.0217, -0.0161], 'height': []}}).encode() + assert self.op.camera_calibration() is None, 'only a whole one' + self.world.params['CalibrationParams'] = b'not an event' + assert self.op.camera_calibration() is None, 'never raises' + def test_camera_by_device(self): assert self.op.camera() == fakes.TICI self.world.device = 'mici' diff --git a/tests/test_client.py b/tests/test_client.py index 9f22e1e0..64b0a057 100644 --- a/tests/test_client.py +++ b/tests/test_client.py @@ -205,6 +205,22 @@ def serve(): self.assertEqual(self.client.waiting_for(), 0.0) self.assertIsNone(self.client.last_output) + def test_a_hello_says_what_the_comma_knows_of_its_cameras(self): + said = [] + + def serve(): + for _ in range(2): + msg = self.peer.recv(timeout=2.0) + said.append(json.loads(bytes(msg.payload))['client']) + self.peer.send_json(P.Msg.HELLO_RESP, msg.seq, {'device': 'test'}) + t = threading.Thread(target=serve, daemon=True) + t.start() + self.client.hello(timeout=2.0, camera={'wide_from_device_euler': [0.0, 0.02, -0.016]}) + self.client.hello(timeout=2.0) + t.join(2.0) + self.assertEqual(said[0]['camera'], {'wide_from_device_euler': [0.0, 0.02, -0.016]}) + self.assertNotIn('camera', said[1], 'a comma that knows nothing says nothing') + if __name__ == '__main__': unittest.main()