navmodel: check images in replay test (#26747)

* move position

* log images

* enable test

* update refs

* ignore

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: f3a711953afea1fcebb4c10b1e2462c4032ac147

selfdrive/navd/map_renderer.cc

@@ -17,6 +17,9 @@ const int NUM_VIPC_BUFFERS = 4;
 const int EARTH_CIRCUMFERENCE_METERS = 40075000;
 const int PIXELS_PER_TILE = 256;
 
+const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE");
+const int LLK_DECIMATION = TEST_MODE ? 1 : 10;
+
 float get_zoom_level_for_scale(float lat, float meters_per_pixel) {
   float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE;
   float num_tiles = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / meters_per_tile;
@@ -89,7 +92,7 @@ void MapRenderer::msgUpdate() {
     auto orientation = location.getCalibratedOrientationNED();
 
     bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid();
-    if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % 10) == 0) {
+    if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) {
       updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2]));
 
       // TODO: use the static rendering mode
@@ -151,6 +154,15 @@ void MapRenderer::update() {
   }
 }
 
+void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf) {
+  MessageBuilder msg;
+  auto thumbnaild = msg.initEvent().initNavThumbnail();
+  thumbnaild.setFrameId(frame_id);
+  thumbnaild.setTimestampEof(ts);
+  thumbnaild.setThumbnail(buf);
+  pm->send("navThumbnail", msg);
+}
+
 void MapRenderer::publish(const double render_time) {
   QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
   uint64_t ts = nanos_since_boot();
@@ -173,7 +185,12 @@ void MapRenderer::publish(const double render_time) {
 
   vipc_server->send(buf, &extra);
 
-  if (frame_id % 100 == 0) {
+  // Send thumbnail
+  if (TEST_MODE) {
+    // Full image in thumbnails in test mode
+    kj::Array<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((const capnp::byte*)cap.bits(), cap.sizeInBytes());
+    sendThumbnail(ts, buffer_kj);
+  } else if (frame_id % 100 == 0) {
     // Write jpeg into buffer
     QByteArray buffer_bytes;
     QBuffer buffer(&buffer_bytes);
@@ -181,14 +198,7 @@ void MapRenderer::publish(const double render_time) {
     cap.save(&buffer, "JPG", 50);
 
     kj::Array<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size());
-
-    // Send thumbnail
-    MessageBuilder msg;
-    auto thumbnaild = msg.initEvent().initNavThumbnail();
-    thumbnaild.setFrameId(frame_id);
-    thumbnaild.setTimestampEof(ts);
-    thumbnaild.setThumbnail(buffer_kj);
-    pm->send("navThumbnail", msg);
+    sendThumbnail(ts, buffer_kj);
   }
 
   // Send state msg

selfdrive/navd/map_renderer.h

@@ -35,6 +35,7 @@ private:
   std::unique_ptr<PubMaster> pm;
   std::unique_ptr<SubMaster> sm;
   void publish(const double render_time);
+  void sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf);
 
   QMapboxGLSettings m_settings;
   QScopedPointer<QMapboxGL> m_map;

selfdrive/test/process_replay/compare_logs.py

@@ -80,7 +80,7 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
   diff = []
   for msg1, msg2 in zip(log1, log2):
     if msg1.which() != msg2.which():
-      print(msg1, msg2)
+      print(msg1.which(), msg2.which())
       raise Exception("msgs not aligned between logs")
 
     msg1 = remove_ignored_fields(msg1, ignore_fields)

selfdrive/test/process_replay/model_replay.py

@@ -23,7 +23,7 @@ from tools.lib.logreader import LogReader
 TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30"
 SEGMENT = 6
 MAX_FRAMES = 100 if PC else 600
-NAV_FRAMES = 20
+NAV_FRAMES = 50
 
 NO_NAV = "NO_NAV" in os.environ  # TODO: make map renderer work in CI
 SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0"))
@@ -43,39 +43,46 @@ def replace_calib(msg, calib):
 
 
 def nav_model_replay(lr):
+  sm = messaging.SubMaster(['navModel', 'navThumbnail'])
   pm = messaging.PubMaster(['liveLocationKalman', 'navRoute'])
-  sock = messaging.sub_sock('navModel', conflate=False, timeout=1000)
+
+  nav = [m for m in lr if m.which() == 'navRoute']
+  llk = [m for m in lr if m.which() == 'liveLocationKalman']
+  assert len(nav) > 0 and len(llk) >= NAV_FRAMES
 
   log_msgs = []
   try:
+    assert "MAPBOX_TOKEN" in os.environ
+    os.environ['MAP_RENDER_TEST_MODE'] = '1'
     managed_processes['mapsd'].start()
     managed_processes['navmodeld'].start()
 
     # setup position and route
-    nav = [m for m in lr if m.which() == 'navRoute']
-    llk = [m for m in lr if m.which() == 'liveLocationKalman']
-    assert len(nav) > 0 and len(llk) > 0
-
-    for _ in range(30):
+    for _ in range(10):
       for s in (llk, nav):
-        msg = s[0]
-        pm.send(msg.which(), msg.as_builder().to_bytes())
-      if messaging.recv_one(sock) is not None:
+        pm.send(s[0].which(), s[0].as_builder().to_bytes())
+      sm.update(1000)
+      if sm.updated['navModel']:
         break
-    else:
-      raise Exception("no navmodeld outputs")
+      time.sleep(1)
 
+    if not sm.updated['navModel']:
+      raise Exception("no navmodeld outputs, failed to initialize")
+
+    # drain
     time.sleep(2)
-    messaging.drain_sock_raw(sock)
+    sm.update(0)
 
     # run replay
-    for _ in range(NAV_FRAMES):
-      # 2Hz decimation
-      for _ in range(10):
-        pm.send(llk[0].which(), llk[0].as_builder().to_bytes())
-        time.sleep(0.1)
-      with Timeout(5, "timed out waiting for nav model outputs"):
-        log_msgs.append(messaging.recv_one_retry(sock))
+    for n in range(NAV_FRAMES):
+      pm.send(llk[n].which(), llk[n].as_builder().to_bytes())
+      m = messaging.recv_one(sm.sock['navThumbnail'])
+      assert m is not None, f"no navThumbnail, frame={n}"
+      log_msgs.append(m)
+
+      m = messaging.recv_one(sm.sock['navModel'])
+      assert m is not None, f"no navModel response, frame={n}"
+      log_msgs.append(m)
   finally:
     managed_processes['mapsd'].stop()
     managed_processes['navmodeld'].stop()
@@ -212,7 +219,7 @@ if __name__ == "__main__":
     try:
       expected_msgs = 2*MAX_FRAMES
       if not NO_NAV:
-        expected_msgs += NAV_FRAMES
+        expected_msgs += NAV_FRAMES*2
       cmp_log = list(LogReader(BASE_URL + log_fn))[:expected_msgs]
 
       ignore = [
@@ -223,6 +230,7 @@ if __name__ == "__main__":
         'driverStateV2.dspExecutionTime',
         'navModel.dspExecutionTime',
         'navModel.modelExecutionTime',
+        'navThumbnail.timestampEof',
       ]
       if PC:
         ignore += [

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-4b2c6516cd460ee443b9006f01233168edf3d170
+30dfeed50ac562f99b0aad985bd6b0e6d8188fcb