From 07606c8f9e04122acecb91ef52202e4c721e9bdb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Mon, 2 Nov 2026 18:36:18 +0000 Subject: [PATCH] modeld: raise low-speed curvature thresholds (#39182) --- openpilot/selfdrive/modeld/modeld.py | 2 +- .../tests/test_action_curvature_threshold.py | 48 +++++++++++++++++++ .../flm/producer_clock_equivalence.py | 2 +- .../tests/test_upstream_producer_transport.py | 2 +- 4 files changed, 51 insertions(+), 3 deletions(-) create mode 100644 openpilot/selfdrive/modeld/tests/test_action_curvature_threshold.py diff --git a/openpilot/selfdrive/modeld/modeld.py b/openpilot/selfdrive/modeld/modeld.py index b07e60b0b2..d78249bba3 100755 --- a/openpilot/selfdrive/modeld/modeld.py +++ b/openpilot/selfdrive/modeld/modeld.py @@ -83,7 +83,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log. lat_action_t) else: desired_accel = model_output['action'][0,1] - desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2 + desired_curvature = model_output['action'][0,0] / (max(2.5, v_ego))**2 stop = should_stop(v_ego, desired_accel) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) if v_ego > MIN_LAT_CONTROL_SPEED: diff --git a/openpilot/selfdrive/modeld/tests/test_action_curvature_threshold.py b/openpilot/selfdrive/modeld/tests/test_action_curvature_threshold.py new file mode 100644 index 0000000000..c25e6d21ce --- /dev/null +++ b/openpilot/selfdrive/modeld/tests/test_action_curvature_threshold.py @@ -0,0 +1,48 @@ +import ast +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value +from openpilot.selfdrive.modeld.constants import LAT_SMOOTH_SECONDS + + +def action_function(): + path = Path(__file__).resolve().parents[1] / "modeld.py" + function = next(node for node in ast.parse(path.read_text()).body + if isinstance(node, ast.FunctionDef) and node.name == "get_action_from_model") + namespace = {"np": np, "log": log, "smooth_value": smooth_value, + "LONG_SMOOTH_SECONDS": 0.3, "LAT_SMOOTH_SECONDS": LAT_SMOOTH_SECONDS, + "MIN_LAT_CONTROL_SPEED": 0.3, "should_stop": lambda speed, accel: False} + exec(compile(ast.Module(body=[function], type_ignores=[]), str(path), "exec"), namespace) + return namespace["get_action_from_model"] + + +@pytest.mark.parametrize("speed", [0.0, 0.3, 0.31, 1.0, 2.0, 2.5, 5.0, 30.0]) +def test_action_curvature_low_speed_threshold(speed): + previous = log.ModelDataV2.Action(desiredCurvature=0.01, desiredAcceleration=0.1) + action = action_function()({"action": np.array([[0.25, 0.5]])}, previous, 0.2, 0.2, speed) + expected = smooth_value(0.25 / max(2.5, speed) ** 2, 0.01, LAT_SMOOTH_SECONDS) if speed > 0.3 else 0.01 + assert action.desiredCurvature == pytest.approx(expected) + assert action.desiredAcceleration == pytest.approx(smooth_value(0.5, 0.1, 0.3)) + assert not action.shouldStop + + +def test_legacy_plan_action_is_unchanged(): + function = action_function() + calls = [] + def curvature(*args): + calls.append(args) + return 0.02 + function.__globals__.update(Plan=SimpleNamespace(VELOCITY=0, ACCELERATION=1, T_FROM_CURRENT_EULER=2, ORIENTATION_RATE=3), + ModelConstants=SimpleNamespace(T_IDXS=np.array([0.0, 1.0])), + get_accel_from_plan=lambda *args, **kwargs: 0.4, + get_curvature_from_plan=curvature) + previous = log.ModelDataV2.Action(desiredCurvature=0.01, desiredAcceleration=0.1) + action = function({"plan": np.zeros((1, 2, 4, 3))}, previous, 0.2, 0.2, 1.0) + assert len(calls) == 1 + assert action.desiredCurvature == pytest.approx(smooth_value(0.02, 0.01, LAT_SMOOTH_SECONDS)) + assert action.desiredAcceleration == pytest.approx(smooth_value(0.4, 0.1, 0.3)) diff --git a/openpilot/starpilot/flm/producer_clock_equivalence.py b/openpilot/starpilot/flm/producer_clock_equivalence.py index 6f395e12b1..87d09ee04d 100644 --- a/openpilot/starpilot/flm/producer_clock_equivalence.py +++ b/openpilot/starpilot/flm/producer_clock_equivalence.py @@ -18,7 +18,7 @@ PRODUCER_SOURCE_HASHES = ( ('openpilot', 'system/camerad/cameras/spectra.cc', '4fa26c17f0fb792a4ab6815657beccddee1d641faa2fd02e5aa7393f717a2f3a'), ('openpilot', 'system/camerad/cameras/camera_qcom2.cc', 'b34c24c9984a0ac086f7b961cb7ff09d0501231da50aaad0e676bb3dc3f2edd9'), ('openpilot', 'system/camerad/cameras/camera_common.cc', 'bd9a416a6ee21047b4deec6df04276d8717cd92fe14de43f3e9daff86b62bb8f'), - ('openpilot', 'selfdrive/modeld/modeld.py', 'cc825a027e667402373617ea28118692cfe8c49bf63217dd42395eb466ddc034'), + ('openpilot', 'selfdrive/modeld/modeld.py', '8d0fec445484b325e55cf4ee3bee95b7061d99b7e125611358eef407aa10e871'), ('openpilot', 'selfdrive/modeld/fill_model_msg.py', 'b3916c34d1073d0c26a7a8adc51e7b148f63a1ac68b388226170fd0f8b8b56b4'), ('openpilot', 'system/sensord/sensord.py', '7c79ab07d775622a43001e5a9b9ccc9a7bafb065940185849b33eefc2fd26420'), ('openpilot', 'system/sensord/sensors/lsm6ds3_gyro.py', '3e561f3d8572e1477c9b4b810c7570837d51e81c5672fe21ec67a68cae00dd92'), diff --git a/openpilot/starpilot/flm/tests/test_upstream_producer_transport.py b/openpilot/starpilot/flm/tests/test_upstream_producer_transport.py index 2296d35713..25fac9414d 100644 --- a/openpilot/starpilot/flm/tests/test_upstream_producer_transport.py +++ b/openpilot/starpilot/flm/tests/test_upstream_producer_transport.py @@ -9,7 +9,7 @@ from openpilot.starpilot.flm import producer_clock_equivalence as clock APPROVED = { - 'selfdrive/modeld/modeld.py': 'cc825a027e667402373617ea28118692cfe8c49bf63217dd42395eb466ddc034', + 'selfdrive/modeld/modeld.py': '8d0fec445484b325e55cf4ee3bee95b7061d99b7e125611358eef407aa10e871', 'visionipc/visionipc_py.cc': '6c131c4f5eccb9f822f1aaae63733f26b1427c52340b61c9cdaf6cd4aee2f89d', 'visionipc/visionipc_server.cc': 'a4325a5171eacb931eb271d06a992d2f27216040df113d92099f62cdfa67d81a', 'visionipc/visionipc_client.cc': 'd142cd60eedabfc581e9770fcd6f0ca81b762ba9dbc608b54dd20a27a317a52d',