IQ.Pilot Release Commit @ a87e9e5

selfdrive/controls/lib/longitudinal_planner.py

@@ -31,6 +31,12 @@ LAUNCH_COMMIT_T = 3.5
 LAUNCH_MOVING_SPEED = 1.2
 LAUNCH_MAX_ACCEL = 1.5
 
+E2E_CRUISE_CONVERGENCE_TAU = 15.0
+E2E_CRUISE_ACCEL_MAX = 0.5
+E2E_MODEL_SPEED_HORIZON = 5.0
+E2E_ACCEL_INTENT_BP = [-0.05, 0.05]
+E2E_MODEL_SPEED_INTENT_BP = [-0.5, 0.0]
+
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
@@ -69,6 +75,23 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt,
   return target_accel, cruise_should_stop
 
 
+def get_e2e_accel(v_ego, v_cruise, model_v, a_target, should_stop):
+  if should_stop or v_cruise <= v_ego or len(model_v) != len(T_IDXS_MPC):
+    return a_target
+
+  convergence_accel = min((v_cruise - v_ego) / E2E_CRUISE_CONVERGENCE_TAU, E2E_CRUISE_ACCEL_MAX)
+  if convergence_accel <= a_target:
+    return a_target
+
+  # Only help the model converge to cruise when both its immediate action and
+  # velocity trajectory show no active deceleration intent. The lead MPC and
+  # cruise candidates remain hard upper bounds on the final acceleration.
+  accel_intent = np.interp(a_target, E2E_ACCEL_INTENT_BP, [0.0, 1.0])
+  model_speed = np.interp(E2E_MODEL_SPEED_HORIZON, T_IDXS_MPC, model_v)
+  speed_intent = np.interp(model_speed - v_ego, E2E_MODEL_SPEED_INTENT_BP, [0.0, 1.0])
+  return float(np.interp(min(accel_intent, speed_intent), [0.0, 1.0], [a_target, convergence_accel]))
+
+
 class LongitudinalPlanner(LongitudinalPlannerIQ):
   def __init__(self, CP, CP_IQ, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
@@ -173,6 +196,8 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
     output_a_target_e2e, output_should_stop_e2e = self.apply_e2e_stop_distance(sm, v_ego, output_a_target_e2e, output_should_stop_e2e)
+    if self.is_e2e(sm):
+      output_a_target_e2e = get_e2e_accel(v_ego, v_cruise, model_v, output_a_target_e2e, output_should_stop_e2e)
 
     if sm['carState'].standstill:
       self.launch_armed = True

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -0,0 +1,31 @@
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_e2e_accel
+
+
+def model_velocity(v_ego, v_future):
+  return np.interp(T_IDXS, [T_IDXS[0], T_IDXS[-1]], [v_ego, v_future])
+
+
+class TestE2eCruiseConvergence:
+  def test_converges_when_model_wants_to_accelerate(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), 0.1, False) == pytest.approx(0.5)
+
+  def test_scales_down_near_cruise_speed(self):
+    assert get_e2e_accel(28.5, 30.0, model_velocity(28.5, 30.0), 0.0, False) == pytest.approx(0.05)
+
+  def test_preserves_active_model_deceleration(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), -0.05, False) == pytest.approx(-0.05)
+
+  def test_preserves_future_model_slowdown(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 18.0), 0.1, False) == pytest.approx(0.1)
+
+  @pytest.mark.parametrize("v_ego, v_cruise, should_stop", [
+    (30.0, 30.0, False),
+    (31.0, 30.0, False),
+    (20.0, 30.0, True),
+  ])
+  def test_never_overrides_cruise_or_stop(self, v_ego, v_cruise, should_stop):
+    assert get_e2e_accel(v_ego, v_cruise, model_velocity(v_ego, v_ego + 5.0), -0.2, should_stop) == pytest.approx(-0.2)

selfdrive/modeld/SConscript

@@ -72,7 +72,10 @@ for model_name in ['dmonitoring_model']:
   # inputs (onnx + tinygrad_repo + flags + metadata script) and output hashes; if it matches, skip
   # declaring the targets entirely. Any mismatch falls back to a normal on-device compile.
   if arch == "larch64":
-    from openpilot.selfdrive.modeld.prebuilt_models import verify_prebuilt, outputs_match
+    from openpilot.selfdrive.modeld.prebuilt_models import packaged_prebuilt_matches, verify_prebuilt, outputs_match
+    if packaged_prebuilt_matches(model_name):
+      print(lenv.PrettyNote('SKIP', f"{model_name} — packaged prebuilt pkl"))
+      continue
     if verify_prebuilt(model_name, flags):
       print(lenv.PrettyNote('SKIP', f"{model_name} — prebuilt pkl"))
       continue

selfdrive/modeld/prebuilt_models.py

@@ -77,7 +77,13 @@ def outputs_match(model_name: str) -> bool:
   return True
 
 
+def packaged_prebuilt_matches(model_name: str) -> bool:
+  return not (MODELS_DIR / f'{model_name}.onnx').is_file() and outputs_match(model_name)
+
+
 def verify_prebuilt(model_name: str, flags: str) -> bool:
+  if not (MODELS_DIR / f'{model_name}.onnx').is_file():
+    return False
   data = _load_checks().get(model_name, {})
   if data.get('signature') != compute_signature(model_name, flags):
     return False

selfdrive/modeld/test_prebuilt_models.py

@@ -0,0 +1,51 @@
+import hashlib
+import json
+
+from openpilot.selfdrive.modeld import prebuilt_models
+
+
+def write_outputs(models_dir, check_path):
+  outputs = {}
+  for name, contents in {
+    'dmonitoring_model_tinygrad.pkl': b'tinygrad',
+    'dmonitoring_model_metadata.pkl': b'metadata',
+  }.items():
+    (models_dir / name).write_bytes(contents)
+    outputs[name] = hashlib.sha256(contents).hexdigest()
+  check_path.write_text(json.dumps({'dmonitoring_model': {'outputs': outputs}}))
+
+
+def test_packaged_prebuilt_without_onnx(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
+  assert not prebuilt_models.verify_prebuilt('dmonitoring_model', 'flags')
+
+
+def test_packaged_prebuilt_rejects_corrupt_output(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  (models_dir / 'dmonitoring_model_tinygrad.pkl').write_bytes(b'corrupt')
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
+
+
+def test_source_checkout_is_not_packaged_prebuilt(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  (models_dir / 'dmonitoring_model.onnx').write_bytes(b'onnx')
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')