nav model replay (#26697)

* nav model replay

* set token

* fix gh actions

* update refs

* fix pc
old-commit-hash: 1da6f6a6e2

.github/workflows/selfdrive_tests.yaml

@@ -294,7 +294,7 @@ jobs:
     - name: Run model replay with ONNX
       timeout-minutes: 2
       run: |
-        ${{ env.RUN_CL }} "ONNXCPU=1 CI=1 coverage run selfdrive/test/process_replay/model_replay.py && \
+        ${{ env.RUN_CL }} "ONNXCPU=1 CI=1 NO_NAV=1 coverage run selfdrive/test/process_replay/model_replay.py && \
                            coverage xml"
     - name: Run unit tests
       timeout-minutes: 5

Jenkinsfile

@@ -11,6 +11,7 @@ export SOURCE_DIR=${env.SOURCE_DIR}
 export GIT_BRANCH=${env.GIT_BRANCH}
 export GIT_COMMIT=${env.GIT_COMMIT}
 export AZURE_TOKEN='${env.AZURE_TOKEN}'
+export MAPBOX_TOKEN='${env.MAPBOX_TOKEN}'
 
 source ~/.bash_profile
 if [ -f /TICI ]; then
@@ -47,6 +48,7 @@ pipeline {
     TEST_DIR = "/data/openpilot"
     SOURCE_DIR = "/data/openpilot_source/"
     AZURE_TOKEN = credentials('azure_token')
+    MAPBOX_TOKEN = credentials('mapbox_token')
   }
   options {
     timeout(time: 4, unit: 'HOURS')

selfdrive/navd/map_renderer.cc

@@ -4,10 +4,10 @@
 #include <string>
 #include <QApplication>
 #include <QBuffer>
-#include <QDebug>
 
 #include "common/util.h"
 #include "common/timing.h"
+#include "common/swaglog.h"
 #include "selfdrive/ui/qt/maps/map_helpers.h"
 
 const float DEFAULT_ZOOM = 13.5; // Don't go below 13 or features will start to disappear
@@ -56,22 +56,27 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set
   m_map->setFramebufferObject(fbo->handle(), fbo->size());
   gl_functions->glViewport(0, 0, WIDTH, HEIGHT);
 
+  QObject::connect(m_map.data(), &QMapboxGL::mapLoadingFailed, [=](QMapboxGL::MapLoadingFailure err_code, const QString &reason) {
+    LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str());
+  });
+
   if (online) {
     vipc_server.reset(new VisionIpcServer("navd"));
     vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT);
     vipc_server->start_listener();
 
     pm.reset(new PubMaster({"navThumbnail"}));
-    sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}));
+    sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}, {"liveLocationKalman"}));
 
     timer = new QTimer(this);
+    timer->setSingleShot(true);
     QObject::connect(timer, SIGNAL(timeout()), this, SLOT(msgUpdate()));
-    timer->start(50);
+    timer->start(0);
   }
 }
 
 void MapRenderer::msgUpdate() {
-  sm->update(0);
+  sm->update(1000);
 
   if (sm->updated("liveLocationKalman")) {
     auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
@@ -92,6 +97,9 @@ void MapRenderer::msgUpdate() {
     }
     updateRoute(route);
   }
+
+  // schedule next update
+  timer->start(0);
 }
 
 void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) {

selfdrive/navd/map_renderer.h

@@ -25,7 +25,6 @@ public:
   bool loaded();
   ~MapRenderer();
 
-
 private:
   std::unique_ptr<QOpenGLContext> ctx;
   std::unique_ptr<QOffscreenSurface> surface;

selfdrive/test/process_replay/compare_logs.py

@@ -2,6 +2,7 @@
 import bz2
 import sys
 import math
+import capnp
 import numbers
 import dictdiffer
 from collections import Counter
@@ -30,20 +31,23 @@ def remove_ignored_fields(msg, ignore):
       continue
 
     for k in keys[:-1]:
-      try:
-        attr = getattr(msg, k)
-      except AttributeError:
-        break
-    else:
-      v = getattr(attr, keys[-1])
-      if isinstance(v, bool):
-        val = False
-      elif isinstance(v, numbers.Number):
-        val = 0
+      # indexing into list
+      if k.isdigit():
+        attr = attr[int(k)]
       else:
-        raise NotImplementedError('Error ignoring field')
-      setattr(attr, keys[-1], val)
-  return msg.as_reader()
+        attr = getattr(attr, k)
+
+    v = getattr(attr, keys[-1])
+    if isinstance(v, bool):
+      val = False
+    elif isinstance(v, numbers.Number):
+      val = 0
+    elif isinstance(v, (list, capnp.lib.capnp._DynamicListBuilder)):
+      val = []
+    else:
+      raise NotImplementedError(f"Unknown type: {type(v)}")
+    setattr(attr, keys[-1], val)
+  return msg
 
 
 def get_field_tolerance(diff_field, field_tolerances):
@@ -79,12 +83,12 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
       print(msg1, msg2)
       raise Exception("msgs not aligned between logs")
 
-    msg1_bytes = remove_ignored_fields(msg1, ignore_fields).as_builder().to_bytes()
-    msg2_bytes = remove_ignored_fields(msg2, ignore_fields).as_builder().to_bytes()
+    msg1 = remove_ignored_fields(msg1, ignore_fields)
+    msg2 = remove_ignored_fields(msg2, ignore_fields)
 
-    if msg1_bytes != msg2_bytes:
-      msg1_dict = msg1.to_dict(verbose=True)
-      msg2_dict = msg2.to_dict(verbose=True)
+    if msg1.to_bytes() != msg2.to_bytes():
+      msg1_dict = msg1.as_reader().to_dict(verbose=True)
+      msg2_dict = msg2.as_reader().to_dict(verbose=True)
 
       dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields)
 

