StarPilot/openpilot/selfdrive/locationd/test/test_lagd.py

import random
import numpy as np
import time
import unittest
from functools import cache

from openpilot.common.test import OpenpilotTestCase
from openpilot.cereal import messaging, log
from opendbc.car.structs import car
from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \
                                               BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC, VERSION, MIN_LAG, MAX_LAG
from openpilot.common.params import Params
from openpilot.starpilot.schema_cache import put_cache
from openpilot.common.hardware import PC

MAX_ERR_FRAMES = 1
DT = 0.05
LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES = int(round(MIN_LAG / DT)), int(round(MAX_LAG / DT))


@cache
def get_test_car_params():
  return car.CarParams.new_message(carFingerprint="cache-test-car", steerRatio=15.0)


def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_threshold=0.0):
  for i in range(n_frames):
    t = i * estimator.dt
    desired_la = np.cos(10 * t) * 0.3
    actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.3

    # if sample is masked out, set it to desired value (no lag)
    rejected = random.uniform(0, 1) < rejection_threshold
    if rejected:
      actual_la = desired_la

    desired_cuvature = float(desired_la / (vego ** 2))
    actual_yr = float(actual_la / vego)
    msgs = [
      (t, "carControl", car.CarControl(latActive=not rejected)),
      (t, "carState", car.CarState(vEgo=vego, steeringPressed=False)),
      (t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)),
      (t, "deviceMotion", log.DeviceMotion(angularVelocityDevice=log.DeviceMotion.XYZMeasurement(z=actual_yr, valid=True),
                                   posenetOK=True, inputsOK=True)),
      (t, "extrinsicsCalibration", log.ExtrinsicsCalibration(rpyCalib=[0, 0, 0], calStatus=log.ExtrinsicsCalibration.Status.calibrated)),
    ]
    for t, w, m in msgs:
      estimator.handle_log(t, w, m)
    estimator.update_points()
    estimator.update_estimate()


class TestLagd(OpenpilotTestCase):
  def test_unqualified_saved_params_are_not_loaded(self):
    params = Params()
    CP = get_test_car_params()
    msg = messaging.new_message('lateralDelay')
    msg.lateralDelay.lateralDelayEstimate = 0.15
    msg.lateralDelay.validBlocks = 5
    msg.lateralDelay.version = VERSION
    raw = msg.to_bytes()
    params.put("LiveDelay", raw, block=True)
    put_cache(params, "CarParamsPrevRoute", CP, block=True)

    self.assertIsNone(retrieve_initial_lag(params, CP))
    self.assertEqual(params.get("LiveDelay"), raw)

  def test_read_saved_params(self):
    params = Params()

    CP = get_test_car_params()

    msg = messaging.new_message('lateralDelay')
    msg.lateralDelay.lateralDelayEstimate = random.random()
    msg.lateralDelay.validBlocks = random.randint(1, 10)
    msg.lateralDelay.version = VERSION
    put_cache(params, "LiveDelay", msg, block=True)
    put_cache(params, "CarParamsPrevRoute", CP, block=True)

    saved_lag_params = retrieve_initial_lag(params, CP)
    assert saved_lag_params is not None

    lag, valid_blocks = saved_lag_params
    assert lag == msg.lateralDelay.lateralDelayEstimate
    assert valid_blocks == msg.lateralDelay.validBlocks

  def test_read_invalid_saved_params(self, subtests):
    params = Params()

    CP = get_test_car_params()

    for msg_dict in [{'version': 0}, {'status': 'invalid'}, {'validBlocks': 100}]:
      with subtests.test(msg=f"lateralDelay={msg_dict}"):
        msg = messaging.new_message('lateralDelay')
        msg.lateralDelay = msg_dict
        put_cache(params, "LiveDelay", msg, block=True)
        put_cache(params, "CarParamsPrevRoute", CP, block=True)
        assert retrieve_initial_lag(params, CP) is None

  def test_ncc(self):
    rng = np.random.default_rng()
    lag_frames = random.randint(1, 19)

    desired_sig = np.sin(np.arange(0.0, 10.0, 0.1))
    actual_sig = np.sin(np.arange(0.0, 10.0, 0.1) - lag_frames * 0.1)
    mask = np.ones(len(desired_sig), dtype=bool)

    corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
    assert np.argmax(corr) == lag_frames

    # add some noise
    desired_sig += rng.normal(0, 0.05, len(desired_sig))
    actual_sig += rng.normal(0, 0.05, len(actual_sig))
    corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
    assert np.argmax(corr)  in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)

    # mask out 40% of the values, and make them noise
    mask = rng.choice([True, False], size=len(desired_sig), p=[0.6, 0.4])
    desired_sig[~mask] = rng.normal(0, 1, size=np.sum(~mask))
    actual_sig[~mask] = rng.normal(0, 1, size=np.sum(~mask))
    corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
    assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)

  def test_empty_estimator(self):
    mocked_CP = car.CarParams(steerActuatorDelay=0.5)
    estimator = LateralLagEstimator(mocked_CP, DT)
    msg = estimator.get_msg(True)
    assert msg.lateralDelay.status == 'unestimated'
    assert np.allclose(msg.lateralDelay.lateralDelay, estimator.initial_lag)
    assert np.allclose(msg.lateralDelay.lateralDelayEstimate, estimator.initial_lag)
    assert msg.lateralDelay.validBlocks == 0
    assert msg.lateralDelay.calPerc == 0

  def test_estimator_basics(self, subtests):
    for lag_frames in range(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1):
      with subtests.test(msg=f"lag_frames={lag_frames}"):
        mocked_CP = car.CarParams(steerActuatorDelay=0.5)
        estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0)
        process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE)
        msg = estimator.get_msg(True)
        assert msg.lateralDelay.status == 'estimated'
        assert np.allclose(msg.lateralDelay.lateralDelay, lag_frames * DT, atol=0.01)
        assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
        assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
        assert msg.lateralDelay.validBlocks == BLOCK_NUM_NEEDED
        assert msg.lateralDelay.calPerc == 100

  def test_estimator_masking(self):
    mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.5), random.randint(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1)
    estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1)
    process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4)
    msg = estimator.get_msg(True)
    assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
    assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
    assert msg.lateralDelay.calPerc == 100

  @unittest.skipIf(PC, "only on device")
  def test_estimator_performance(self):
    mocked_CP = car.CarParams(steerActuatorDelay=0.5)
    estimator = LateralLagEstimator(mocked_CP, DT)

    ds = []
    for _ in range(1000):
      st = time.perf_counter()
      estimator.update_points()
      estimator.update_estimate()
      d = time.perf_counter() - st
      ds.append(d)

    assert np.mean(ds) < DT