mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 16:26:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

lagd: disable in release (#35426)

Browse Source 
* disable in release

* pass

* one liner

* Update selfdrive/locationd/lagd.py

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* test

---------

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
This commit is contained in:
Shane Smiskol
2025-06-02 21:29:05 -07:00
committed by GitHub
parent 108790870e
commit 6f6adc10a8
2 changed files with 20 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/lagd.py
+8 -3
View File
@@ -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)
selfdrive/locationd/test/test_lagd.py
+12
View File
@@ -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)