selfdrive/test/process_replay/model_replay.py

@@ -20,14 +20,17 @@ from system.version import get_commit
 from tools.lib.framereader import FrameReader
 from tools.lib.logreader import LogReader
 
-TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
-SEGMENT = 0
+TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30"
+SEGMENT = 6
 MAX_FRAMES = 100 if PC else 600
+NAV_FRAMES = 20
 
+NO_NAV = "NO_NAV" in os.environ  # TODO: make map renderer work in CI
 SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0"))
 
 VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
                "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD}
+
 def get_log_fn(ref_commit, test_route):
   return f"{test_route}_model_tici_{ref_commit}.bz2"
 
@@ -39,6 +42,47 @@ def replace_calib(msg, calib):
   return msg
 
 
+def nav_model_replay(lr):
+  pm = messaging.PubMaster(['liveLocationKalman', 'navRoute'])
+  sock = messaging.sub_sock('navModel', conflate=False, timeout=1000)
+
+  log_msgs = []
+  try:
+    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 s in (llk, nav):
+        msg = s[0]
+        pm.send(msg.which(), msg.as_builder().to_bytes())
+      if messaging.recv_one(sock) is not None:
+        break
+    else:
+      raise Exception("no navmodeld outputs")
+
+    time.sleep(2)
+    messaging.drain_sock_raw(sock)
+
+    # 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))
+  finally:
+    managed_processes['mapsd'].stop()
+    managed_processes['navmodeld'].stop()
+
+  return log_msgs
+
+
 def model_replay(lr, frs):
   if not PC:
     spinner = Spinner()
@@ -154,8 +198,10 @@ if __name__ == "__main__":
     'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera"), readahead=True)
   }
 
-  # run replay
+  # run replays
   log_msgs = model_replay(lr, frs)
+  if not NO_NAV:
+    log_msgs += nav_model_replay(lr)
 
   # get diff
   failed = False
@@ -164,15 +210,29 @@ if __name__ == "__main__":
       ref_commit = f.read().strip()
     log_fn = get_log_fn(ref_commit, TEST_ROUTE)
     try:
-      cmp_log = list(LogReader(BASE_URL + log_fn))[:2*MAX_FRAMES]
+      expected_msgs = 2*MAX_FRAMES
+      if not NO_NAV:
+        expected_msgs += NAV_FRAMES
+      cmp_log = list(LogReader(BASE_URL + log_fn))[:expected_msgs]
 
       ignore = [
         'logMonoTime',
         'modelV2.frameDropPerc',
         'modelV2.modelExecutionTime',
         'driverStateV2.modelExecutionTime',
-        'driverStateV2.dspExecutionTime'
+        'driverStateV2.dspExecutionTime',
+        'navModel.dspExecutionTime',
+        'navModel.modelExecutionTime',
       ]
+      if PC:
+        ignore += [
+          'modelV2.laneLines.0.t',
+          'modelV2.laneLines.1.t',
+          'modelV2.laneLines.2.t',
+          'modelV2.laneLines.3.t',
+          'modelV2.roadEdges.0.t',
+          'modelV2.roadEdges.1.t',
+        ]
       # TODO this tolerance is absurdly large
       tolerance = 2.0 if PC else None
       results: Any = {TEST_ROUTE: {}}

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-30efb4238327d723e17a3bda7e7c19c18f8a3b18
+4b2c6516cd460ee443b9006f01233168edf3d170