Files
StarPilot/openpilot/starpilot/conditional_mode/policy.py
T
firestar5683 d8708b65c7 CEM
Implement conditional driving modes with explicit scene, button and traffic state ownership.

Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com>
Co-authored-by: Harald Schäfer <harald.the.engineer@gmail.com>
Co-authored-by: whoisdomi <dcantu83@gmail.com>
2026-10-14 20:02:39 -05:00

565 lines
22 KiB
Python

"""Source-independent CEM/Conditional Chill mode selection.
Only a validated adapter may construct ``SceneEvidence``. This pure policy
never reads Params, IPC, or car hardware; the runtime adapter uses it after
qualifying fresh scene and vehicle authority.
"""
from __future__ import annotations
from dataclasses import dataclass
from enum import StrEnum
import math
MPH_TO_MPS = 0.44704
SCENE_MAX_AGE_S = 0.25 # Provisional adapter freshness budget; not a qualified detector threshold.
CEM_HOLD_S = 0.5
CEM_EXIT_BUFFER_S = 0.25
CEM_SLOW_LEAD_HOLD_S = 1.5
CEM_OPEN_ROAD_HANDOFF_S = 0.75
CEM_STOP_RELEASE_SUPPRESS_S = 2.0
CCM_SPEED_CONFIRM_S = 0.35
CCM_LEAD_CONFIRM_S = 1.0
CCM_LAUNCH_CONFIRM_S = 0.0
CCM_MIN_DWELL_S = 1.2
CCM_EXIT_BUFFER_S = 0.35
class ModeChoice(StrEnum):
STOCK = 'stock'
CEM = 'conditional_experimental'
CCM = 'conditional_chill'
class ManualIntent(StrEnum):
NONE = 'none'
FORCE_EXPERIMENTAL = 'force_experimental'
FORCE_CHILL = 'force_chill'
class Reason(StrEnum):
STOCK = 'stock'
UNAVAILABLE = 'unavailable'
MANUAL_EXPERIMENTAL = 'manual_experimental'
MANUAL_CHILL = 'manual_chill'
CEM_SPEED = 'cem_speed'
CEM_OPEN_ROAD = 'cem_open_road'
CEM_SIGNAL = 'cem_signal'
CEM_CURVE = 'cem_curve'
CEM_LEAD = 'cem_lead'
CEM_STOP = 'cem_stop'
CEM_SPEED_LIMIT = 'cem_speed_limit'
CEM_HOLD = 'cem_hold'
CCM_SPEED = 'ccm_speed'
CCM_LEAD = 'ccm_lead'
CCM_LAUNCH = 'ccm_launch'
CCM_HOLD = 'ccm_hold'
CCM_VETO = 'ccm_veto'
SCENE_UNAVAILABLE = 'scene_unavailable'
NO_TRIGGER = 'no_trigger'
@dataclass(frozen=True)
class Authority:
"""Fresh capability is separate from current lateral/longitudinal actuation."""
fresh: bool
system_long_capable: bool
safe_mode: bool
driving_enabled: bool
long_active: bool
lat_active: bool
@dataclass(frozen=True)
class LeadEvidence:
present: bool
tracked: bool
radar: bool
distance_m: float | None = None
speed_mps: float | None = None
accel_mps2: float | None = None
model_probability: float | None = None
@dataclass(frozen=True)
class SceneEvidence:
"""Optional means unavailable, never an implicit negative observation."""
observed_mono_s: float | None = None
speed_mps: float | None = None
set_speed_mps: float | None = None
lead: LeadEvidence | None = None
following_lead: bool | None = None
left_blinker: bool | None = None
right_blinker: bool | None = None
lane_available: bool | None = None
curve_detected: bool | None = None
slow_lead_detected: bool | None = None
stop_light_detected: bool | None = None
slc_experimental: bool | None = None
standstill: bool | None = None
traffic_mode: bool | None = None
stop_sign_confirmed: bool | None = None
forcing_stop: bool | None = None
committed_stop: bool | None = None
adjacent_lead_ambiguous: bool | None = None
low_speed_stop_scene: bool | None = None
launch_candidate: bool | None = None
standstill_stop_hold: bool | None = None
launch_forced_exit: bool | None = None
launch_lead: bool | None = None
@dataclass(frozen=True)
class ModeSettings:
"""Frozen defaults converted to SI; selection and input validation are external."""
cem_speed_mps: float = 0.0
cem_speed_with_lead_mps: float = 0.0
cem_signal_mps: float = 0.0
cem_open_road: bool = False
cem_curves: bool = False
cem_curves_with_lead: bool = False
cem_lead: bool = True
cem_stop: bool = True
ccm_speed_mps: float = 45 * MPH_TO_MPS
ccm_speed_with_lead_mps: float = 35 * MPH_TO_MPS
ccm_set_speed_margin_mps: float = 3 * MPH_TO_MPS
ccm_lead: bool = True
ccm_launch: bool = False
@dataclass(frozen=True)
class Decision:
requested_experimental: bool
effective_longitudinal: bool
qualified: bool
warm: bool
reason: Reason
status_code: int
def _finite(value: object, *, low: float = 0.0, high: float = 1000.0) -> bool:
if isinstance(value, bool) or not isinstance(value, (int, float)):
return False
try:
return math.isfinite(value) and low <= value <= high
except OverflowError:
return False
def _bool(value: object) -> bool:
return type(value) is bool
def _valid_settings(settings: ModeSettings) -> bool:
numeric = (
settings.cem_speed_mps,
settings.cem_speed_with_lead_mps,
settings.cem_signal_mps,
settings.ccm_speed_mps,
settings.ccm_speed_with_lead_mps,
settings.ccm_set_speed_margin_mps,
)
flags = (
settings.cem_open_road,
settings.cem_curves,
settings.cem_curves_with_lead,
settings.cem_lead,
settings.cem_stop,
settings.ccm_lead,
settings.ccm_launch,
)
return all(_finite(value, high=70.0) for value in numeric) and all(_bool(value) for value in flags)
def restore_manual_status(choice: ModeChoice, session_code: int, persisted_code: int, persist: bool) -> ManualIntent:
"""Pure frozen CEStatus/CCStatus normalization; no storage side effects."""
if type(persist) is not bool:
return ManualIntent.NONE
allowed = (
{1: ManualIntent.FORCE_CHILL, 2: ManualIntent.FORCE_EXPERIMENTAL}
if choice is ModeChoice.CEM
else ({1: ManualIntent.FORCE_EXPERIMENTAL, 2: ManualIntent.FORCE_CHILL} if choice is ModeChoice.CCM else {})
)
if type(session_code) is int and session_code in allowed:
return allowed[session_code]
if persist and type(persisted_code) is int:
return allowed.get(persisted_code, ManualIntent.NONE)
return ManualIntent.NONE
def next_manual_status(choice: ModeChoice, current_code: int, current_experimental: bool) -> int:
"""Frozen wheel-button cycle: manual -> automatic; automatic -> opposite manual mode."""
if type(current_experimental) is not bool:
return 0
if choice is ModeChoice.CEM:
return 0 if type(current_code) is int and current_code in (1, 2) else (1 if current_experimental else 2)
if choice is ModeChoice.CCM:
return 0 if type(current_code) is int and current_code in (1, 2) else (2 if current_experimental else 1)
return 0
def _manual_code(choice: ModeChoice, manual: ManualIntent) -> int:
if choice is ModeChoice.CEM:
return {ManualIntent.FORCE_CHILL: 1, ManualIntent.FORCE_EXPERIMENTAL: 2}.get(manual, 0)
if choice is ModeChoice.CCM:
return {ManualIntent.FORCE_EXPERIMENTAL: 1, ManualIntent.FORCE_CHILL: 2}.get(manual, 0)
return 0
def _scene_current(now_s: float, scene: SceneEvidence) -> bool:
observed = scene.observed_mono_s
speed = scene.speed_mps
return observed is not None and speed is not None and _finite(observed, high=1e12) and 0 <= now_s - observed <= SCENE_MAX_AGE_S and _finite(speed, high=80.0)
def _lead_valid(lead: LeadEvidence | None) -> bool:
return lead is not None and all(_bool(value) for value in (lead.present, lead.tracked, lead.radar))
def _stable_lead(scene: SceneEvidence) -> bool:
lead = scene.lead
speed = scene.speed_mps
if lead is None or speed is None or not _lead_valid(lead) or not lead.present or not lead.tracked or not _finite(speed, high=80.0):
return False
distance = lead.distance_m
lead_speed = lead.speed_mps
acceleration = lead.accel_mps2
probability = lead.model_probability
if distance is None or lead_speed is None or acceleration is None:
return False
if not all(_finite(value, high=500.0) for value in (distance, lead_speed)):
return False
if not _finite(acceleration, low=-20.0, high=20.0):
return False
if not lead.radar and not _finite(probability, high=1.0):
return False
confident = lead.radar or probability is not None and probability >= 0.9
max_distance = min(90.0, max(35.0, speed * 4.5))
max_closing_speed = max(1.25, 0.05 * speed)
return bool(
confident and distance < max_distance and lead_speed > 1.5 and max(0.0, -acceleration) <= 0.2 and max(0.0, speed - lead_speed) <= max_closing_speed
)
def _credible_slow_lead(scene: SceneEvidence) -> bool:
lead, speed = scene.lead, scene.speed_mps
if lead is None or speed is None or not _lead_valid(lead) or not lead.present or not _finite(speed, high=80.0):
return False
if not lead.radar and not (_finite(lead.model_probability, high=1.0) and lead.model_probability is not None and lead.model_probability >= 0.85):
return False
return bool(_finite(lead.distance_m, high=500.0) and lead.distance_m is not None and lead.distance_m < max(40.0, speed * 4.0))
def _open_road_handoff(scene: SceneEvidence) -> bool:
lead, speed = scene.lead, scene.speed_mps
if lead is None or speed is None or not _lead_valid(lead) or not lead.present or not _finite(speed, high=80.0):
return False
if lead.distance_m is None or lead.speed_mps is None or not all(_finite(value, high=500.0) for value in (lead.distance_m, lead.speed_mps)):
return False
return lead.speed_mps >= 1.0 and lead.distance_m >= max(35.0, speed * 1.5) and max(0.0, speed - lead.speed_mps) <= 3.0
class ConditionalModePolicy:
"""One stateful policy per drive/session; old scene state never crosses modes."""
def __init__(self):
self.reset()
def reset(self) -> None:
self.choice = ModeChoice.STOCK
self.last_now: float | None = None
self._reset_cem()
self.ccm_candidate: Reason | None = None
self.ccm_candidate_since = 0.0
self.ccm_chill = False
self.ccm_active_reason = Reason.NO_TRIGGER
self.ccm_launch_lead = False
self.ccm_hold_until = 0.0
self.ccm_false_since: float | None = None
def _reset_cem(self) -> None:
self.cem_hold_until = 0.0
self.cem_false_since: float | None = None
self.cem_active = False
self.cem_active_reason = Reason.NO_TRIGGER
self.cem_slow_lead_until = 0.0
self.cem_open_road_until = 0.0
self.cem_previous_open_road = False
self.cem_previous_standstill = False
self.cem_previous_stop_hold = False
self.cem_stop_release_pending = False
self.cem_launch_suppress_until = 0.0
@staticmethod
def _decision(requested: bool, authority: Authority, qualified: bool, warm: bool, reason: Reason, status: int) -> Decision:
return Decision(requested, bool(requested and qualified and authority.driving_enabled and authority.long_active), qualified, warm, reason, status)
def step(
self,
now_s: float,
choice: ModeChoice,
manual: ManualIntent,
authority: Authority,
scene: SceneEvidence,
settings: ModeSettings | None = None,
*,
stock_experimental: bool = False,
) -> Decision:
if settings is None:
settings = ModeSettings()
if not isinstance(choice, ModeChoice):
self.reset()
return self._decision(False, authority, False, False, Reason.UNAVAILABLE, 0)
if not _finite(now_s, high=1e12) or (self.last_now is not None and now_s < self.last_now):
self.reset()
return self._decision(False, authority, False, False, Reason.UNAVAILABLE, 0)
if choice is not self.choice or (self.last_now is not None and now_s - self.last_now > SCENE_MAX_AGE_S):
self.reset()
self.choice = choice
self.last_now = now_s
authority_valid = all(
_bool(value)
for value in (authority.fresh, authority.system_long_capable, authority.safe_mode, authority.driving_enabled, authority.long_active, authority.lat_active)
)
qualified = bool(authority_valid and authority.fresh and authority.system_long_capable and not authority.safe_mode)
warm = bool(authority_valid and authority.fresh and (authority.driving_enabled or authority.lat_active))
if choice is ModeChoice.STOCK and scene.committed_stop is not True:
return self._decision(bool(stock_experimental) if type(stock_experimental) is bool else False, authority, qualified, warm, Reason.STOCK, 0)
if choice not in (ModeChoice.CEM, ModeChoice.CCM, ModeChoice.STOCK) or not qualified or not _valid_settings(settings):
self.reset()
return self._decision(False, authority, False, False, Reason.UNAVAILABLE, 0)
if (warm and authority.driving_enabled and authority.long_active and
_scene_current(now_s, scene) and scene.committed_stop is True):
return self._decision(True, authority, True, warm, Reason.CEM_STOP, 8)
if not isinstance(manual, ManualIntent):
manual = ManualIntent.NONE
if manual is not ManualIntent.NONE:
# Manual intent clears confirmation/dwell but preserves the observed stop-release quiet period.
suppress_until = self.cem_launch_suppress_until if choice is ModeChoice.CEM else 0.0
current_scene = _scene_current(now_s, scene)
if (
choice is ModeChoice.CEM
and current_scene
and scene.standstill is False
and (self.cem_previous_standstill and self.cem_previous_stop_hold or self.cem_stop_release_pending)
):
suppress_until = now_s + CEM_STOP_RELEASE_SUPPRESS_S
self.reset()
self.choice = choice
self.last_now = now_s
if choice is ModeChoice.CEM:
self.cem_launch_suppress_until = suppress_until
self.cem_previous_standstill = current_scene and scene.standstill is True
requested = manual is ManualIntent.FORCE_EXPERIMENTAL
return self._decision(
requested, authority, qualified, warm, Reason.MANUAL_EXPERIMENTAL if requested else Reason.MANUAL_CHILL, _manual_code(choice, manual)
)
if not warm or not _scene_current(now_s, scene):
self._reset_cem()
self.ccm_chill = False
self.ccm_candidate = None
self.ccm_false_since = None
return self._decision(choice is ModeChoice.CCM, authority, qualified, warm, Reason.SCENE_UNAVAILABLE, 0)
return self._cem(now_s, authority, warm, scene, settings) if choice is ModeChoice.CEM else self._ccm(now_s, authority, warm, scene, settings)
def _cem_trigger(self, scene: SceneEvidence, settings: ModeSettings, *, launch_suppressed: bool = False) -> Reason | None:
speed = scene.speed_mps
if speed is None:
return None
lead = scene.lead
following = scene.following_lead
if not launch_suppressed and _bool(following) and speed >= 1.0:
limit = settings.cem_speed_with_lead_mps if following else settings.cem_speed_mps
if limit > speed:
return Reason.CEM_SPEED
set_speed = scene.set_speed_mps
if (
not launch_suppressed
and settings.cem_open_road
and lead is not None
and _lead_valid(lead)
and _bool(following)
and not (lead.present or lead.tracked or following)
and set_speed is not None
and _finite(set_speed, high=80.0)
):
if 0 < set_speed <= speed <= set_speed + MPH_TO_MPS:
return Reason.CEM_OPEN_ROAD
if (
_bool(scene.left_blinker)
and _bool(scene.right_blinker)
and _bool(scene.lane_available)
and settings.cem_signal_mps > speed
and (scene.left_blinker or scene.right_blinker)
and not scene.lane_available
):
return Reason.CEM_SIGNAL
if settings.cem_curves and scene.curve_detected is True and _bool(following) and (settings.cem_curves_with_lead or not following):
return Reason.CEM_CURVE
if not launch_suppressed and settings.cem_lead and scene.slow_lead_detected is True and speed <= 35.31:
return Reason.CEM_LEAD
if settings.cem_stop and (scene.stop_light_detected is True or scene.forcing_stop is True):
return Reason.CEM_STOP
if scene.slc_experimental is True:
return Reason.CEM_SPEED_LIMIT
return None
def _cem(self, now_s: float, authority: Authority, warm: bool, scene: SceneEvidence, settings: ModeSettings) -> Decision:
committed = settings.cem_stop and scene.forcing_stop is True and authority.driving_enabled and authority.long_active
if not _bool(scene.standstill) or (scene.standstill and not committed and not _bool(scene.standstill_stop_hold)):
self._reset_cem()
return self._decision(False, authority, True, warm, Reason.SCENE_UNAVAILABLE, 0)
released = not scene.standstill and (self.cem_previous_standstill and self.cem_previous_stop_hold or self.cem_stop_release_pending)
if released:
self.cem_launch_suppress_until = now_s + CEM_STOP_RELEASE_SUPPRESS_S
self.cem_hold_until = self.cem_slow_lead_until = 0.0
self.cem_false_since = None
self.cem_active = False
self.cem_stop_release_pending = False
if scene.standstill:
held = committed or scene.standstill_stop_hold is True
self.cem_hold_until = self.cem_slow_lead_until = self.cem_open_road_until = 0.0
self.cem_false_since = None
self.cem_previous_open_road = False
if held:
self.cem_stop_release_pending = False
elif self.cem_previous_stop_hold:
self.cem_stop_release_pending = True
if self.cem_stop_release_pending:
self.cem_launch_suppress_until = now_s + CEM_STOP_RELEASE_SUPPRESS_S
self.cem_active = held
self.cem_previous_standstill = True
self.cem_previous_stop_hold = held
self.cem_active_reason = Reason.CEM_STOP if held else Reason.NO_TRIGGER
return self._decision(held, authority, True, warm, self.cem_active_reason, 8 if held else 0)
self.cem_previous_standstill = self.cem_previous_stop_hold = False
trigger = self._cem_trigger(scene, settings, launch_suppressed=now_s < self.cem_launch_suppress_until)
if trigger is not None:
self.cem_hold_until = now_s + CEM_HOLD_S
self.cem_false_since = None
self.cem_open_road_until = 0.0
self.cem_slow_lead_until = now_s + CEM_SLOW_LEAD_HOLD_S if trigger is Reason.CEM_LEAD else 0.0
self.cem_active_reason = trigger
else:
if self.cem_active and self.cem_false_since is None:
self.cem_false_since = now_s
elif not self.cem_active:
self.cem_false_since = None
if not settings.cem_open_road:
self.cem_open_road_until = 0.0
elif self.cem_previous_open_road and _open_road_handoff(scene):
self.cem_open_road_until = now_s + CEM_OPEN_ROAD_HANDOFF_S
open_road_hold = trigger is None and now_s < self.cem_open_road_until and _open_road_handoff(scene)
slow_lead_hold = settings.cem_lead and now_s < self.cem_slow_lead_until and _credible_slow_lead(scene)
if not slow_lead_hold:
self.cem_slow_lead_until = 0.0
if open_road_hold:
self.cem_active_reason = Reason.CEM_OPEN_ROAD
if slow_lead_hold and trigger is None:
self.cem_active_reason = Reason.CEM_LEAD
self.cem_active = bool(
trigger is not None
or slow_lead_hold
or open_road_hold
or now_s < self.cem_hold_until
or self.cem_false_since is not None
and now_s - self.cem_false_since < CEM_EXIT_BUFFER_S
)
self.cem_previous_open_road = trigger is Reason.CEM_OPEN_ROAD
reason = trigger if trigger is not None else Reason.CEM_HOLD if self.cem_active else Reason.NO_TRIGGER
status_reason = self.cem_active_reason if self.cem_active else Reason.NO_TRIGGER
status = {
Reason.CEM_CURVE: 3,
Reason.CEM_LEAD: 4,
Reason.CEM_SIGNAL: 5,
Reason.CEM_SPEED: 6,
Reason.CEM_OPEN_ROAD: 6,
Reason.CEM_SPEED_LIMIT: 7,
Reason.CEM_STOP: 8,
}.get(status_reason, 0)
return self._decision(self.cem_active, authority, True, warm, reason, status)
def _ccm(self, now_s: float, authority: Authority, warm: bool, scene: SceneEvidence, settings: ModeSettings) -> Decision:
speed = scene.speed_mps
if speed is None:
return self._decision(True, authority, True, warm, Reason.SCENE_UNAVAILABLE, 0)
veto_fields = (
scene.standstill,
scene.left_blinker,
scene.right_blinker,
scene.traffic_mode,
scene.slc_experimental,
scene.curve_detected,
scene.slow_lead_detected,
scene.stop_light_detected,
scene.stop_sign_confirmed,
scene.forcing_stop,
scene.adjacent_lead_ambiguous,
scene.low_speed_stop_scene,
)
launch = settings.ccm_launch and scene.launch_candidate is True
hard_veto = veto_fields[1:-1]
if any(value is True for value in hard_veto) or (scene.standstill is True and not launch) or (scene.low_speed_stop_scene is True and not launch):
self.ccm_chill = False
self.ccm_candidate = None
self.ccm_false_since = None
return self._decision(True, authority, True, warm, Reason.CCM_VETO, 0)
if any(not _bool(value) for value in veto_fields):
self.ccm_chill = False
self.ccm_candidate = None
self.ccm_false_since = None
return self._decision(True, authority, True, warm, Reason.SCENE_UNAVAILABLE, 0)
if settings.ccm_launch and (not _bool(scene.launch_candidate) or not _bool(scene.launch_forced_exit) or launch and not _bool(scene.launch_lead)):
self.ccm_chill = False
self.ccm_candidate = None
self.ccm_false_since = None
return self._decision(True, authority, True, warm, Reason.SCENE_UNAVAILABLE, 0)
lead = scene.lead
candidate = None
if launch:
candidate = Reason.CCM_LAUNCH
elif (
lead is not None
and _lead_valid(lead)
and not lead.present
and not lead.tracked
and scene.set_speed_mps is not None
and _finite(scene.set_speed_mps, high=80.0)
and speed >= settings.ccm_speed_mps
and scene.set_speed_mps - speed >= settings.ccm_set_speed_margin_mps
):
candidate = Reason.CCM_SPEED
elif settings.ccm_lead and speed >= settings.ccm_speed_with_lead_mps and _stable_lead(scene):
candidate = Reason.CCM_LEAD
if candidate is not None:
if candidate is not self.ccm_candidate:
self.ccm_candidate = candidate
self.ccm_candidate_since = now_s
self.ccm_false_since = None
confirm = {Reason.CCM_SPEED: CCM_SPEED_CONFIRM_S, Reason.CCM_LEAD: CCM_LEAD_CONFIRM_S, Reason.CCM_LAUNCH: CCM_LAUNCH_CONFIRM_S}[candidate]
if self.ccm_chill or now_s - self.ccm_candidate_since >= confirm:
self.ccm_chill = True
self.ccm_active_reason = candidate
self.ccm_launch_lead = candidate is Reason.CCM_LAUNCH and scene.launch_lead is True
self.ccm_hold_until = max(self.ccm_hold_until, now_s + CCM_MIN_DWELL_S)
else:
self.ccm_candidate = None
if settings.ccm_launch and scene.launch_forced_exit is True:
self.ccm_chill = False
self.ccm_active_reason = Reason.NO_TRIGGER
self.ccm_false_since = None
self.ccm_hold_until = 0.0
if self.ccm_chill and self.ccm_false_since is None:
self.ccm_false_since = now_s
if self.ccm_chill and not (now_s < self.ccm_hold_until or self.ccm_false_since is not None and now_s - self.ccm_false_since < CCM_EXIT_BUFFER_S):
self.ccm_chill = False
self.ccm_active_reason = Reason.NO_TRIGGER
reason = self.ccm_active_reason if self.ccm_chill and candidate else Reason.CCM_HOLD if self.ccm_chill else Reason.NO_TRIGGER
lead_status = self.ccm_active_reason is Reason.CCM_LEAD or self.ccm_active_reason is Reason.CCM_LAUNCH and self.ccm_launch_lead
status = 4 if self.ccm_chill and lead_status else 6 if self.ccm_chill else 0
return self._decision(not self.ccm_chill, authority, True, warm, reason, status)