From b6a29d0d1c37eaa4bb6093d041df89e1c3f9e16f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Fri, 7 Feb 2025 20:43:34 -0800 Subject: [PATCH] Move to lagd --- selfdrive/locationd/lagd.py | 125 ++++++++++++++++++ selfdrive/locationd/torqued.py | 100 +------------- .../test/process_replay/process_replay.py | 16 ++- system/manager/process_config.py | 1 + 4 files changed, 145 insertions(+), 97 deletions(-) create mode 100644 selfdrive/locationd/lagd.py diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py new file mode 100644 index 0000000000..f8e5fb061a --- /dev/null +++ b/selfdrive/locationd/lagd.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +import numpy as np +from collections import deque + +import cereal.messaging as messaging +from cereal import car +from openpilot.common.params import Params +from openpilot.common.realtime import config_realtime_process, DT_CTRL +from openpilot.selfdrive.locationd.helpers import ParameterEstimator + +MIN_LAG_VEL = 15.0 +MAX_SANE_LAG = 3.0 +MIN_HIST_LEN_SEC = 60 +MAX_LAG_HIST_LEN_SEC = 300 +MOVING_CORR_WINDOW = 60 +OVERLAP_FACTOR = 0.25 + + +class LagEstimator(ParameterEstimator): + def __init__(self, CP, dt, min_hist_len_sec, max_lag_hist_len_sec, moving_corr_window, overlap_factor): + self.dt = dt + self.min_hist_len = int(min_hist_len_sec / self.dt) + self.window_len = int(moving_corr_window / self.dt) + self.initial_lag = CP.steerActuatorDelay + self.current_lag = self.initial_lag + + self.lat_active = False + self.steering_pressed = False + self.v_ego = 0.0 + self.lags = deque(maxlen= int(max_lag_hist_len_sec / (moving_corr_window * overlap_factor))) + self.curvature = deque(maxlen=int(moving_corr_window / self.dt)) + self.desired_curvature = deque(maxlen=int(moving_corr_window / self.dt)) + self.frame = 0 + + def correlation_lags(self, sig_len, dt): + return np.arange(0, sig_len) * dt + + def actuator_delay(self, expected_sig, actual_sig, dt, max_lag=MAX_SANE_LAG): + assert len(expected_sig) == len(actual_sig) + correlations = np.correlate(expected_sig, actual_sig, mode='full') + lags = self.correlation_lags(len(expected_sig), dt) + + # only consider negative time shifts within the max_lag + n_frames_max_delay = int(max_lag / dt) + correlations = correlations[len(expected_sig) - 1: len(expected_sig) - 1 + n_frames_max_delay] + lags = lags[:n_frames_max_delay] + + max_corr_index = np.argmax(correlations) + + lag, corr = lags[max_corr_index], correlations[max_corr_index] + return lag, corr + + def handle_log(self, t, which, msg) -> None: + if which == "carControl": + self.lat_active = msg.latActive + elif which == "carState": + self.steering_pressed = msg.steeringPressed + self.v_ego = msg.vEgo + elif which == "controlsState": + curvature = msg.curvature + desired_curvature = msg.desiredCurvature + if self.lat_active and not self.steering_pressed: + self.curvature.append((t, curvature)) + self.desired_curvature.append((t, desired_curvature)) + self.frame += 1 + + def get_msg(self, valid: bool, with_points: bool): + if len(self.curvature) >= self.min_hist_len: + if self.frame % int(self.window_len * OVERLAP_FACTOR) == 0: + _, curvature = zip(*self.curvature) + _, desired_curvature = zip(*self.desired_curvature) + delay_curvature, _ = self.actuator_delay(curvature, desired_curvature, self.dt) + if delay_curvature != 0.0: + self.lags.append(delay_curvature) + # FIXME: this is fragile and ugly, refactor this + if len(self.lags) > 0: + steer_actuation_delay = float(np.mean(self.lags)) + else: + steer_actuation_delay = self.initial_lag + else: + steer_actuation_delay = self.initial_lag + + msg = messaging.new_message('liveActuatorDelay') + msg.valid = valid + + liveActuatorDelay = msg.liveActuatorDelay + liveActuatorDelay.steerActuatorDelay = steer_actuation_delay + liveActuatorDelay.totalPoints = len(self.curvature) + + if with_points: + liveActuatorDelay.points = [[c, dc] for ((_, c), (_, dc)) in zip(self.curvature, self.desired_curvature)] + + return msg + + +def main(): + config_realtime_process([0, 1, 2, 3], 5) + + pm = messaging.PubMaster(['liveActuatorDelay', 'alertDebug']) + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState'], poll='controlsState') + + params = Params() + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + estimator = LagEstimator(CP, DT_CTRL, MIN_HIST_LEN_SEC, MAX_LAG_HIST_LEN_SEC, MOVING_CORR_WINDOW, OVERLAP_FACTOR) + + while True: + sm.update() + if sm.all_checks(): + for which in sm.updated.keys(): + if sm.updated[which]: + t = sm.logMonoTime[which] * 1e-9 + estimator.handle_log(t, which, sm[which]) + + if sm.frame % 25 == 0: + msg = estimator.get_msg(sm.all_checks(), with_points=True) + alert_msg = messaging.new_message('alertDebug') + alert_msg.alertDebug.alertText1 = f"Lag estimateb (fixed: {CP.steerActuatorDelay:.2f} s)" + alert_msg.alertDebug.alertText2 = f"{msg.liveActuatorDelay.steerActuatorDelay:.2f} s" + + pm.send('liveActuatorDelay', msg) + pm.send('alertDebug', alert_msg) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index cbad35ee07..986c5349f5 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -5,7 +5,7 @@ from collections import deque, defaultdict import cereal.messaging as messaging from cereal import car, log from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, DT_MDL, DT_CTRL +from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY @@ -41,91 +41,6 @@ def slope2rot(slope): return np.array([[cos, -sin], [sin, cos]]) -MIN_LAG_VEL = 15.0 -MAX_SANE_LAG = 3.0 -MIN_HIST_LEN_SEC = 30 -MAX_HIST_LEN_SEC = 120 -MAX_LAG_HIST_LEN_SEC = 300 -MOVING_CORR_WINDOW = 30 -OVERLAP_FACTOR = 0.25 - -class LagEstimator(ParameterEstimator): - def __init__(self, CP, dt, min_hist_len_sec, max_lag_hist_len_sec, moving_corr_window, overlap_factor): - self.dt = dt - self.min_hist_len = int(min_hist_len_sec / self.dt) - self.window_len = int(moving_corr_window / self.dt) - self.initial_lag = CP.steerActuatorDelay - self.current_lag = self.initial_lag - - self.lat_active = False - self.steering_pressed = False - self.v_ego = 0.0 - self.lags = deque(maxlen= int(max_lag_hist_len_sec / (moving_corr_window * overlap_factor))) - self.curvature = deque(maxlen=int(moving_corr_window / self.dt)) - self.desired_curvature = deque(maxlen=int(moving_corr_window / self.dt)) - self.frame = 0 - - def correlation_lags(self, sig_len, dt): - return np.arange(0, sig_len) * dt - - def actuator_delay(self, expected_sig, actual_sig, dt, max_lag=MAX_SANE_LAG): - assert len(expected_sig) == len(actual_sig) - correlations = np.correlate(expected_sig, actual_sig, mode='full') - lags = self.correlation_lags(len(expected_sig), dt) - - # only consider negative time shifts within the max_lag - n_frames_max_delay = int(max_lag / dt) - correlations = correlations[len(expected_sig) - 1: len(expected_sig) - 1 + n_frames_max_delay] - lags = lags[:n_frames_max_delay] - - max_corr_index = np.argmax(correlations) - - lag, corr = lags[max_corr_index], correlations[max_corr_index] - return lag, corr - - def handle_log(self, t, which, msg) -> None: - if which == "carControl": - self.lat_active = msg.latActive - elif which == "carState": - self.steering_pressed = msg.steeringPressed - self.v_ego = msg.vEgo - elif which == "controlsState": - curvature = msg.curvature - desired_curvature = msg.desiredCurvature - if self.lat_active and not self.steering_pressed: - self.curvature.append((t, curvature)) - self.desired_curvature.append((t, desired_curvature)) - self.frame += 1 - - def get_msg(self, valid: bool, with_points: bool): - if len(self.curvature) >= self.min_hist_len: - if self.frame % int(self.window_len * OVERLAP_FACTOR) == 0: - _, curvature = zip(*self.curvature) - _, desired_curvature = zip(*self.desired_curvature) - delay_curvature, _ = self.actuator_delay(curvature, desired_curvature, self.dt) - if delay_curvature != 0.0: - self.lags.append(delay_curvature) - # FIXME: this is fragile and ugly, refactor this - if len(self.lags) > 0: - steer_actuation_delay = float(np.mean(self.lags)) - else: - steer_actuation_delay = self.initial_lag - else: - steer_actuation_delay = self.initial_lag - - msg = messaging.new_message('liveActuatorDelay') - msg.valid = valid - - liveActuatorDelay = msg.liveActuatorDelay - liveActuatorDelay.steerActuatorDelay = steer_actuation_delay - liveActuatorDelay.totalPoints = len(self.curvature) - - if with_points: - liveActuatorDelay.points = [[c, dc] for ((_, c), (_, dc)) in zip(self.curvature, self.desired_curvature)] - - return msg - - class TorqueBuckets(PointBuckets): def add_point(self, x, y): for bound_min, bound_max in self.x_bounds: @@ -325,14 +240,11 @@ class TorqueEstimator(ParameterEstimator): def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) - pm = messaging.PubMaster(['liveTorqueParameters', 'liveActuatorDelay']) - sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'controlsState', 'liveCalibration', 'livePose'], poll='livePose') + pm = messaging.PubMaster(['liveTorqueParameters']) + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose'], poll='livePose') params = Params() - CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - estimator = TorqueEstimator(CP) - - lag_estimator = LagEstimator(CP, DT_MDL, MIN_HIST_LEN_SEC, MAX_LAG_HIST_LEN_SEC, MOVING_CORR_WINDOW, OVERLAP_FACTOR) + estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) while True: sm.update() @@ -341,12 +253,10 @@ def main(demo=False): if sm.updated[which]: t = sm.logMonoTime[which] * 1e-9 estimator.handle_log(t, which, sm[which]) - lag_estimator.handle_log(t, which, sm[which]) # 4Hz driven by livePose if sm.frame % 5 == 0: - pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose']))) - pm.send('liveActuatorDelay', lag_estimator.get_msg(valid=sm.all_checks(['carControl', 'carState', 'controlsState']), with_points=True)) + pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) # Cache points every 60 seconds while onroad if sm.frame % 240 == 0: diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 8fc5be1f2d..e4b3ac88e2 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -400,6 +400,9 @@ def torqued_rcv_callback(msg, cfg, frame): # should_recv always true to increment frame return (frame - 1) == 0 or msg.which() == 'livePose' +def lagd_rcv_callback(msg, cfg, frame): + return (frame - 1) == 0 or msg.which() == 'controlsState' + def dmonitoringmodeld_rcv_callback(msg, cfg, frame): return msg.which() == "driverCameraState" @@ -559,13 +562,22 @@ CONFIGS = [ ), ProcessConfig( proc_name="torqued", - pubs=["livePose", "liveCalibration", "carState", "carControl", "carOutput", "controlsState"], - subs=["liveTorqueParameters", "liveActuatorDelay"], + pubs=["livePose", "liveCalibration", "carState", "carControl", "carOutput"], + subs=["liveTorqueParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, should_recv_callback=torqued_rcv_callback, tolerance=NUMPY_TOLERANCE, ), + ProcessConfig( + proc_name="lagd", + pubs=["carState", "carControl", "controlsState"], + subs=["liveActuatorDelay"], + ignore=["logMonoTime"], + init_callback=get_car_params_callback, + should_recv_callback=lagd_rcv_callback, + tolerance=NUMPY_TOLERANCE, + ), ProcessConfig( proc_name="modeld", pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState", "carState"], diff --git a/system/manager/process_config.py b/system/manager/process_config.py index b9655c83f3..1335666650 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -85,6 +85,7 @@ procs = [ NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), + PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad), PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)), PythonProcess("joystickd", "tools.joystick.joystickd", or_(joystick, notcar)), PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad),