Share corroborated CEM lead departure across vehicle configurations

openpilot/selfdrive/controls/plannerd.py

@@ -201,7 +201,7 @@ def conditional_handoff_for_frame(host: ConditionalPlannerHost | None, sm, CP, n
 
 def apply_conditional_stop_hold(host, planner, sm, CP, now_ns: int, drive_id: int, *, now_boot_ns: int | None = None, sample_skew_ns: int = 0) -> bool:
   def unavailable():
-    owner = getattr(host, 'ford_stopped_lead_release', None)
+    owner = getattr(host, 'stopped_lead_release', None)
     if owner is not None:
       owner.reset()
     return False
@@ -241,12 +241,12 @@ def apply_conditional_stop_hold(host, planner, sm, CP, now_ns: int, drive_id: in
       not any(lead.present for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo))):
     host.mode.projector.stop_detector.reset()
     return False
-  from openpilot.starpilot.car.ford.stop_release import owner_for
-  owner = getattr(host, 'ford_stopped_lead_release', None)
+  from openpilot.starpilot.conditional_mode.stop_release import owner_for
+  owner = getattr(host, 'stopped_lead_release', None)
   if owner is None:
     owner = owner_for(CP)
     if owner is not None:
-      host.ford_stopped_lead_release = owner
+      host.stopped_lead_release = owner
   if owner is not None and owner.sample(sm, CP, planner, host.mode.projector.stop_detector, now_ns=now_ns, drive_id=drive_id):
     detector = host.mode.projector.stop_detector
     detector.committed = detector.standstill_committed = False

openpilot/starpilot/car/ford/stop_release.py

@@ -1,92 +1 @@
-"""Corroborated Mach-E lead departure clears only an existing CEM model hold."""
-import math
-
-from opendbc.car import structs
-from opendbc.car.ford.aol import qualified
-from opendbc.car.ford.values import CAR
-from openpilot.starpilot.model_geometry import STANDSTILL_RELEASE_SPEED_MPS
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.starpilot.conditional_mode.projection import _standstill_path_extent
-from openpilot.starpilot.conditional_mode.stop import FROZEN_RAW_STOP_DISTANCE_M, STOP_MODEL_RELEASE_MARGIN_M
-
-SOURCES = ('carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState', 'modelV2')
-MAX_AGE_NS = 150_000_000
-CLEAR_DWELL_NS = 500_000_000
-
-
-def _supported(cp):
-  try:
-    return bool(cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and cp.openpilotLongitudinalControl and qualified(cp, marked_only=True))
-  except (AttributeError, TypeError, ValueError):
-    return False
-
-
-def owner_for(cp):
-  return StoppedLeadRelease() if _supported(cp) else None
-
-
-class StoppedLeadRelease:
-  def __init__(self):
-    self.reset()
-
-  def reset(self):
-    self.key = None
-    self.since_ns = self.last_ns = self.model_ns = 0
-
-  def sample(self, sm, cp, planner, detector, *, now_ns, drive_id):
-    try:
-      car, model, control = sm['carState'], sm['modelV2'], sm['carControl']
-      if (not _supported(cp) or type(now_ns) is not int or type(drive_id) is not int or not 0 < drive_id < now_ns or
-          not detector.committed or not detector.standstill_committed or detector.standstill_reason == 'sign' or
-          not all(sm.valid[name] and sm.alive[name] and drive_id < sm.logMonoTime[name] <= now_ns and
-                  now_ns - sm.logMonoTime[name] <= MAX_AGE_NS and
-                  drive_id < int(sm.recv_time[name] * 1e9) <= now_ns and
-                  now_ns - int(sm.recv_time[name] * 1e9) <= MAX_AGE_NS for name in SOURCES) or
-          not car.canValid or car.canTimeout or car.vehicleSensorsInvalid or car.accFaulted or
-          car.doorOpen or car.seatbeltUnlatched or car.regenBraking or
-          not car.standstill or car.vEgoRaw != 0. or car.gearShifter != structs.CarState.GearShifter.drive or
-          not math.isfinite(car.vEgo) or abs(car.vEgo) > STANDSTILL_RELEASE_SPEED_MPS or
-          car.gasPressed or car.brakePressed or not control.enabled or not control.longActive or
-          not sm['selfdriveState'].enabled or sm['controlsState'].forceDecel or
-          planner.mpc.solution_status != 0 or type(planner.output_should_stop) is not bool or
-          not math.isfinite(planner.output_a_target) or
-          planner.force_stop_plan.forcing or planner.force_stop_plan.should_stop or
-          model.action.shouldStop or not math.isfinite(model.action.desiredAcceleration) or model.action.desiredAcceleration < .3):
-        self.reset()
-        return False
-      extent = _standstill_path_extent(model)
-      if extent is None or extent < max(FROZEN_RAW_STOP_DISTANCE_M, detector.commit_distance_m) + STOP_MODEL_RELEASE_MARGIN_M:
-        self.reset()
-        return False
-      velocities, times = tuple(model.velocity.x), tuple(ModelConstants.T_IDXS)
-      if len(velocities) != 33 or len(times) != 33 or not all(math.isfinite(v) and -.05 <= v <= 100. for v in velocities):
-        self.reset()
-        return False
-      tail = [v for v, t in zip(velocities, times, strict=True) if t >= 2.]
-      if not tail or min(tail) < .3 or tail[-1] < 5. or any(b < a for a, b in zip(tail, tail[1:], strict=False)):
-        self.reset()
-        return False
-      leads = [lead for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo) if lead.present]
-      if not leads or any(not lead.radar or lead.radarTrackId < 0 or not all(math.isfinite(v) for v in
-          (lead.dRel, lead.vLead, lead.aLeadK, lead.yRel, lead.modelProb)) or
-          not 6.3 <= lead.dRel <= 18. or lead.vLead < .6 or lead.aLeadK < -.15 or
-          abs(lead.yRel) > 1. or lead.modelProb < .95 for lead in leads):
-        self.reset()
-        return False
-      key = (drive_id, tuple((str(config.safetyModel), config.safetyParam) for config in cp.safetyConfigs),
-             tuple(sorted({lead.radarTrackId for lead in leads})))
-      stamp = int(sm.logMonoTime['modelV2'])
-      if (key != self.key or not self.last_ns < now_ns <= self.last_ns + MAX_AGE_NS or stamp <= self.model_ns):
-        self.reset()
-        # Unknown derived geometry cannot establish a new light-hold proof.
-        if detector.standstill_reason != 'light':
-          return False
-        self.key, self.since_ns = key, now_ns
-      self.last_ns, self.model_ns = now_ns, stamp
-      # Confirm the scene while MPC settles; its current braking/stop decision
-      # remains an unconditional release veto. No target acceleration is raised.
-      return bool(now_ns - self.since_ns >= CLEAR_DWELL_NS and
-                  detector.standstill_reason == 'light' and not planner.output_should_stop and planner.output_a_target > .1)
-    except (AttributeError, KeyError, IndexError, TypeError, ValueError, OverflowError, RuntimeError):
-      self.reset()
-      return False
+from openpilot.starpilot.conditional_mode.stop_release import StoppedLeadRelease as StoppedLeadRelease, owner_for as owner_for

