Laikad: Cache orbit and nav data (#24831)

* Cache orbit and nav data

* Cleanup

* Cleanup

* Use ProcessPoolExecutor to fetch orbits

* update laika repo

* Minor

* Create json de/serializers
Save cache only 1 minute at max

* Update laika repo

* Speed up json by caching json in ephemeris class

* Update laika

* Fix test

* Use constant
old-commit-hash: c3fa9151f39994984b60a19cdd7425dba73ec2fc

selfdrive/locationd/test/test_laikad.py

@@ -1,14 +1,16 @@
 #!/usr/bin/env python3
+import time
 import unittest
 from datetime import datetime
 from unittest import mock
-from unittest.mock import Mock
+from unittest.mock import Mock, patch
 
+from common.params import Params
 from laika.ephemeris import EphemerisType
 from laika.gps_time import GPSTime
-from laika.helpers import ConstellationId
+from laika.helpers import ConstellationId, TimeRangeHolder
 from laika.raw_gnss import GNSSMeasurement, read_raw_ublox
-from selfdrive.locationd.laikad import Laikad, create_measurement_msg
+from selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad, create_measurement_msg
 from selfdrive.test.openpilotci import get_url
 from tools.lib.logreader import LogReader
 
@@ -20,12 +22,14 @@ def get_log(segs=range(0)):
   return [m for m in logs if m.which() == 'ubloxGnss']
 
 
-def verify_messages(lr, laikad):
+def verify_messages(lr, laikad, return_one_success=False):
   good_msgs = []
   for m in lr:
     msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True)
     if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
       good_msgs.append(msg)
+      if return_one_success:
+        return msg
   return good_msgs
 
 
@@ -35,6 +39,9 @@ class TestLaikad(unittest.TestCase):
   def setUpClass(cls):
     cls.logs = get_log(range(1))
 
+  def setUp(self):
+    Params().delete(EPHEMERIS_CACHE)
+
   def test_create_msg_without_errors(self):
     gpstime = GPSTime.from_datetime(datetime.now())
     meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.})
@@ -81,8 +88,7 @@ class TestLaikad(unittest.TestCase):
     first_gps_time = self.get_first_gps_time()
     # Pretend process has loaded the orbits on startup by using the time of the first gps message.
     laikad.fetch_orbits(first_gps_time, block=True)
-    self.assertEqual(29, len(laikad.astro_dog.orbits.values()))
-    self.assertGreater(min([len(v) for v in laikad.astro_dog.orbits.values()]), 0)
+    self.dict_has_values(laikad.astro_dog.orbits)
 
   @unittest.skip("Use to debug live data")
   def test_laika_get_orbits_now(self):
@@ -109,6 +115,54 @@ class TestLaikad(unittest.TestCase):
     self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0)
     self.assertEqual(None, laikad.orbit_fetch_future)
 
+  def test_cache(self):
+    laikad = Laikad(auto_update=True, save_ephemeris=True)
+    first_gps_time = self.get_first_gps_time()
+
+    def wait_for_cache():
+      max_time = 2
+      while Params().get(EPHEMERIS_CACHE) is None:
+        time.sleep(0.1)
+        max_time -= 0.1
+        if max_time == 0:
+          self.fail("Cache has not been written after 2 seconds")
+    # Test cache with no ephemeris
+    laikad.cache_ephemeris(t=GPSTime(0, 0))
+    wait_for_cache()
+    Params().delete(EPHEMERIS_CACHE)
+
+    laikad.astro_dog.get_navs(first_gps_time)
+    laikad.fetch_orbits(first_gps_time, block=True)
+
+    # Wait for cache to save
+    wait_for_cache()
+
+    # Check both nav and orbits separate
+    laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV)
+    # Verify orbits and nav are loaded from cache
+    self.dict_has_values(laikad.astro_dog.orbits)
+    self.dict_has_values(laikad.astro_dog.nav)
+    # Verify cache is working for only nav by running a segment
+    msg = verify_messages(self.logs, laikad, return_one_success=True)
+    self.assertIsNotNone(msg)
+
+    with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method:
+      # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently
+      laikad.astro_dog.orbit_fetched_times = TimeRangeHolder()
+      laikad.fetch_orbits(first_gps_time, block=False)
+      mock_method.assert_not_called()
+
+      # Verify cache is working for only orbits by running a segment
+      laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
+      msg = verify_messages(self.logs, laikad, return_one_success=True)
+      self.assertIsNotNone(msg)
+      # Verify orbit data is not downloaded
+      mock_method.assert_not_called()
+
+  def dict_has_values(self, dct):
+    self.assertGreater(len(dct), 0)
+    self.assertGreater(min([len(v) for v in dct.values()]), 0)
+
 
 if __name__ == "__main__":
   unittest.main()