mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-09-12 03:03:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix: align curvatured tests with current lookup semantics

Browse Source
This commit is contained in:
infiniteCable2
2026-08-15 14:14:15 +02:00
parent 09fbdb9990
commit 88c0cc111e
2 changed files with 22 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/tests/test_curvatured.py
+13 -11
View File
@@ -175,10 +175,12 @@ class TestCurvatureDController(OpenpilotTestCase):
outer_idx = CurvatureDLookup.curvature_index(1.5e-3)
assert outer_idx is not None
self._set_curve(msg, 3, {outer_idx: 8.0e-5})
self._set_curve(msg, 0, {outer_idx: 8.0e-5})
controller.update_live_params(msg.lateralCurvatureParameters)
v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3])
# Use the lowest speed anchor so the global outer fade range remains
# inside the independent lateral-acceleration safety gate.
v_ego = float(CurvatureDLookup.SPEED_ANCHORS[0])
outer = controller.get_correction(1.5e-3, v_ego)
assert outer > 0.0
@@ -193,10 +195,10 @@ class TestCurvatureDController(OpenpilotTestCase):
msg.lateralCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size()
outer_idx = len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS) - 1
self._set_curve(msg, 3, {outer_idx: 8.0e-5})
self._set_curve(msg, 0, {outer_idx: 8.0e-5})
controller.update_live_params(msg.lateralCurvatureParameters)
v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3])
v_ego = float(CurvatureDLookup.SPEED_ANCHORS[0])
last_edge = float(CurvatureDLookup.CURVATURE_BUCKET_MAX)
fade_mid = 0.5 * (last_edge + float(CurvatureDLookup.CURVATURE_MAX))
@@ -229,11 +231,11 @@ class TestCurvatureDController(OpenpilotTestCase):
# Wrap the source to count calls
call_count = {"n": 0}
original = CurvatureDLookup.interp_curve_value
def counting(*args, **kwargs):
original = CurvatureDLookup.interp_curve_value.__func__
def counting(cls, *args, **kwargs):
call_count["n"] += 1
return original(*args, **kwargs)
CurvatureDLookup.interp_curve_value = counting # ty: ignore[invalid-assignment]
return original(cls, *args, **kwargs)
CurvatureDLookup.interp_curve_value = classmethod(counting) # ty: ignore[invalid-assignment]
try:
# First call: cache miss, calls interp_curve_value once
first = controller.get_correction(32e-6, v_ego)
@@ -246,17 +248,17 @@ class TestCurvatureDController(OpenpilotTestCase):
# v_ego noise below quantization must still hit the cache
v_ego_step = 10 ** -CACHE_V_EGO_DECIMALS
noised = controller.get_correction(32e-6, v_ego + v_ego_step * 0.5)
noised = controller.get_correction(32e-6, round(v_ego, CACHE_V_EGO_DECIMALS) + v_ego_step * 0.25)
assert noised == first
assert call_count["n"] == 1
# Curvature noise below quantization must still hit the cache
curvature_step = 10 ** -CACHE_CURVATURE_DECIMALS
noised = controller.get_correction(32e-6 + curvature_step * 0.5, v_ego)
noised = controller.get_correction(round(32e-6, CACHE_CURVATURE_DECIMALS) + curvature_step * 0.25, v_ego)
assert noised == first
assert call_count["n"] == 1
finally:
CurvatureDLookup.interp_curve_value = original
CurvatureDLookup.interp_curve_value = classmethod(original)
def test_get_correction_cache_invalidates_on_live_params_update(self):
"""Cache must be invalidated when fit_corrections / fit_valid change,
openpilot/selfdrive/locationd/test/test_curvatured.py
+9 -7
View File
@@ -155,8 +155,8 @@ class TestCurvatureEstimator(OpenpilotTestCase):
mid = CurvatureDLookup.required_support_bucket_count(3)
high = CurvatureDLookup.required_support_bucket_count(6)
assert low == len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS)
assert low >= mid >= high >= CurvatureDLookup.MIN_REQUIRED_SUPPORT_BUCKETS
assert len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS) >= low >= mid >= high >= CurvatureDLookup.MIN_REQUIRED_SUPPORT_BUCKETS
assert low > high
def test_fit_valid_no_longer_requires_global_total_samples(self):
speed_idx = 3
@@ -215,6 +215,7 @@ class TestCurvatureEstimator(OpenpilotTestCase):
v_ego = 22.0
self._train_speed_curve(estimator, v_ego)
estimator.use_params = True
estimator._update_current_lookup(desired_curvature, v_ego)
msg = estimator.get_msg(include_debug=True, include_preview=True)
idx = CurvatureDLookup.indices(desired_curvature, v_ego)
@@ -252,10 +253,11 @@ class TestCurvatureEstimator(OpenpilotTestCase):
speed_idx = 3
counts = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32)
bias = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32)
selected = np.array([5, 6, 7, 8], dtype=int)
required = CurvatureDLookup.required_support_bucket_count(speed_idx)
selected = np.arange(required, dtype=int)
counts[speed_idx, selected] = CurvatureDLookup.MIN_BUCKET_POINTS[selected] + 40.0
bias[speed_idx, selected] = np.array([2.0e-6, 6.0e-6, 1.2e-5, 2.0e-5], dtype=np.float32)
bias[speed_idx, selected] = np.linspace(2.0e-6, 2.0e-5, required, dtype=np.float32)
fit_corrections, fit_valid = CurvatureDLookup.build_fit_corrections(bias, counts)
@@ -286,11 +288,11 @@ class TestCurvatureEstimator(OpenpilotTestCase):
fit_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool)
fit_valid[speed_idx, 3] = True
fit_valid[speed_idx, 6] = True
fit_valid[speed_idx, 8] = True
fit_corrections[speed_idx, 3] = 1.0e-6
fit_corrections[speed_idx, 6] = 8.0e-6
fit_corrections[speed_idx, 8] = 8.0e-6
gap_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[4])
gap_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[5])
valid_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[3])
assert CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, gap_curvature) == 0.0