openpilot: send the cameras' calibration in the hello

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

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:

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

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

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

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

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}

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'

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