pull/34975/head
Kacper Rączy 3 weeks ago
parent ba80a30f35
commit 31b9e8dd75
  1. 25
      selfdrive/locationd/test/test_lagd.py

@ -1,7 +1,6 @@
import random import random
import numpy as np import numpy as np
import time import time
from unittest import mock
import pytest import pytest
from cereal import messaging from cereal import messaging
@ -51,15 +50,15 @@ class TestLagd:
corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20] 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) assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)
def test_estimator(self): def test_estimator(self, mocker):
class ZeroMock(mock.Mock): class ZeroMock(mocker.Mock):
def __getattr__(self, *args): def __getattr__(self, *args):
return 0 return 0
dt = 0.05 dt = 0.05
lag_frames = random.randint(1, 20) lag_frames = random.randint(1, 20)
mocked_CP = mock.Mock(steerActuatorDelay=1.0) mocked_CP = mocker.Mock(steerActuatorDelay=1.0)
estimator = LateralLagEstimator(mocked_CP, 0.05, estimator = LateralLagEstimator(mocked_CP, 0.05,
block_count=10, min_valid_block_count=0, block_count=10, min_valid_block_count=0,
block_size=1, okay_window_sec=100 * dt, block_size=1, okay_window_sec=100 * dt,
@ -67,14 +66,14 @@ class TestLagd:
for i in range(100): for i in range(100):
t = i * dt t = i * dt
vego = 20.0 vego = 20.0
desired_cuvature = np.cos(t) * 100 / (vego ** 2) desired_cuvature = np.cos(t) / (vego ** 2)
actual_yr = np.cos(t - lag_frames * dt) * 100 / vego actual_yr = np.cos(t - lag_frames * dt) / vego
msgs = [ msgs = [
(t, "carControl", mock.Mock(latActive=True)), (t, "carControl", mocker.Mock(latActive=True)),
(t, "carState", mock.Mock(vEgo=vego, steeringPressed=False)), (t, "carState", mocker.Mock(vEgo=vego, steeringPressed=False)),
(t, "controlsState", mock.Mock(desiredCurvature=desired_cuvature, (t, "controlsState", mocker.Mock(desiredCurvature=desired_cuvature,
lateralControlState=mock.Mock(which=mock.Mock(return_value='debugControlState'), debugControlState=ZeroMock()))), lateralControlState=mocker.Mock(which=mocker.Mock(return_value='debugControlState'), debugControlState=ZeroMock()))),
(t, "livePose", mock.Mock(orientationNED=ZeroMock(), (t, "livePose", mocker.Mock(orientationNED=ZeroMock(),
velocityDevice=ZeroMock(), velocityDevice=ZeroMock(),
accelerationDevice=ZeroMock(), accelerationDevice=ZeroMock(),
angularVelocityDevice=ZeroMock(z=actual_yr))), angularVelocityDevice=ZeroMock(z=actual_yr))),
@ -92,8 +91,8 @@ class TestLagd:
@pytest.mark.skipif(PC, reason="only on device") @pytest.mark.skipif(PC, reason="only on device")
@pytest.mark.timeout(30) @pytest.mark.timeout(30)
def test_estimator_performance(self): def test_estimator_performance(self, mocker):
mocked_CP = mock.Mock(steerActuatorDelay=0.1) mocked_CP = mocker.Mock(steerActuatorDelay=0.1)
estimator = LateralLagEstimator(mocked_CP, 0.05) estimator = LateralLagEstimator(mocked_CP, 0.05)
ds = [] ds = []

Loading…
Cancel
Save