diff --git a/cereal/services.py b/cereal/services.py index aad83177bb..82fc04bd00 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -36,6 +36,7 @@ _services: dict[str, tuple] = { "errorLogMessage": (True, 0., 1), "liveCalibration": (True, 4., 4), "liveTorqueParameters": (True, 4., 1), + "liveDelay": (True, 4., 1), "androidLog": (True, 0.), "carState": (True, 100., 10), "carControl": (True, 100., 10), diff --git a/common/params_keys.h b/common/params_keys.h index fd701cda18..e630792357 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -71,6 +71,7 @@ inline static std::unordered_map keys = { {"LastPowerDropDetected", CLEAR_ON_MANAGER_START}, {"LastUpdateException", CLEAR_ON_MANAGER_START}, {"LastUpdateTime", PERSISTENT}, + {"LiveDelay", PERSISTENT}, {"LiveParameters", PERSISTENT}, {"LiveParametersV2", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index af307b0336..a3e3ae4a8c 100644 --- a/selfdrive/locationd/helpers.py +++ b/selfdrive/locationd/helpers.py @@ -1,10 +1,48 @@ import numpy as np from typing import Any +from functools import cache from cereal import log from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot +@cache +def fft_next_good_size(n: int) -> int: + """ + smallest composite of 2, 3, 5, 7, 11 that is >= n + inspired by pocketfft + """ + if n <= 6: + return n + best, f2 = 2 * n, 1 + while f2 < best: + f23 = f2 + while f23 < best: + f235 = f23 + while f235 < best: + f2357 = f235 + while f2357 < best: + f235711 = f2357 + while f235711 < best: + best = f235711 if f235711 >= n else best + f235711 *= 11 + f2357 *= 7 + f235 *= 5 + f23 *= 3 + f2 *= 2 + return best + + +def parabolic_peak_interp(R, max_index): + if max_index == 0 or max_index == len(R) - 1: + return max_index + + y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1] + offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1) + + return max_index + offset + + def rotate_cov(rot_matrix, cov_in): return rot_matrix @ cov_in @ rot_matrix.T diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py new file mode 100755 index 0000000000..5ce82309ea --- /dev/null +++ b/selfdrive/locationd/lagd.py @@ -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) diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py new file mode 100644 index 0000000000..53af2d2573 --- /dev/null +++ b/selfdrive/locationd/test/test_lagd.py @@ -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 diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index b1399ab43b..c2e8259135 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -75,7 +75,7 @@ class SelfdriveD: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 3ee761ee44..9715e48615 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -462,7 +462,7 @@ CONFIGS = [ proc_name="selfdrived", pubs=[ "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", - "longitudinalPlan", "livePose", "liveParameters", "radarState", + "longitudinalPlan", "livePose", "liveDelay", "liveParameters", "radarState", "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput", "gpsLocationExternal", "gpsLocation", "controlsState", "carControl", "driverAssistance", "alertDebug", @@ -551,6 +551,15 @@ CONFIGS = [ tolerance=NUMPY_TOLERANCE, processing_time=0.004, ), + ProcessConfig( + proc_name="lagd", + pubs=["livePose", "liveCalibration", "carState", "carControl", "controlsState"], + subs=["liveDelay"], + ignore=["logMonoTime"], + init_callback=get_car_params_callback, + should_recv_callback=MessageBasedRcvCallback("livePose"), + tolerance=NUMPY_TOLERANCE, + ), ProcessConfig( proc_name="ubloxd", pubs=["ubloxRaw"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d4b02296d6..d7eb3686db 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -37fd7afb99bf188c1e5375d01f011ae35821f640 \ No newline at end of file +4576c437dc14bc830956dd272dade4d7f027dab5 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index bb30ab2acd..d479369693 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -41,23 +41,23 @@ source_segments = [ ] segments = [ - ("BODY", "regenA67A128BCD8|2024-08-30--02-36-22--0"), - ("HYUNDAI", "regenCCD47FEBC0C|2025-03-04--03-21-48--0"), - ("HYUNDAI2", "regen306779F6870|2024-10-03--04-03-23--0"), - ("TOYOTA", "regen4A5115B248D|2025-03-04--03-21-43--0"), - ("TOYOTA2", "regen6E484EDAB96|2024-08-30--02-47-37--0"), - ("TOYOTA3", "8011d605be1cbb77|000000cc--8e8d8ec716--6"), - ("HONDA", "regenB8CABEC09CC|2025-03-04--03-32-55--0"), - ("HONDA2", "regen4B38A7428CD|2024-08-30--02-56-31--0"), - ("CHRYSLER", "regenF3DBBA9E8DF|2024-08-30--02-59-03--0"), - ("RAM", "regenDB02684E00A|2024-08-30--03-02-54--0"), - ("SUBARU", "regen5E3347D0A0F|2025-03-04--03-23-55--0"), - ("GM", "regen9ADBECBCD1C|2024-08-30--03-13-04--0"), - ("NISSAN", "regen58464878D07|2024-08-30--03-15-31--0"), - ("VOLKSWAGEN", "regenED976DEB757|2024-08-30--03-18-02--0"), + ("BODY", "regen2F3C7259F1B|2025-04-08--23-00-23--0"), + ("HYUNDAI", "regenAA0FC4ED71E|2025-04-08--22-57-50--0"), + ("HYUNDAI2", "regenAFB9780D823|2025-04-08--23-00-34--0"), + ("TOYOTA", "regen218A4DCFAA1|2025-04-08--22-57-51--0"), + ("TOYOTA2", "regen107352E20EB|2025-04-08--22-57-46--0"), + ("TOYOTA3", "regen1455E3B4BDF|2025-04-09--03-26-06--0"), + ("HONDA", "regenB328FF8BA0A|2025-04-08--22-57-45--0"), + ("HONDA2", "regen6170C8C9A35|2025-04-08--22-57-46--0"), + ("CHRYSLER", "regen5B28FC2A437|2025-04-08--23-04-24--0"), + ("RAM", "regenBF81EA96E08|2025-04-08--23-06-54--0"), + ("SUBARU", "regen7366F13F6A1|2025-04-08--23-07-07--0"), + ("GM", "regen1271097D038|2025-04-09--03-26-00--0"), + ("NISSAN", "regen15D60604EAB|2025-04-08--23-06-59--0"), + ("VOLKSWAGEN", "regen0F2F06C9539|2025-04-08--23-06-56--0"), ("MAZDA", "regenACF84CCF482|2024-08-30--03-21-55--0"), - ("FORD", "regenA75209BD115|2025-03-04--03-23-53--0"), - ("RIVIAN", "bc095dc92e101734|000000db--ee9fe46e57--1"), + ("FORD", "regen755D8CB1E1F|2025-04-08--23-13-43--0"), + ("RIVIAN", "regen5FCAC896BBE|2025-04-08--23-13-35--0"), ] # dashcamOnly makes don't need to be tested until a full port is done @@ -195,7 +195,7 @@ if __name__ == "__main__": continue # to speed things up, we only test all segments on card - if cfg.proc_name != 'card' and car_brand not in ('HYUNDAI', 'TOYOTA', 'HONDA', 'SUBARU', 'FORD'): + if cfg.proc_name != 'card' and car_brand not in ('HYUNDAI', 'TOYOTA', 'HONDA', 'SUBARU', 'FORD', 'RIVIAN'): continue cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.zst") diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index ebc87800b7..4cd952219c 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -44,7 +44,6 @@ PROCS = { "./camerad": 10.0, "selfdrive.controls.plannerd": 9.0, "./ui": 18.0, - "selfdrive.locationd.paramsd": 9.0, "./sensord": 7.0, "selfdrive.controls.radard": 2.0, "selfdrive.modeld.modeld": 22.0, @@ -53,6 +52,8 @@ PROCS = { "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0, "selfdrive.locationd.locationd": 25.0, + "selfdrive.locationd.paramsd": 9.0, + "selfdrive.locationd.lagd": 11.0, "selfdrive.ui.soundd": 3.0, "selfdrive.monitoring.dmonitoringd": 4.0, "./proclogd": 2.0, diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 6e048c339e..e25d556037 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -94,6 +94,7 @@ procs = [ PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI), PythonProcess("pandad", "selfdrive.pandad.pandad", always_run), PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), + PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad), NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),