Add low Gnss laikad test (#26987)

* add low gnss test

* enable laikad

* temp fix for cache

* update replay

* save LaikadEphemeris for testing

* rem

* update refs

* add comment

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
old-commit-hash: b7ce77b3aa

selfdrive/locationd/laikad.py

@@ -391,13 +391,17 @@ def process_msg(laikad, gnss_msg, mono_time, block=False):
 
 
 def clear_tmp_cache():
+  # TODO: remove this function once laika downloader is changed to not write to disk
+  if "CI" in os.environ or "REPLAY" in os.environ:
+    return
+
   if os.path.exists(DOWNLOADS_CACHE_FOLDER):
     shutil.rmtree(DOWNLOADS_CACHE_FOLDER)
   os.mkdir(DOWNLOADS_CACHE_FOLDER)
 
 
 def main(sm=None, pm=None, qc=None):
-  #clear_tmp_cache()
+  clear_tmp_cache()
 
   use_qcom = not Params().get_bool("UbloxAvailable", block=True)
   if use_qcom or (qc is not None and qc):

selfdrive/locationd/locationd.cc

@@ -589,12 +589,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
     this->handle_sensor(t, log.getAccelerometer());
   } else if (log.isGyroscope()) {
     this->handle_sensor(t, log.getGyroscope());
-  } else if (log.isGpsLocation()) {
+  //} else if (log.isGpsLocation()) {
-    this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
+  //  this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
-  } else if (log.isGpsLocationExternal()) {
+  //} else if (log.isGpsLocationExternal()) {
-    this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
+  //  this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
-  //} else if (log.isGnssMeasurements()) {
+  } else if (log.isGnssMeasurements()) {
-  //  this->handle_gnss(t, log.getGnssMeasurements());
+    this->handle_gnss(t, log.getGnssMeasurements());
   } else if (log.isCarState()) {
     this->handle_car_state(t, log.getCarState());
   } else if (log.isCameraOdometry()) {
@@ -658,17 +658,12 @@ void Localizer::determine_gps_mode(double current_time) {
 
 int Localizer::locationd_thread() {
   ublox_available = Params().getBool("UbloxAvailable", true);
-  const char* gps_location_socket;
+
-  if (ublox_available) {
+  const std::initializer_list<const char *> service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration",
-    gps_location_socket = "gpsLocationExternal";
-  } else {
-    gps_location_socket = "gpsLocation";
-  }
-  const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
                                                           "carState", "carParams", "accelerometer", "gyroscope"};
 
   // TODO: remove carParams once we're always sending at 100Hz
-  SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"});
+  SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"});
   PubMaster pm({"liveLocationKalman"});
 
   uint64_t cnt = 0;

selfdrive/locationd/test/test_laikad.py

@@ -22,7 +22,26 @@ def get_log(segs=range(0)):
   logs = []
   for i in segs:
     logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i)))
-  return [m for m in logs if m.which() == 'ubloxGnss']
+  all_logs = [m for m in logs if m.which() == 'ubloxGnss']
+
+  low_gnss = []
+  for m in all_logs:
+    if m.ubloxGnss.which() != 'measurementReport':
+      continue
+    if m.ubloxGnss.measurementReport.numMeas == 0:
+      continue
+
+    MAX_MEAS = 7
+    if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS:
+      mb = m.as_builder()
+      mb.ubloxGnss.measurementReport.numMeas = MAX_MEAS
+      mb.ubloxGnss.measurementReport.measurements = list(m.ubloxGnss.measurementReport.measurements)[:MAX_MEAS]
+      mb.ubloxGnss.measurementReport.measurements[0].pseudorange += 1000
+      low_gnss.append(mb.as_reader())
+    else:
+      low_gnss.append(m)
+
+  return all_logs, low_gnss
 
 
 def verify_messages(lr, laikad, return_one_success=False):
@@ -59,8 +78,9 @@ class TestLaikad(unittest.TestCase):
 
   @classmethod
   def setUpClass(cls):
-    logs = get_log(range(1))
+    logs, low_gnss = get_log(range(1))
     cls.logs = logs
+    cls.low_gnss = low_gnss
     first_gps_time = get_first_gps_time(logs)
     cls.first_gps_time = first_gps_time
 
@@ -254,6 +274,18 @@ class TestLaikad(unittest.TestCase):
       # Verify orbit data is not downloaded
       mock_method.assert_not_called()
 
+  def test_low_gnss_meas(self):
+    cnt = 0
+    laikad = Laikad()
+    for m in self.low_gnss:
+      msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
+      if msg is None:
+        continue
+      gm = msg.gnssMeasurements
+      if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid:
+        cnt += 1
+    self.assertEqual(cnt, 554)
+
   def dict_has_values(self, dct):
     self.assertGreater(len(dct), 0)
     self.assertGreater(min([len(v) for v in dct.values()]), 0)

selfdrive/locationd/test/test_locationd.py

@@ -15,7 +15,7 @@ from selfdrive.manager.process_config import managed_processes
 
 class TestLocationdProc(unittest.TestCase):
   MAX_WAITS = 1000
-  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
+  LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration',
               'accelerometer', 'gyroscope', 'magnetometer']
 
   def setUp(self):
@@ -46,25 +46,14 @@ class TestLocationdProc(unittest.TestCase):
     except capnp.lib.capnp.KjException:
       msg = messaging.new_message(name, 0)
 
-
+    if name == "gnssMeasurements":
-    if name == "gpsLocationExternal":
+      msg.gnssMeasurements.measTime = t
-      msg.gpsLocationExternal.flags = 1
+      msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
-      msg.gpsLocationExternal.verticalAccuracy = 1.0
+      msg.gnssMeasurements.positionECEF.std = [0,0,0]
-      msg.gpsLocationExternal.speedAccuracy = 1.0
+      msg.gnssMeasurements.positionECEF.valid = True
-      msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
+      msg.gnssMeasurements.velocityECEF.value = []
-      msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
+      msg.gnssMeasurements.velocityECEF.std = [0,0,0]
-      msg.gpsLocationExternal.latitude = float(self.lat)
+      msg.gnssMeasurements.velocityECEF.valid = True
-      msg.gpsLocationExternal.longitude = float(self.lon)
-      msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
-      msg.gpsLocationExternal.altitude = float(self.alt)
-    #if name == "gnssMeasurements":
-    #  msg.gnssMeasurements.measTime = t
-    #  msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
-    #  msg.gnssMeasurements.positionECEF.std = [0,0,0]
-    #  msg.gnssMeasurements.positionECEF.valid = True
-    #  msg.gnssMeasurements.velocityECEF.value = []
-    #  msg.gnssMeasurements.velocityECEF.std = [0,0,0]
-    #  msg.gnssMeasurements.velocityECEF.valid = True
     elif name == 'cameraOdometry':
       msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
       msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]

selfdrive/test/process_replay/process_replay.py

@@ -331,7 +331,7 @@ CONFIGS = [
     pub_sub={
       "cameraOdometry": ["liveLocationKalman"],
       "accelerometer": [], "gyroscope": [],
-      "gpsLocationExternal": [], "liveCalibration": [], "carState": [],
+      "gnssMeasurements": [], "liveCalibration": [], "carState": [],
     },
     ignore=["logMonoTime", "valid"],
     init_callback=get_car_params,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-acc5655633af6ade096ad41f680fd8a28c2e3790
+660149849230afe36363100ab0f5935038d8191e