openpilot/starpilot/conditional_mode/stop_release.py

@@ -0,0 +1,103 @@
+"""Corroborated lead departure clears only an existing CEM model hold."""
+import math
+
+from opendbc.car import structs
+from openpilot.selfdrive.controls.radard import get_RadarState_from_vision
+from openpilot.starpilot.model_geometry import STANDSTILL_RELEASE_SPEED_MPS
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.starpilot.conditional_mode.projection import _standstill_path_extent
+from openpilot.starpilot.conditional_mode.stop import FROZEN_RAW_STOP_DISTANCE_M, STOP_MODEL_RELEASE_MARGIN_M
+
+SOURCES = ('carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState', 'modelV2')
+MAX_AGE_NS = 150_000_000
+CLEAR_DWELL_NS = 500_000_000
+
+
+def _supported(cp):
+  try:
+    return bool(cp.openpilotLongitudinalControl and not (cp.passive or cp.dashcamOnly or cp.notCar))
+  except (AttributeError, TypeError, ValueError):
+    return False
+
+
+def owner_for(cp):
+  return StoppedLeadRelease() if _supported(cp) else None
+
+
+class StoppedLeadRelease:
+  def __init__(self):
+    self.reset()
+
+  def reset(self):
+    self.key = None
+    self.since_ns = self.last_ns = self.model_ns = 0
+
+  def sample(self, sm, cp, planner, detector, *, now_ns, drive_id):
+    try:
+      car, model, control = sm['carState'], sm['modelV2'], sm['carControl']
+      if (not _supported(cp) or type(now_ns) is not int or type(drive_id) is not int or not 0 < drive_id < now_ns or
+          not detector.committed or not detector.standstill_committed or detector.standstill_reason == 'sign' or
+          not all(sm.valid[name] and sm.alive[name] and drive_id < sm.logMonoTime[name] <= now_ns and
+                  now_ns - sm.logMonoTime[name] <= MAX_AGE_NS and
+                  drive_id < int(sm.recv_time[name] * 1e9) <= now_ns and
+                  now_ns - int(sm.recv_time[name] * 1e9) <= MAX_AGE_NS for name in SOURCES) or
+          not car.canValid or car.canTimeout or car.vehicleSensorsInvalid or car.accFaulted or
+          car.doorOpen or car.seatbeltUnlatched or car.regenBraking or
+          not car.standstill or car.vEgoRaw != 0. or car.gearShifter != structs.CarState.GearShifter.drive or
+          not math.isfinite(car.vEgo) or abs(car.vEgo) > STANDSTILL_RELEASE_SPEED_MPS or
+          car.gasPressed or car.brakePressed or not control.enabled or not control.longActive or
+          not sm['selfdriveState'].enabled or sm['controlsState'].forceDecel or
+          planner.mpc.solution_status != 0 or type(planner.output_should_stop) is not bool or
+          not math.isfinite(planner.output_a_target) or
+          planner.force_stop_plan.forcing or planner.force_stop_plan.should_stop or
+          model.action.shouldStop or not math.isfinite(model.action.desiredAcceleration) or model.action.desiredAcceleration < .3):
+        self.reset()
+        return False
+      extent = _standstill_path_extent(model)
+      if extent is None or extent < max(FROZEN_RAW_STOP_DISTANCE_M, detector.commit_distance_m) + STOP_MODEL_RELEASE_MARGIN_M:
+        self.reset()
+        return False
+      velocities, times = tuple(model.velocity.x), tuple(ModelConstants.T_IDXS)
+      if len(velocities) != 33 or len(times) != 33 or not all(math.isfinite(v) and -.05 <= v <= 100. for v in velocities):
+        self.reset()
+        return False
+      tail = [v for v, t in zip(velocities, times, strict=True) if t >= 2.]
+      if not tail or min(tail) < .3 or tail[-1] < 5. or any(b < a for a, b in zip(tail, tail[1:], strict=False)):
+        self.reset()
+        return False
+      leads = [(slot, lead) for slot, lead in enumerate((sm['radarState'].leadOne, sm['radarState'].leadTwo)) if lead.present]
+      if not leads or any(lead.radar and lead.radarTrackId < 0 or not all(math.isfinite(v) for v in
+          (lead.dRel, lead.vLead, lead.aLeadK, lead.yRel, lead.modelProb)) or
+          not 6.3 <= lead.dRel <= 18. or lead.vLead < .6 or lead.aLeadK < -.15 or
+          abs(lead.yRel) > 1. or lead.modelProb < .95 for _, lead in leads):
+        self.reset()
+        return False
+      for slot, lead in leads:
+        if not lead.radar:
+          if lead.radarTrackId != -1:
+            self.reset()
+            return False
+          source = model.leadsV3[slot]
+          projected = get_RadarState_from_vision(source, car.vEgo, model.velocity.x[0], source.prob)
+          if (source.prob < .95 or not math.isfinite(source.prob) or
+              any(not math.isfinite(projected[field]) or abs(projected[field] - getattr(lead, field)) > .5
+                  for field in ('dRel', 'yRel', 'vLead', 'aLeadK'))):
+            self.reset()
+            return False
+      key = (drive_id, cp.carFingerprint, tuple((str(config.safetyModel), config.safetyParam) for config in cp.safetyConfigs),
+             tuple((slot, bool(lead.radar), lead.radarTrackId if lead.radar else -1) for slot, lead in leads))
+      stamp = int(sm.logMonoTime['modelV2'])
+      if (key != self.key or not self.last_ns < now_ns <= self.last_ns + MAX_AGE_NS or stamp <= self.model_ns):
+        self.reset()
+        # Unknown derived geometry cannot establish a new light-hold proof.
+        if detector.standstill_reason != 'light':
+          return False
+        self.key, self.since_ns = key, now_ns
+      self.last_ns, self.model_ns = now_ns, stamp
+      # Confirm the scene while MPC settles; its current braking/stop decision
+      # remains an unconditional release veto. No target acceleration is raised.
+      return bool(now_ns - self.since_ns >= CLEAR_DWELL_NS and
+                  detector.standstill_reason == 'light' and not planner.output_should_stop and planner.output_a_target > .1)
+    except (AttributeError, KeyError, IndexError, TypeError, ValueError, OverflowError, RuntimeError):
+      self.reset()
+      return False

