From a9bdc792a184d191824d2ec089658eb3a96f1e45 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 8 Jun 2022 16:33:05 +0200 Subject: [PATCH] LaikaD: Fix offline handling (#24781) * Test handling no internet correctly. * Clean * Comment * remove del --- selfdrive/locationd/laikad.py | 30 ++++++++++++++----------- selfdrive/locationd/test/test_laikad.py | 22 ++++++++---------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index a55302458..4ec159cd2 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -30,12 +30,12 @@ class Laikad: self.orbit_p: Optional[Process] = None self.orbit_q = Queue() - def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): + def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if ublox_msg.which == 'measurementReport': report = ublox_msg.measurementReport if report.gpsWeek > 0: latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) - self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block=False) + self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block) new_meas = read_raw_ublox(report) measurements = process_measurements(new_meas, self.astro_dog) @@ -110,25 +110,29 @@ class Laikad: def get_orbit_data(self, t: GPSTime, queue): cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") start_time = time.monotonic() - self.astro_dog.get_orbit_data(t, only_predictions=True) + try: + self.astro_dog.get_orbit_data(t, only_predictions=True) + except RuntimeError as e: + cloudlog.info(f"No orbit data found. {e}") + return cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") - queue.put((self.astro_dog.orbits, self.astro_dog.orbit_fetched_times)) + if queue is not None: + queue.put((self.astro_dog.orbits, self.astro_dog.orbit_fetched_times)) def fetch_orbits(self, t: GPSTime, block): if t not in self.astro_dog.orbit_fetched_times: + if block: + self.get_orbit_data(t, None) + return if self.orbit_p is None: self.orbit_p = Process(target=self.get_orbit_data, args=(t, self.orbit_q)) self.orbit_p.start() - ret = None - if block: - ret = self.orbit_q.get(block=True) - elif not self.orbit_q.empty(): + if not self.orbit_q.empty(): ret = self.orbit_q.get() - - if ret: - self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret - self.orbit_p.join() - self.orbit_p = None + if ret: + self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret + self.orbit_p.join() + self.orbit_p = None def __del__(self): if self.orbit_p is not None: diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 630a7525e..7a8c1ecb1 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import unittest from datetime import datetime +from unittest import mock +from unittest.mock import Mock from laika.ephemeris import EphemerisType from laika.gps_time import GPSTime @@ -21,7 +23,7 @@ def get_log(segs=range(0)): def verify_messages(lr, laikad): good_msgs = [] for m in lr: - msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime) + 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) return good_msgs @@ -52,28 +54,21 @@ class TestLaikad(unittest.TestCase): def test_laika_online_nav_only(self): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) + # Disable fetch_orbits to test NAV only + laikad.fetch_orbits = Mock() correct_msgs = verify_messages(self.logs, laikad) correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_offline(self): - # Set auto_update to false forces to use ephemeris messages + @mock.patch('laika.downloader.download_and_cache_file') + def test_laika_offline(self, downloader_mock): + downloader_mock.side_effect = IOError laikad = Laikad(auto_update=False) correct_msgs = verify_messages(self.logs, laikad) - self.assertEqual(256, len(correct_msgs)) self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_offline_ephem_at_start(self): - # Test offline but process ephemeris msgs of segment first - laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV) - ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris'] - correct_msgs = verify_messages(ephemeris_logs+self.logs, laikad) - - self.assertEqual(554, len(correct_msgs)) - self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_get_orbits(self): laikad = Laikad(auto_update=False) first_gps_time = None @@ -97,5 +92,6 @@ class TestLaikad(unittest.TestCase): self.assertLess(0, len(laikad.astro_dog.orbits[prn])) print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) + if __name__ == "__main__": unittest.main()