modeld: raise low-speed curvature thresholds (#39182)

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:

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))

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'),

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',