From 84a95cdc894759d7eb679b28e234e754624f4be4 Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Wed, 18 Jan 2023 23:29:12 -0700 Subject: [PATCH] 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 old-commit-hash: b7ce77b3aa48c3605554f63068a13860023eff86 --- selfdrive/locationd/laikad.py | 6 +++- selfdrive/locationd/locationd.cc | 23 +++++------- selfdrive/locationd/test/test_laikad.py | 36 +++++++++++++++++-- selfdrive/locationd/test/test_locationd.py | 29 +++++---------- .../test/process_replay/process_replay.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 6 files changed, 59 insertions(+), 39 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index b8d86c63fd..ac8fac01f0 100755 --- a/selfdrive/locationd/laikad.py +++ b/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): diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 8941b50248..e78b3d3903 100755 --- a/selfdrive/locationd/locationd.cc +++ b/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()) { - this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); - } else if (log.isGpsLocationExternal()) { - this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); - //} else if (log.isGnssMeasurements()) { - // this->handle_gnss(t, log.getGnssMeasurements()); + //} else if (log.isGpsLocation()) { + // this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); + //} else if (log.isGpsLocationExternal()) { + // this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); + } else if (log.isGnssMeasurements()) { + 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) { - gps_location_socket = "gpsLocationExternal"; - } else { - gps_location_socket = "gpsLocation"; - } - const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + + const std::initializer_list service_list = {"gnssMeasurements", "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; diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 6059eab68f..8cd84d91c2 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/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) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 9f643e2b8f..943969a49e 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/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 == "gpsLocationExternal": - msg.gpsLocationExternal.flags = 1 - msg.gpsLocationExternal.verticalAccuracy = 1.0 - msg.gpsLocationExternal.speedAccuracy = 1.0 - msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 - msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] - msg.gpsLocationExternal.latitude = float(self.lat) - 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 + 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] diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 22ec099285..24ee87e471 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/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, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1aa3aa6f1e..629a9a2f51 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -acc5655633af6ade096ad41f680fd8a28c2e3790 \ No newline at end of file +660149849230afe36363100ab0f5935038d8191e \ No newline at end of file