From bdd769b9554e7e45e976dabd6595403943e864bb Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 16:37:14 +0200 Subject: [PATCH 1/4] laikad: use cython version of gnss kf --- selfdrive/locationd/laikad.py | 2 +- selfdrive/locationd/models/gnss_kf.py | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index ba665b75309847..20a4dc97f15181 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -138,7 +138,7 @@ def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement self.gnss_kf.predict(t) def kf_valid(self, t: float) -> List[bool]: - filter_time = self.gnss_kf.filter.filter_time + filter_time = self.gnss_kf.filter.get_filter_time() return [filter_time is not None, filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP, all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))] diff --git a/selfdrive/locationd/models/gnss_kf.py b/selfdrive/locationd/models/gnss_kf.py index ce0ff84dc2705a..f4be889c734b8c 100755 --- a/selfdrive/locationd/models/gnss_kf.py +++ b/selfdrive/locationd/models/gnss_kf.py @@ -3,12 +3,16 @@ from typing import List import numpy as np -import sympy as sp -from rednose.helpers.ekf_sym import EKF_sym, gen_code from selfdrive.locationd.models.constants import ObservationKind from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr +if __name__ == '__main__': # Generating sympy + import sympy as sp + from rednose.helpers.ekf_sym import gen_code +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module + class States(): ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters From 42fae89ee1d07d92e2dd089b4d3ec731a963ad45 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 16:47:15 +0200 Subject: [PATCH 2/4] fix import error --- selfdrive/locationd/models/gnss_kf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/locationd/models/gnss_kf.py b/selfdrive/locationd/models/gnss_kf.py index f4be889c734b8c..6d27551e4d0bf3 100755 --- a/selfdrive/locationd/models/gnss_kf.py +++ b/selfdrive/locationd/models/gnss_kf.py @@ -11,7 +11,7 @@ import sympy as sp from rednose.helpers.ekf_sym import gen_code else: - from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module + from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module,import-error class States(): From b8e71aec460e9f13e6f717d8e9720a2cec6c90f4 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 18:48:19 +0200 Subject: [PATCH 3/4] test liblocationd separate --- .github/workflows/selfdrive_tests.yaml | 3 +- .../locationd/test/_test_locationd_lib.py | 106 ++++++++++++++++++ selfdrive/locationd/test/test_locationd.py | 94 ---------------- 3 files changed, 108 insertions(+), 95 deletions(-) create mode 100755 selfdrive/locationd/test/_test_locationd_lib.py diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index fedeaa734bb65d..52dd012baaade8 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -299,6 +299,7 @@ jobs: $UNIT_TEST selfdrive/loggerd && \ $UNIT_TEST selfdrive/car && \ $UNIT_TEST selfdrive/locationd && \ + selfdrive/locationd/test/_test_locationd_lib.py && \ $UNIT_TEST selfdrive/athena && \ $UNIT_TEST selfdrive/thermald && \ $UNIT_TEST selfdrive/hardware/tici && \ @@ -364,7 +365,7 @@ jobs: CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only" - name: "Upload coverage to Codecov" uses: codecov/codecov-action@v2 - + model_replay_onnx: name: model replay onnx runs-on: ubuntu-20.04 diff --git a/selfdrive/locationd/test/_test_locationd_lib.py b/selfdrive/locationd/test/_test_locationd_lib.py new file mode 100755 index 00000000000000..8a0ed3ef05923e --- /dev/null +++ b/selfdrive/locationd/test/_test_locationd_lib.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +"""This test can't be run together with other locationd tests. +cffi.dlopen breaks the list of registered filters.""" +import os +import random +import unittest + +from cffi import FFI + +import cereal.messaging as messaging +from cereal import log + +SENSOR_DECIMATION = 1 +VISION_DECIMATION = 1 + +LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so')) + + +class TestLocationdLib(unittest.TestCase): + def setUp(self): + header = '''typedef ...* Localizer_t; +Localizer_t localizer_init(); +void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); +void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' + + self.ffi = FFI() + self.ffi.cdef(header) + self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH) + + self.localizer = self.lib.localizer_init() + + self.buff_size = 2048 + self.msg_buff = self.ffi.new(f'char[{self.buff_size}]') + + def localizer_handle_msg(self, msg_builder): + bytstr = msg_builder.to_bytes() + self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr)) + + def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True): + self.lib.localizer_get_message_bytes(self.localizer, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size) + return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8) + + def test_liblocalizer(self): + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = random.randint(1, 10) + msg.liveCalibration.rpyCalib = [random.random() / 10 for _ in range(3)] + + self.localizer_handle_msg(msg) + liveloc = self.localizer_get_msg() + self.assertTrue(liveloc is not None) + + @unittest.skip("temporarily disabled due to false positives") + def test_device_fell(self): + msg = messaging.new_message('sensorEvents', 1) + msg.sensorEvents[0].sensor = 1 + msg.sensorEvents[0].timestamp = msg.logMonoTime + msg.sensorEvents[0].type = 1 + msg.sensorEvents[0].init('acceleration') + msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0] # zero with gravity + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertTrue(ret.liveLocationKalman.deviceStable) + + msg = messaging.new_message('sensorEvents', 1) + msg.sensorEvents[0].sensor = 1 + msg.sensorEvents[0].timestamp = msg.logMonoTime + msg.sensorEvents[0].type = 1 + msg.sensorEvents[0].init('acceleration') + msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0] # more than 40 m/s**2 + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertFalse(ret.liveLocationKalman.deviceStable) + + def test_posenet_spike(self): + for _ in range(SENSOR_DECIMATION): + msg = messaging.new_message('carState') + msg.carState.vEgo = 6.0 # more than 5 m/s + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertTrue(ret.liveLocationKalman.posenetOK) + + for _ in range(20 * VISION_DECIMATION): # size of hist_old + msg = messaging.new_message('cameraOdometry') + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [0.1, 0.1, 0.1] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [2.0, 0.1, 0.1] + self.localizer_handle_msg(msg) + + for _ in range(20 * VISION_DECIMATION): # size of hist_new + msg = messaging.new_message('cameraOdometry') + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [1.0, 1.0, 1.0] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [10.1, 0.1, 0.1] # more than 4 times larger + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertFalse(ret.liveLocationKalman.posenetOK) + +if __name__ == "__main__": + unittest.main() + diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 7f5d7521005502..29036b8387cc9f 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -1,110 +1,16 @@ #!/usr/bin/env python3 -import os import json import random import unittest import time import capnp -from cffi import FFI -from cereal import log import cereal.messaging as messaging from cereal.services import service_list from common.params import Params from selfdrive.manager.process_config import managed_processes -SENSOR_DECIMATION = 1 -VISION_DECIMATION = 1 - -LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so')) - - -class TestLocationdLib(unittest.TestCase): - def setUp(self): - header = '''typedef ...* Localizer_t; -Localizer_t localizer_init(); -void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); -void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' - - self.ffi = FFI() - self.ffi.cdef(header) - self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH) - - self.localizer = self.lib.localizer_init() - - self.buff_size = 2048 - self.msg_buff = self.ffi.new(f'char[{self.buff_size}]') - - def localizer_handle_msg(self, msg_builder): - bytstr = msg_builder.to_bytes() - self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr)) - - def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True): - self.lib.localizer_get_message_bytes(self.localizer, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size) - return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8) - - def test_liblocalizer(self): - msg = messaging.new_message('liveCalibration') - msg.liveCalibration.validBlocks = random.randint(1, 10) - msg.liveCalibration.rpyCalib = [random.random() / 10 for _ in range(3)] - - self.localizer_handle_msg(msg) - liveloc = self.localizer_get_msg() - self.assertTrue(liveloc is not None) - - @unittest.skip("temporarily disabled due to false positives") - def test_device_fell(self): - msg = messaging.new_message('sensorEvents', 1) - msg.sensorEvents[0].sensor = 1 - msg.sensorEvents[0].timestamp = msg.logMonoTime - msg.sensorEvents[0].type = 1 - msg.sensorEvents[0].init('acceleration') - msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0] # zero with gravity - self.localizer_handle_msg(msg) - - ret = self.localizer_get_msg() - self.assertTrue(ret.liveLocationKalman.deviceStable) - - msg = messaging.new_message('sensorEvents', 1) - msg.sensorEvents[0].sensor = 1 - msg.sensorEvents[0].timestamp = msg.logMonoTime - msg.sensorEvents[0].type = 1 - msg.sensorEvents[0].init('acceleration') - msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0] # more than 40 m/s**2 - self.localizer_handle_msg(msg) - - ret = self.localizer_get_msg() - self.assertFalse(ret.liveLocationKalman.deviceStable) - - def test_posenet_spike(self): - for _ in range(SENSOR_DECIMATION): - msg = messaging.new_message('carState') - msg.carState.vEgo = 6.0 # more than 5 m/s - self.localizer_handle_msg(msg) - - ret = self.localizer_get_msg() - self.assertTrue(ret.liveLocationKalman.posenetOK) - - for _ in range(20 * VISION_DECIMATION): # size of hist_old - msg = messaging.new_message('cameraOdometry') - msg.cameraOdometry.rot = [0.0, 0.0, 0.0] - msg.cameraOdometry.rotStd = [0.1, 0.1, 0.1] - msg.cameraOdometry.trans = [0.0, 0.0, 0.0] - msg.cameraOdometry.transStd = [2.0, 0.1, 0.1] - self.localizer_handle_msg(msg) - - for _ in range(20 * VISION_DECIMATION): # size of hist_new - msg = messaging.new_message('cameraOdometry') - msg.cameraOdometry.rot = [0.0, 0.0, 0.0] - msg.cameraOdometry.rotStd = [1.0, 1.0, 1.0] - msg.cameraOdometry.trans = [0.0, 0.0, 0.0] - msg.cameraOdometry.transStd = [10.1, 0.1, 0.1] # more than 4 times larger - self.localizer_handle_msg(msg) - - ret = self.localizer_get_msg() - self.assertFalse(ret.liveLocationKalman.posenetOK) - class TestLocationdProc(unittest.TestCase): MAX_WAITS = 1000 From bc2bd09edfaffe994c087972c883cbf3e871f2ae Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 19:52:55 +0200 Subject: [PATCH 4/4] Revert "laikad: use cython version of gnss kf" This reverts commit bdd769b9554e7e45e976dabd6595403943e864bb. --- selfdrive/locationd/laikad.py | 2 +- selfdrive/locationd/models/gnss_kf.py | 8 ++------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 20a4dc97f15181..ba665b75309847 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -138,7 +138,7 @@ def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement self.gnss_kf.predict(t) def kf_valid(self, t: float) -> List[bool]: - filter_time = self.gnss_kf.filter.get_filter_time() + filter_time = self.gnss_kf.filter.filter_time return [filter_time is not None, filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP, all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))] diff --git a/selfdrive/locationd/models/gnss_kf.py b/selfdrive/locationd/models/gnss_kf.py index 6d27551e4d0bf3..ce0ff84dc2705a 100755 --- a/selfdrive/locationd/models/gnss_kf.py +++ b/selfdrive/locationd/models/gnss_kf.py @@ -3,16 +3,12 @@ from typing import List import numpy as np +import sympy as sp +from rednose.helpers.ekf_sym import EKF_sym, gen_code from selfdrive.locationd.models.constants import ObservationKind from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr -if __name__ == '__main__': # Generating sympy - import sympy as sp - from rednose.helpers.ekf_sym import gen_code -else: - from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module,import-error - class States(): ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters