diff --git a/openpilot/selfdrive/controls/plannerd.py b/openpilot/selfdrive/controls/plannerd.py index c7f929ea96..7678351dea 100755 --- a/openpilot/selfdrive/controls/plannerd.py +++ b/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 diff --git a/openpilot/starpilot/car/ford/stop_release.py b/openpilot/starpilot/car/ford/stop_release.py index 03f8dac74e..c4a31c7137 100644 --- a/openpilot/starpilot/car/ford/stop_release.py +++ b/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 diff --git a/openpilot/starpilot/conditional_mode/stop_release.py b/openpilot/starpilot/conditional_mode/stop_release.py new file mode 100644 index 0000000000..6480144815 --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/conditional_mode/tests/test_stop_release.py b/openpilot/starpilot/conditional_mode/tests/test_stop_release.py new file mode 100644 index 0000000000..3df141ee73 --- /dev/null +++ b/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() diff --git a/prebuilt.json b/prebuilt.json index 9510dedfaa..b31d75020f 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "ea3fb57acb3d06d9e1cfdbc46137ce18dc096aad4077f95479765356422a6484", "schema_version": 1} +{"protected_inventory_sha256": "a2fbb60c55f751914d8c6933c6228f20ae1ba345b67abd78f1f0a1c503079eb7", "schema_version": 1}