diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index a61793e223..e070c16b1c 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -157,7 +157,8 @@ class LateralLagEstimator: 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, - max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE): + max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE, + enabled: bool = True): self.dt = dt self.window_sec = window_sec self.okay_window_sec = okay_window_sec @@ -172,6 +173,7 @@ class LateralLagEstimator: self.min_confidence = min_confidence self.max_lat_accel = max_lat_accel self.max_lat_accel_diff = max_lat_accel_diff + self.enabled = enabled self.t = 0.0 self.lat_active = False @@ -206,7 +208,7 @@ class LateralLagEstimator: liveDelay = msg.liveDelay valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get() - if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): + if self.enabled and self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): if valid_std > MAX_LAG_STD: liveDelay.status = log.LiveDelayData.Status.invalid else: @@ -367,7 +369,10 @@ def main(): params = Params() CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency) + # TODO: remove me, lagd is in shadow mode on release + is_release = params.get_bool("IsReleaseBranch") + + lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency, enabled=not is_release) if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None: lag, valid_blocks = initial_lag_params lag_learner.reset(lag, valid_blocks) diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index aad8cff229..a7f9c75ab4 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -108,6 +108,18 @@ class TestLagd: assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED + def test_disabled_estimator(self): + mocked_CP = car.CarParams(steerActuatorDelay=0.8) + estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, enabled=False) + lag_frames = 5 + process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) + msg = estimator.get_msg(True) + assert msg.liveDelay.status == 'unestimated' + assert np.allclose(msg.liveDelay.lateralDelay, 1.0, atol=0.01) + assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) + assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) + assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED + def test_estimator_masking(self): mocked_CP, lag_frames = car.CarParams(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)