Reapply "Online lateral lag learning" (#34975)
* Online lateral lag learning (#34974)
This reverts commit b4cc9e68d1.
* pad to the best size for fft
* Fix static analysis
* Add typing
* Fix typing
* MAX_LAG
* Calculate cross correlation regardless if the points are valid
* Back to lagd
* Add lagd to process_config
* Lagd in test onroad
* Move lag estimator for lagd
* Remove duplicate entry from test_onroad
* Update process replay
* pre-fill the data
* Update cpu usage
* 25sec window
* Change the meaning of lateralDelayEstimate
* No newline
* Fix typing
* Prefill
* Update ref commit
* Add a unit test
* Fix static issues
* Time limit
* Or timeout
* Use mocker
* Update estimate every time
* empty test
* DT const
* enable RIVIAN again
* Update ref commit
* Update that again
* Improve the tests
* Fix static
* Add masking test
* Increase timeout
* Add liveDelay to selfdrived
* Add liveDelay to selfdrived in process_replay
* Fix block_avg restore after num_blocks
* regen most
* Update bolt
* Update ref commit
* Change the key name
* Add assert
* True weighted average
pull/35005/head
Kacper Rączy
2 weeks ago
committed by
GitHub
parent
5d1816e2b8
commit
f237649a7a
11 changed files with 542 additions and 21 deletions
@ -0,0 +1,333 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import numpy as np |
||||
import capnp |
||||
from collections import deque |
||||
from functools import partial |
||||
|
||||
import cereal.messaging as messaging |
||||
from cereal import car, log |
||||
from cereal.services import SERVICE_LIST |
||||
from openpilot.common.params import Params |
||||
from openpilot.common.realtime import config_realtime_process |
||||
from openpilot.common.swaglog import cloudlog |
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp |
||||
|
||||
BLOCK_SIZE = 100 |
||||
BLOCK_NUM = 50 |
||||
BLOCK_NUM_NEEDED = 5 |
||||
MOVING_WINDOW_SEC = 300.0 |
||||
MIN_OKAY_WINDOW_SEC = 25.0 |
||||
MIN_RECOVERY_BUFFER_SEC = 2.0 |
||||
MIN_VEGO = 15.0 |
||||
MIN_ABS_YAW_RATE = np.radians(1.0) |
||||
MIN_NCC = 0.95 |
||||
MAX_LAG = 1.0 |
||||
|
||||
|
||||
def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int): |
||||
""" |
||||
References: |
||||
D. Padfield. "Masked FFT registration". In Proc. Computer Vision and |
||||
Pattern Recognition, pp. 2918-2925 (2010). |
||||
:DOI:`10.1109/CVPR.2010.5540032` |
||||
""" |
||||
|
||||
eps = np.finfo(np.float64).eps |
||||
expected_sig = np.asarray(expected_sig, dtype=np.float64) |
||||
actual_sig = np.asarray(actual_sig, dtype=np.float64) |
||||
|
||||
expected_sig[~mask] = 0.0 |
||||
actual_sig[~mask] = 0.0 |
||||
|
||||
rotated_expected_sig = expected_sig[::-1] |
||||
rotated_mask = mask[::-1] |
||||
|
||||
fft = partial(np.fft.fft, n=n) |
||||
|
||||
actual_sig_fft = fft(actual_sig) |
||||
rotated_expected_sig_fft = fft(rotated_expected_sig) |
||||
actual_mask_fft = fft(mask.astype(np.float64)) |
||||
rotated_mask_fft = fft(rotated_mask.astype(np.float64)) |
||||
|
||||
number_overlap_masked_samples = np.fft.ifft(rotated_mask_fft * actual_mask_fft).real |
||||
number_overlap_masked_samples[:] = np.round(number_overlap_masked_samples) |
||||
number_overlap_masked_samples[:] = np.fmax(number_overlap_masked_samples, eps) |
||||
masked_correlated_actual_fft = np.fft.ifft(rotated_mask_fft * actual_sig_fft).real |
||||
masked_correlated_expected_fft = np.fft.ifft(actual_mask_fft * rotated_expected_sig_fft).real |
||||
|
||||
numerator = np.fft.ifft(rotated_expected_sig_fft * actual_sig_fft).real |
||||
numerator -= masked_correlated_actual_fft * masked_correlated_expected_fft / number_overlap_masked_samples |
||||
|
||||
actual_squared_fft = fft(actual_sig ** 2) |
||||
actual_sig_denom = np.fft.ifft(rotated_mask_fft * actual_squared_fft).real |
||||
actual_sig_denom -= masked_correlated_actual_fft ** 2 / number_overlap_masked_samples |
||||
actual_sig_denom[:] = np.fmax(actual_sig_denom, 0.0) |
||||
|
||||
rotated_expected_squared_fft = fft(rotated_expected_sig ** 2) |
||||
expected_sig_denom = np.fft.ifft(actual_mask_fft * rotated_expected_squared_fft).real |
||||
expected_sig_denom -= masked_correlated_expected_fft ** 2 / number_overlap_masked_samples |
||||
expected_sig_denom[:] = np.fmax(expected_sig_denom, 0.0) |
||||
|
||||
denom = np.sqrt(actual_sig_denom * expected_sig_denom) |
||||
|
||||
# zero-out samples with very small denominators |
||||
tol = 1e3 * eps * np.max(np.abs(denom), keepdims=True) |
||||
nonzero_indices = denom > tol |
||||
|
||||
ncc = np.zeros_like(denom, dtype=np.float64) |
||||
ncc[nonzero_indices] = numerator[nonzero_indices] / denom[nonzero_indices] |
||||
np.clip(ncc, -1, 1, out=ncc) |
||||
|
||||
return ncc |
||||
|
||||
|
||||
class Points: |
||||
def __init__(self, num_points: int): |
||||
self.times = deque[float]([0.0] * num_points, maxlen=num_points) |
||||
self.okay = deque[bool]([False] * num_points, maxlen=num_points) |
||||
self.desired = deque[float]([0.0] * num_points, maxlen=num_points) |
||||
self.actual = deque[float]([0.0] * num_points, maxlen=num_points) |
||||
|
||||
@property |
||||
def num_points(self): |
||||
return len(self.desired) |
||||
|
||||
@property |
||||
def num_okay(self): |
||||
return np.count_nonzero(self.okay) |
||||
|
||||
def update(self, t: float, desired: float, actual: float, okay: bool): |
||||
self.times.append(t) |
||||
self.okay.append(okay) |
||||
self.desired.append(desired) |
||||
self.actual.append(actual) |
||||
|
||||
def get(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: |
||||
return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay) |
||||
|
||||
|
||||
class BlockAverage: |
||||
def __init__(self, num_blocks: int, block_size: int, valid_blocks: int, initial_value: float): |
||||
self.num_blocks = num_blocks |
||||
self.block_size = block_size |
||||
self.block_idx = valid_blocks % num_blocks |
||||
self.idx = 0 |
||||
|
||||
self.values = np.tile(initial_value, (num_blocks, 1)) |
||||
self.valid_blocks = valid_blocks |
||||
|
||||
def update(self, value: float): |
||||
self.values[self.block_idx] = (self.idx * self.values[self.block_idx] + value) / (self.idx + 1) |
||||
self.idx = (self.idx + 1) % self.block_size |
||||
if self.idx == 0: |
||||
self.block_idx = (self.block_idx + 1) % self.num_blocks |
||||
self.valid_blocks = min(self.valid_blocks + 1, self.num_blocks) |
||||
|
||||
def get(self) -> tuple[float, float]: |
||||
valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx] |
||||
valid_and_current_idx = valid_block_idx + ([self.block_idx] if self.idx > 0 else []) |
||||
|
||||
valid_mean = float(np.mean(self.values[valid_block_idx], axis=0).item()) if len(valid_block_idx) > 0 else float('nan') |
||||
current_mean = float(np.mean(self.values[valid_and_current_idx], axis=0).item()) if len(valid_and_current_idx) > 0 else float('nan') |
||||
return valid_mean, current_mean |
||||
|
||||
|
||||
class LateralLagEstimator: |
||||
inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"} |
||||
|
||||
def __init__(self, CP: car.CarParams, dt: float, |
||||
block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE, |
||||
window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC, |
||||
min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC): |
||||
self.dt = dt |
||||
self.window_sec = window_sec |
||||
self.okay_window_sec = okay_window_sec |
||||
self.min_recovery_buffer_sec = min_recovery_buffer_sec |
||||
self.initial_lag = CP.steerActuatorDelay + 0.2 |
||||
self.block_size = block_size |
||||
self.block_count = block_count |
||||
self.min_valid_block_count = min_valid_block_count |
||||
self.min_vego = min_vego |
||||
self.min_yr = min_yr |
||||
self.min_ncc = min_ncc |
||||
|
||||
self.t = 0.0 |
||||
self.lat_active = False |
||||
self.steering_pressed = False |
||||
self.steering_saturated = False |
||||
self.desired_curvature = 0.0 |
||||
self.v_ego = 0.0 |
||||
self.yaw_rate = 0.0 |
||||
|
||||
self.last_lat_inactive_t = 0.0 |
||||
self.last_steering_pressed_t = 0.0 |
||||
self.last_steering_saturated_t = 0.0 |
||||
self.last_estimate_t = 0.0 |
||||
|
||||
self.calibrator = PoseCalibrator() |
||||
|
||||
self.reset(self.initial_lag, 0) |
||||
|
||||
def reset(self, initial_lag: float, valid_blocks: int): |
||||
window_len = int(self.window_sec / self.dt) |
||||
self.points = Points(window_len) |
||||
self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag) |
||||
|
||||
def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: |
||||
msg = messaging.new_message('liveDelay') |
||||
|
||||
msg.valid = valid |
||||
|
||||
liveDelay = msg.liveDelay |
||||
|
||||
valid_mean_lag, current_mean_lag = self.block_avg.get() |
||||
if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag): |
||||
liveDelay.status = log.LiveDelayData.Status.estimated |
||||
liveDelay.lateralDelay = valid_mean_lag |
||||
else: |
||||
liveDelay.status = log.LiveDelayData.Status.unestimated |
||||
liveDelay.lateralDelay = self.initial_lag |
||||
if not np.isnan(current_mean_lag): |
||||
liveDelay.lateralDelayEstimate = current_mean_lag |
||||
else: |
||||
liveDelay.lateralDelayEstimate = self.initial_lag |
||||
liveDelay.validBlocks = self.block_avg.valid_blocks |
||||
if debug: |
||||
liveDelay.points = self.block_avg.values.flatten().tolist() |
||||
|
||||
return msg |
||||
|
||||
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): |
||||
if which == "carControl": |
||||
self.lat_active = msg.latActive |
||||
elif which == "carState": |
||||
self.steering_pressed = msg.steeringPressed |
||||
self.v_ego = msg.vEgo |
||||
elif which == "controlsState": |
||||
self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated |
||||
self.desired_curvature = msg.desiredCurvature |
||||
elif which == "liveCalibration": |
||||
self.calibrator.feed_live_calib(msg) |
||||
elif which == "livePose": |
||||
device_pose = Pose.from_live_pose(msg) |
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) |
||||
self.yaw_rate = calibrated_pose.angular_velocity.z |
||||
self.t = t |
||||
|
||||
def points_enough(self): |
||||
return self.points.num_points >= int(self.okay_window_sec / self.dt) |
||||
|
||||
def points_valid(self): |
||||
return self.points.num_okay >= int(self.okay_window_sec / self.dt) |
||||
|
||||
def update_points(self): |
||||
if not self.lat_active: |
||||
self.last_lat_inactive_t = self.t |
||||
if self.steering_pressed: |
||||
self.last_steering_pressed_t = self.t |
||||
if self.steering_saturated: |
||||
self.last_steering_saturated_t = self.t |
||||
|
||||
la_desired = self.desired_curvature * self.v_ego * self.v_ego |
||||
la_actual_pose = self.yaw_rate * self.v_ego |
||||
|
||||
fast = self.v_ego > self.min_vego |
||||
turning = np.abs(self.yaw_rate) >= self.min_yr |
||||
has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated |
||||
self.t - last_t >= self.min_recovery_buffer_sec |
||||
for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t] |
||||
) |
||||
okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and fast and turning and has_recovered |
||||
|
||||
self.points.update(self.t, la_desired, la_actual_pose, okay) |
||||
|
||||
def update_estimate(self): |
||||
if not self.points_enough(): |
||||
return |
||||
|
||||
times, desired, actual, okay = self.points.get() |
||||
# check if there are any new valid data points since the last update |
||||
is_valid = self.points_valid() |
||||
if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t: |
||||
new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t) |
||||
is_valid = is_valid and not (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:])) |
||||
|
||||
delay, corr = self.actuator_delay(desired, actual, okay, self.dt, MAX_LAG) |
||||
if corr < self.min_ncc or not is_valid: |
||||
return |
||||
|
||||
self.block_avg.update(delay) |
||||
self.last_estimate_t = self.t |
||||
|
||||
def actuator_delay(self, expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, dt: float, max_lag: float) -> tuple[float, float]: |
||||
assert len(expected_sig) == len(actual_sig) |
||||
max_lag_samples = int(max_lag / dt) |
||||
padded_size = fft_next_good_size(len(expected_sig) + max_lag_samples) |
||||
|
||||
ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask, padded_size) |
||||
|
||||
# only consider lags from 0 to max_lag |
||||
roi_ncc = ncc[len(expected_sig) - 1: len(expected_sig) - 1 + max_lag_samples] |
||||
|
||||
max_corr_index = np.argmax(roi_ncc) |
||||
corr = roi_ncc[max_corr_index] |
||||
lag = parabolic_peak_interp(roi_ncc, max_corr_index) * dt |
||||
|
||||
return lag, corr |
||||
|
||||
|
||||
def retrieve_initial_lag(params_reader: Params, CP: car.CarParams): |
||||
last_lag_data = params_reader.get("LiveDelay") |
||||
last_carparams_data = params_reader.get("CarParamsPrevRoute") |
||||
|
||||
if last_lag_data is not None: |
||||
try: |
||||
with log.Event.from_bytes(last_lag_data) as last_lag_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP: |
||||
ld = last_lag_msg.liveDelay |
||||
if last_CP.carFingerprint != CP.carFingerprint: |
||||
raise Exception("Car model mismatch") |
||||
|
||||
lag, valid_blocks = ld.lateralDelayEstimate, ld.validBlocks |
||||
assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks" |
||||
return lag, valid_blocks |
||||
except Exception as e: |
||||
cloudlog.error(f"Failed to retrieve initial lag: {e}") |
||||
|
||||
return None |
||||
|
||||
|
||||
def main(): |
||||
config_realtime_process([0, 1, 2, 3], 5) |
||||
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0"))) |
||||
|
||||
pm = messaging.PubMaster(['liveDelay']) |
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose') |
||||
|
||||
params_reader = Params() |
||||
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams) |
||||
|
||||
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency) |
||||
if (initial_lag_params := retrieve_initial_lag(params_reader, CP)) is not None: |
||||
lag, valid_blocks = initial_lag_params |
||||
lag_learner.reset(lag, valid_blocks) |
||||
|
||||
while True: |
||||
sm.update() |
||||
if sm.all_checks(): |
||||
for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): |
||||
if sm.updated[which]: |
||||
t = sm.logMonoTime[which] * 1e-9 |
||||
lag_learner.handle_log(t, which, sm[which]) |
||||
lag_learner.update_points() |
||||
|
||||
# 4Hz driven by livePose |
||||
if sm.frame % 5 == 0: |
||||
lag_learner.update_estimate() |
||||
lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG) |
||||
lag_msg_dat = lag_msg.to_bytes() |
||||
pm.send('liveDelay', lag_msg_dat) |
||||
|
||||
if sm.frame % 1200 == 0: # cache every 60 seconds |
||||
params_reader.put_nonblocking("LiveDelay", lag_msg_dat) |
||||
@ -0,0 +1,137 @@ |
||||
import random |
||||
import numpy as np |
||||
import time |
||||
import pytest |
||||
|
||||
from cereal import messaging |
||||
from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \ |
||||
BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC |
||||
from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams |
||||
from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE |
||||
from openpilot.common.params import Params |
||||
from openpilot.tools.lib.logreader import LogReader |
||||
from openpilot.system.hardware import PC |
||||
|
||||
MAX_ERR_FRAMES = 1 |
||||
DT = 0.05 |
||||
|
||||
|
||||
def process_messages(mocker, estimator, lag_frames, n_frames, vego=20.0, rejection_threshold=0.0): |
||||
class ZeroMock(mocker.Mock): |
||||
def __getattr__(self, *args): |
||||
return 0 |
||||
|
||||
for i in range(n_frames): |
||||
t = i * estimator.dt |
||||
desired_la = np.cos(t) |
||||
actual_la = np.cos(t - lag_frames * estimator.dt) |
||||
|
||||
# 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 = desired_la / (vego ** 2) |
||||
actual_yr = actual_la / vego |
||||
msgs = [ |
||||
(t, "carControl", mocker.Mock(latActive=not rejected)), |
||||
(t, "carState", mocker.Mock(vEgo=vego, steeringPressed=False)), |
||||
(t, "controlsState", mocker.Mock(desiredCurvature=desired_cuvature, |
||||
lateralControlState=mocker.Mock(which=mocker.Mock(return_value='debugControlState'), debugControlState=ZeroMock()))), |
||||
(t, "livePose", mocker.Mock(orientationNED=ZeroMock(), |
||||
velocityDevice=ZeroMock(), |
||||
accelerationDevice=ZeroMock(), |
||||
angularVelocityDevice=ZeroMock(z=actual_yr))), |
||||
] |
||||
for t, w, m in msgs: |
||||
estimator.handle_log(t, w, m) |
||||
estimator.update_points() |
||||
estimator.update_estimate() |
||||
|
||||
|
||||
class TestLagd: |
||||
def test_read_saved_params(self): |
||||
params = Params() |
||||
|
||||
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams]) |
||||
CP = next(m for m in lr if m.which() == "carParams").carParams |
||||
|
||||
msg = messaging.new_message('liveDelay') |
||||
msg.liveDelay.lateralDelayEstimate = random.random() |
||||
msg.liveDelay.validBlocks = random.randint(1, 10) |
||||
params.put("LiveDelay", msg.to_bytes()) |
||||
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes()) |
||||
|
||||
saved_lag_params = retrieve_initial_lag(params, CP) |
||||
assert saved_lag_params is not None |
||||
|
||||
lag, valid_blocks = saved_lag_params |
||||
assert lag == msg.liveDelay.lateralDelayEstimate |
||||
assert valid_blocks == msg.liveDelay.validBlocks |
||||
|
||||
def test_ncc(self): |
||||
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 += np.random.normal(0, 0.05, len(desired_sig)) |
||||
actual_sig += np.random.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 = np.random.choice([True, False], size=len(desired_sig), p=[0.6, 0.4]) |
||||
desired_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask)) |
||||
actual_sig[~mask] = np.random.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, mocker): |
||||
mocked_CP = mocker.Mock(steerActuatorDelay=0.8) |
||||
estimator = LateralLagEstimator(mocked_CP, DT) |
||||
msg = estimator.get_msg(True) |
||||
assert msg.liveDelay.status == 'unestimated' |
||||
assert np.allclose(msg.liveDelay.lateralDelay, estimator.initial_lag) |
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, estimator.initial_lag) |
||||
assert msg.liveDelay.validBlocks == 0 |
||||
|
||||
def test_estimator_basics(self, mocker, subtests): |
||||
for lag_frames in range(5): |
||||
with subtests.test(msg=f"lag_frames={lag_frames}"): |
||||
mocked_CP = mocker.Mock(steerActuatorDelay=0.8) |
||||
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0) |
||||
process_messages(mocker, estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) |
||||
msg = estimator.get_msg(True) |
||||
assert msg.liveDelay.status == 'estimated' |
||||
assert np.allclose(msg.liveDelay.lateralDelay, lag_frames * DT, atol=0.01) |
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) |
||||
assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED |
||||
|
||||
def test_estimator_masking(self, mocker): |
||||
mocked_CP, lag_frames = mocker.Mock(steerActuatorDelay=0.8), random.randint(1, 19) |
||||
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1) |
||||
process_messages(mocker, 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.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) |
||||
|
||||
@pytest.mark.skipif(PC, reason="only on device") |
||||
@pytest.mark.timeout(60) |
||||
def test_estimator_performance(self, mocker): |
||||
mocked_CP = mocker.Mock(steerActuatorDelay=0.8) |
||||
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 |
||||
@ -1 +1 @@ |
||||
37fd7afb99bf188c1e5375d01f011ae35821f640 |
||||
4576c437dc14bc830956dd272dade4d7f027dab5 |
||||
Loading…
Reference in new issue