openpilot/starpilot/conditional_mode/tests/test_stop_release.py

@@ -0,0 +1,167 @@
+import pytest
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.structs import CarParams
+from openpilot.starpilot.conditional_mode.stop_release import owner_for
+
+
+BRANDS = sorted({interface.__module__.split('.')[-2] for interface in interfaces.values()})
+
+
+@pytest.mark.parametrize('brand', BRANDS)
+def test_cem_departure_owner_is_shared_across_active_longitudinal_configs(brand):
+  for identity, interface in interfaces.items():
+    if interface.__module__.split('.')[-2] != brand:
+      continue
+    cp = interface.get_non_essential_params(identity)
+    cp.openpilotLongitudinalControl = True
+    assert (owner_for(cp) is not None) == (not (cp.passive or cp.dashcamOnly or cp.notCar)), identity
+
+
+@pytest.mark.parametrize('flag', ['passive', 'dashcamOnly', 'notCar'])
+def test_inactive_profiles_never_get_departure_owner(flag):
+  cp = CarParams(openpilotLongitudinalControl=True)
+  setattr(cp, flag, True)
+  assert owner_for(cp) is None
+
+
+def test_stock_longitudinal_never_gets_departure_owner():
+  assert owner_for(CarParams(openpilotLongitudinalControl=False)) is None
+
+
+from types import SimpleNamespace
+from functools import lru_cache
+from openpilot.cereal import messaging
+from openpilot.selfdrive.controls.radard import get_RadarState_from_vision
+from openpilot.starpilot.conditional_mode.stop_release import StoppedLeadRelease
+from openpilot.starpilot.longitudinal.tests import test_cem_green_release as green
+from openpilot.starpilot.tests.test_ford_recorded_departure import _motion_payloads, _samples
+
+
+@lru_cache(maxsize=1)
+def recorded_motion():
+  return next(sample for sample in _samples() if 334.44 < sample['routeSeconds'] < 334.48)
+
+
+def scene(now, vision=False):
+  motion = recorded_motion()
+  payloads = _motion_payloads(green.serialized_scene(stamp=green.BOOT + now - green.MONO - 2_000_000,
+    speed=0., standstill=True), motion)
+  for service in ('carControl', 'controlsState'):
+    event = messaging.new_message(service)
+    setattr(event, service, payloads[service])
+    payloads[service] = getattr(event, service)
+  if vision:
+    model = payloads['modelV2']
+    for slot, lead in enumerate((payloads['radarState'].leadOne, payloads['radarState'].leadTwo)):
+      if lead.present:
+        raw = model.leadsV3[slot]
+        lead.from_dict(get_RadarState_from_vision(raw, payloads['carState'].vEgo, model.velocity.x[0], raw.prob))
+  return green.CallerFrame(payloads, now - 2_000_000)
+
+
+@pytest.mark.parametrize('brand', BRANDS)
+@pytest.mark.parametrize('vision', [False, True])
+def test_shared_release_preserves_native_target_with_radar_or_correlated_vision(brand, vision):
+  cp = next(interface.get_non_essential_params(identity) for identity, interface in interfaces.items()
+            if interface.__module__.split('.')[-2] == brand)
+  cp.openpilotLongitudinalControl = True
+  owner = owner_for(cp)
+  if cp.passive or cp.dashcamOnly or cp.notCar:
+    assert owner is None
+    return
+  assert owner is not None
+  detector = SimpleNamespace(committed=True, standstill_committed=True, standstill_reason='light', commit_distance_m=3.)
+  planner = SimpleNamespace(mpc=SimpleNamespace(solution_status=0), output_should_stop=False, output_a_target=.5,
+                            force_stop_plan=SimpleNamespace(forcing=False, should_stop=False))
+  results = []
+  for tick in range(13):
+    now = green.MONO + 1_000_000_000 + tick * 50_000_000
+    results.append(owner.sample(scene(now, vision), cp, planner, detector, now_ns=now, drive_id=green.MONO-1_000_000_000))
+    assert planner.output_a_target == .5 and planner.output_should_stop is False
+  assert not any(results[:10]) and all(results[10:])
+
+
+@pytest.mark.parametrize('defect', ['mismatched-model', 'weak-model-lead', 'native-stop', 'native-braking', 'force-stop',
+                                  'second-stopped', 'model-stop', 'stale', 'sign', 'config-change', 'source-change'])
+def test_vision_release_requires_continuous_corroboration_and_native_go(defect):
+  cp = CarParams(openpilotLongitudinalControl=True, carFingerprint='KIA_NIRO_EV')
+  owner = StoppedLeadRelease()
+  detector = SimpleNamespace(committed=True, standstill_committed=True, standstill_reason='light', commit_distance_m=3.)
+  planner = SimpleNamespace(mpc=SimpleNamespace(solution_status=0), output_should_stop=False, output_a_target=.5,
+                            force_stop_plan=SimpleNamespace(forcing=False, should_stop=False))
+  results = []
+  for tick in range(13):
+    now = green.MONO + 1_000_000_000 + tick * 50_000_000
+    sm = scene(now, True)
+    if defect == 'mismatched-model':
+      sm['radarState'].leadOne.dRel += 1.
+    elif defect == 'weak-model-lead':
+      sm['modelV2'].leadsV3[0].prob = .5
+    elif defect == 'native-stop':
+      planner.output_should_stop = True
+    elif defect == 'native-braking':
+      planner.output_a_target = -.1
+    elif defect == 'force-stop':
+      planner.force_stop_plan.forcing = True
+    elif defect == 'second-stopped':
+      sm['radarState'].leadTwo.vLead = 0.
+    elif defect == 'model-stop':
+      sm['modelV2'].action.shouldStop = True
+    elif defect == 'stale':
+      sm.logMonoTime['radarState'] = now - 150_000_001
+    elif defect == 'sign':
+      detector.standstill_reason = 'sign'
+    elif defect == 'config-change':
+      cp.carFingerprint = str(tick)
+    elif defect == 'source-change' and tick % 2:
+      sm = scene(now, False)
+    results.append(owner.sample(sm, cp, planner, detector, now_ns=now, drive_id=green.MONO-1_000_000_000))
+  assert not any(results)
+
+
+@pytest.mark.parametrize('vision', [False, True])
+def test_niro_actual_cem_hold_releases_only_after_native_go(monkeypatch, vision):
+  from opendbc.car.hyundai.values import CAR
+  from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+  case = green.CemGreenCallerTests(methodName='runTest')
+  case.setUp()
+  case.cp = interfaces[CAR.KIA_NIRO_EV].get_non_essential_params(CAR.KIA_NIRO_EV)
+  case.cp.openpilotLongitudinalControl = True
+  case.cp.pcmCruise = False
+  case.planner = LongitudinalPlanner(case.cp, init_v=4.27)
+  try:
+    case.prime()
+    original = green.serialized_scene
+    samples = [sample for sample in _samples() if 334.29 <= sample['routeSeconds'] <= 335.3]
+    motion = samples[0]
+    def moving_lead(**kwargs):
+      payloads = _motion_payloads(original(**kwargs), motion)
+      if vision:
+        model = payloads['modelV2']
+        for slot, lead in enumerate((payloads['radarState'].leadOne, payloads['radarState'].leadTwo)):
+          if lead.present:
+            raw = model.leadsV3[slot]
+            lead.from_dict(get_RadarState_from_vision(raw, payloads['carState'].vEgo, model.velocity.x[0], raw.prob))
+      return payloads
+    monkeypatch.setattr(green, 'serialized_scene', moving_lead)
+    observations = []
+    original_sample = StoppedLeadRelease.sample
+    def observed(owner, sm, cp, planner, detector, **kwargs):
+      before = float(planner.output_a_target), bool(planner.output_should_stop)
+      released = original_sample(owner, sm, cp, planner, detector, **kwargs)
+      observations.append((released, before, float(planner.output_a_target), bool(planner.output_should_stop)))
+      return released
+    monkeypatch.setattr(StoppedLeadRelease, 'sample', observed)
+    results = []
+    for motion in samples:
+      _, _, held, plan = case.cycle(stopped=True, position=motion['model']['position']['x'],
+        velocity=motion['model']['velocity']['x'], should_stop=False,
+        desired_accel=motion['model']['action']['desiredAcceleration'], lead=True)
+      results.append((held, plan.shouldStop, plan.aTarget))
+    assert any(released for released, _, _, _ in observations), (observations, results)
+    assert all(before == (target, stop) for _, before, target, stop in observations)
+    assert any(not held and not stop and target > .1 for held, stop, target in results)
+    assert not case.owner.mode.projector.stop_detector.committed
+  finally:
+    case.doCleanups()

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "ea3fb57acb3d06d9e1cfdbc46137ce18dc096aad4077f95479765356422a6484", "schema_version": 1}
+{"protected_inventory_sha256": "a2fbb60c55f751914d8c6933c6228f20ae1ba345b67abd78f1f0a1c503079eb7", "schema_version": 1}