From 99945aded5caa40b7972e5955dce35a08090fb8b Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Sun, 16 Jul 2023 15:37:13 -0700
Subject: [PATCH] nav: always publish map and run model (#28924)

* always render

* more test

* test case

* update test

* vipc checks

* clear cache

---------

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 5b86268635a595602d0b3eae5a22e95119c3d860
---
 cereal                                    |  2 +-
 selfdrive/navd/map_renderer.cc            | 23 +++++++++--------
 selfdrive/navd/map_renderer.h             |  4 +++
 selfdrive/navd/tests/test_map_renderer.py | 30 +++++++++++++++++++++--
 4 files changed, 46 insertions(+), 13 deletions(-)

diff --git a/cereal b/cereal
index a2f1f0cb8d..5a822f43ca 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit a2f1f0cb8dd45ea4265255855da7de8fd89156ed
+Subproject commit 5a822f43cae3ee77eea6ecb8c63c398da82142b0
diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc
index dee99e66b0..539783fff4 100644
--- a/selfdrive/navd/map_renderer.cc
+++ b/selfdrive/navd/map_renderer.cc
@@ -105,18 +105,20 @@ void MapRenderer::msgUpdate() {
       float bearing = RAD2DEG(orientation.getValue()[2]);
       updatePosition(get_point_along_line(pos.getValue()[0], pos.getValue()[1], bearing, MAP_OFFSET), bearing);
 
-      // TODO: use the static rendering mode
-      if (!loaded() && frame_id > 0) {
-        for (int i = 0; i < 5 && !loaded(); i++) {
-          LOGW("map render retry #%d, %d", i+1, m_map.isNull());
-          QApplication::processEvents(QEventLoop::AllEvents, 100);
-          update();
+      // TODO: use the static rendering mode instead
+      // retry render a few times
+      for (int i = 0; i < 5 && !rendered(); i++) {
+        QApplication::processEvents(QEventLoop::AllEvents, 100);
+        update();
+        if (rendered()) {
+          LOGW("rendered after %d retries", i+1);
+          break;
         }
+      }
 
-        if (!loaded()) {
-          LOGE("failed to render map after retry");
-          publish(0, false);
-        }
+      // fallback to sending a blank frame
+      if (!rendered()) {
+        publish(0, false);
       }
     }
   }
@@ -162,6 +164,7 @@ void MapRenderer::update() {
 
   if ((vipc_server != nullptr) && loaded()) {
     publish((end_t - start_t) / 1000.0, true);
+    last_llk_rendered = (*sm)["liveLocationKalman"].getLogMonoTime();
   }
 }
 
diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h
index 41489d41da..5739ba88f2 100644
--- a/selfdrive/navd/map_renderer.h
+++ b/selfdrive/navd/map_renderer.h
@@ -43,6 +43,10 @@ private:
   void initLayers();
 
   uint32_t frame_id = 0;
+  uint64_t last_llk_rendered = 0;
+  bool rendered() {
+    return last_llk_rendered == (*sm)["liveLocationKalman"].getLogMonoTime();
+  }
 
   QTimer* timer;
 
diff --git a/selfdrive/navd/tests/test_map_renderer.py b/selfdrive/navd/tests/test_map_renderer.py
index 381f7ed0f9..515ab70f74 100755
--- a/selfdrive/navd/tests/test_map_renderer.py
+++ b/selfdrive/navd/tests/test_map_renderer.py
@@ -3,10 +3,11 @@ import os
 import unittest
 
 import cereal.messaging as messaging
+from cereal.visionipc import VisionIpcClient, VisionStreamType
 from selfdrive.manager.process_config import managed_processes
 
 LLK_DECIMATION = 10
-
+CACHE_PATH = "/data/mbgl-cache-navd.db"
 
 def gen_llk():
   msg = messaging.new_message('liveLocationKalman')
@@ -24,6 +25,10 @@ class TestMapRenderer(unittest.TestCase):
   def setUp(self):
     self.sm = messaging.SubMaster(['mapRenderState'])
     self.pm = messaging.PubMaster(['liveLocationKalman'])
+    self.vipc = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True)
+
+    if os.path.exists(CACHE_PATH):
+      os.remove(CACHE_PATH)
 
   def tearDown(self):
     managed_processes['mapsd'].stop()
@@ -37,6 +42,11 @@ class TestMapRenderer(unittest.TestCase):
       if self.sm.updated['mapRenderState']:
         break
     assert self.sm.updated['mapRenderState'], "renderer didn't start"
+    assert VisionIpcClient.available_streams("navd", False) == {VisionStreamType.VISION_STREAM_MAP, }
+
+    # connect to vipc
+    assert self.vipc.connect(False)
+    self.vipc.recv()
 
     # run test
     for i in range(20*LLK_DECIMATION):
@@ -53,13 +63,29 @@ class TestMapRenderer(unittest.TestCase):
 
       # check output
       assert self.sm.valid['mapRenderState'] == valid
-      assert 0. < self.sm['mapRenderState'].renderTime < 0.1
       assert self.sm['mapRenderState'].frameId == (prev_frame_id + 1)
       assert self.sm['mapRenderState'].locationMonoTime == llk.logMonoTime
+      if not valid:
+        assert self.sm['mapRenderState'].renderTime == 0.
+      else:
+        assert 0. < self.sm['mapRenderState'].renderTime < 0.1
+
+      # check vision ipc output
+      assert self.vipc.recv() is not None
+      assert self.vipc.valid == valid
+      assert self.vipc.timestamp_sof == llk.logMonoTime
+      assert self.vipc.frame_id == self.sm['mapRenderState'].frameId
 
   def test_with_internet(self):
     self._run_test(True)
 
+  def test_with_no_internet(self):
+    token = os.environ['MAPBOX_TOKEN']
+    try:
+      os.environ['MAPBOX_TOKEN'] = 'invalid_token'
+      self._run_test(False)
+    finally:
+      os.environ['MAPBOX_TOKEN'] = token
 
 if __name__ == "__main__":
   unittest.main()