"""Current-drive Force Stop owner with bounded preference reads and live scene inputs.""" from dataclasses import replace from openpilot.starpilot.conditional_mode.curve import frozen_raw_curve from openpilot.starpilot.conditional_mode.preferences import CEMOptions, decode_preferences, PreferenceError from openpilot.starpilot.conditional_mode.projection import _horizon from openpilot.starpilot.conditional_mode.stop import ( FROZEN_RAW_STOP_DISTANCE_M, STOP_MODEL_RELEASE_MARGIN_M, StopFrame as LightFrame, StopLead, StopLightDetector, ) from openpilot.starpilot.lead_tracking import LeadDetector from openpilot.starpilot.longitudinal.force_stop import ForceStop, StopFrame, StopPlan, vehicle_tune from openpilot.starpilot.model_geometry import road_curvature, standstill_clear_horizon, standstill_release_speed from openpilot.starpilot.saved_source import read_saved from openpilot.starpilot.longitudinal.stop_resume import StopResume KEY = 'ForceStops' OFFSET_KEY = 'ForceStopDistanceOffset' SOURCE_AGE_NS = 150_000_000 def standstill_takeoff_light(model, car) -> bool | None: """Fresh takeoff metadata only; Force Stop keeps its strict spatial policy.""" if not standstill_release_speed(car.standstill, car.vEgo, car.vEgoRaw): return None should_stop = model.action.shouldStop if type(should_stop) is not bool: return None horizon = standstill_clear_horizon(model) if horizon is None: return None return should_stop or horizon < FROZEN_RAW_STOP_DISTANCE_M + STOP_MODEL_RELEASE_MARGIN_M class ForceStopRuntime: def __init__(self, params): self.params = params self.policy = ForceStop() self.resume = StopResume() self.light = StopLightDetector() self.lead = LeadDetector() self.plan = StopPlan() self.drive_id = 0 self.identity = None self.sources = None self.next_refresh_ns = 0 self.enabled = False self.takeoff_preferences_valid = False self.offset = 0 self.options = CEMOptions() self.clock_offset = None def reset(self): self.resume.reset() self.policy.reset() self.light.reset() self.lead.reset() self.plan = StopPlan() def _preferences(self, now_ns: int): if now_ns < self.next_refresh_ns: return self.next_refresh_ns = now_ns + 1_000_000_000 keys = (KEY, OFFSET_KEY, 'SafeMode', 'ConditionalModeConfig') try: values = tuple(read_saved(self.params, key, 4096 if key == 'ConditionalModeConfig' else 16) for key in keys) if self.sources is not None and values[:3] != self.sources[:3]: self.reset() self.sources = values if not all(readable for _, readable in values): raise ValueError('unreadable Force Stop settings') master, offset, safe, conditional = (raw for raw, _ in values) if master not in (None, b'0', b'1') or safe not in (None, b'0', b'1'): raise ValueError('invalid Force Stop switch') self.offset = int(offset) if offset is not None else 0 if not -20 <= self.offset <= 20: raise ValueError('invalid stop offset') self.options = decode_preferences(conditional).cem if conditional is not None else CEMOptions() self.enabled = master == b'1' and safe in (None, b'0') self.takeoff_preferences_valid = True except (OSError, RuntimeError, TypeError, ValueError, PreferenceError): self.enabled = False self.takeoff_preferences_valid = False self.reset() def sample(self, sm, cp, *, now_ns: int, now_boot_ns: int, drive_id: int, follow_seconds: float | None, traffic_mode: bool | None, takeoff_enabled: bool = False) -> StopPlan: self._preferences(now_ns) try: identity = (str(cp.carFingerprint), bool(cp.openpilotLongitudinalControl), bool(cp.passive), bool(cp.dashcamOnly), bool(cp.notCar)) if identity != self.identity or drive_id != self.drive_id: self.reset() self.identity, self.drive_id = identity, drive_id offset = now_boot_ns - now_ns if self.clock_offset is None or abs(offset - self.clock_offset) > 1_000_000: self.clock_offset = offset self.reset() return self.plan services = ('modelV2', 'carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState') current = all(sm.valid[name] and sm.alive[name] and drive_id < sm.logMonoTime[name] <= now_ns and 0 <= now_ns - sm.logMonoTime[name] <= SOURCE_AGE_NS and drive_id < int(sm.recv_time[name] * 1e9) <= now_ns and 0 <= now_ns - int(sm.recv_time[name] * 1e9) <= SOURCE_AGE_NS for name in services) car, control, state, model = (sm[name] for name in ('carState', 'carControl', 'selfdriveState', 'modelV2')) if ((not self.enabled and not takeoff_enabled) or (takeoff_enabled and not self.takeoff_preferences_valid) or not identity[1] or any(identity[2:]) or drive_id <= 0 or not current or not car.canValid or car.canTimeout or not (control.longActive or control.latActive) or sm['controlsState'].forceDecel or (traffic_mode is not False and not self.policy.forcing) or not 0 < model.timestampEof <= now_boot_ns or now_boot_ns - model.timestampEof > SOURCE_AGE_NS): self.reset() return self.plan stamp = int(sm.logMonoTime['modelV2']) horizon = _horizon(model) curvature = road_curvature(model, car.vEgo) raw_curve = frozen_raw_curve(model, car.vEgo, sm['controlsState'].curvature, car.leftBlinker, car.rightBlinker) lead = sm['radarState'].leadOne perception_available = horizon is not None and curvature is not None and raw_curve is not None tracked = None light_context = False if perception_available: tracked = self.lead.step(lead, model, speed_mps=float(car.vEgo), t_follow_s=follow_seconds, standstill=bool(car.standstill), observed_mono_s=stamp / 1e9, now_mono_s=now_ns / 1e9, radar_fresh=True, model_fresh=True, car_fresh=True, headway_fresh=follow_seconds is not None) perception_available = tracked.tracked is not None if perception_available: assert raw_curve is not None and tracked is not None stop_lead = StopLead(bool(lead.present), float(lead.dRel), float(lead.vLead), bool(lead.radar), float(lead.modelProb), tracked.tracked) observation = self.light.step(LightFrame( observed_mono_s=min(int(sm.logMonoTime[name]) for name in services) / 1e9, now_mono_s=now_ns / 1e9, speed_mps=float(car.vEgo), model_horizon_m=horizon, model_stop_time_s=self.options.model_stop_s if self.options.stop_lights or takeoff_enabled else 0.0, traffic_mode=False, stop_sign_confirmed=False, forcing_stop=self.policy.forcing, lead=stop_lead, standstill=bool(car.standstill), left_blinker=bool(car.leftBlinker), right_blinker=bool(car.rightBlinker), steering_angle_deg=float(car.steeringAngleDeg), driving_in_curve=raw_curve[1], car_fingerprint=str(cp.carFingerprint), dashboard_stop_sign=False, pedal_override=bool(car.gasPressed), model_tick_mono_s=stamp / 1e9, model_should_stop=bool(model.action.shouldStop))) perception_available = observation.light_detected is not None light_context = observation.light_detected is True or (observation.standstill_hold is True and observation.standstill_reason == 'light') else: self.light.reset() self.lead.reset() takeoff_observed, takeoff_light = perception_available, bool(light_context) if takeoff_enabled and not perception_available: fallback = standstill_takeoff_light(model, car) if fallback is not None: takeoff_observed, takeoff_light = True, fallback if takeoff_enabled and not self.enabled: # Perception may veto takeoff while Force Stop stays entirely inactive. self.resume.reset() self.policy.reset() self.plan = StopPlan(model_ns=stamp, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light) return self.plan # Model trajectories can oscillate around zero velocity at a predicted # stop. Invalid geometry cannot acquire a stop or prove that it is clear; # an existing commitment continues from fresh car odometry below. resume_requested = self.resume.consume(now_ns=now_ns, drive_id=drive_id, car_ns=int(sm.logMonoTime['carState'])) # Keep model perception warm during AOL and a manual brake stop. Only # actual longitudinal ownership may produce a stop target or hold. if not control.longActive or not state.enabled or car.brakePressed: self.resume.reset() self.policy.reset() self.plan = StopPlan() return self.plan frame = StopFrame(stamp, float(car.vEgo), horizon, light_context, horizon is not None and horizon < 50, bool(model.action.shouldStop), bool(car.standstill), True, bool(car.gasPressed), tracked is not None and tracked.tracked is True, bool(lead.present), float(lead.dRel), float(lead.vLead), raw_curve[1] if raw_curve is not None else False, curvature[0] if curvature is not None else None, bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested, perception_available=perception_available) self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset) if takeoff_enabled: self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light) return self.plan except (AttributeError, KeyError, TypeError, ValueError, OverflowError, RuntimeError): self.reset() return self.plan