Make DEC reliably complete model-predicted stops

selfdrive/controls/lib/longitudinal_planner.py

@@ -135,6 +135,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     # DEC is the sole ACC/e2e authority. Cache its decision once for both the governor and output arbitration.
     is_e2e = self.is_e2e(sm)
+    stop_constraint = self.dec.stop_constraint()
     v_cruise = LongitudinalPlannerSP.update_accel_controller(
       self, sm, v_cruise, engaged=not reset_state, cruise_initialized=v_cruise_initialized, acc_selected=not is_e2e,
       planner_speed=self.v_desired_filter.x, previous_mpc_source=self.mpc.source, previous_should_stop=self.output_should_stop,
@@ -173,7 +174,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
     if is_e2e:
-      output_a_target = min(output_a_target_e2e, output_a_target_mpc)
+      output_a_target = min(output_a_target_e2e, output_a_target_mpc, stop_constraint.accel_ceiling_mps2)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc:
         self.mpc.source = LongitudinalPlanSource.e2e
@@ -181,6 +182,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
 
+    self.output_should_stop |= stop_constraint.hold
+
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
     self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])

selfdrive/ui/sunnypilot/layouts/settings/cruise.py

@@ -84,7 +84,7 @@ class CruiseLayout(Widget):
 
     self.dec_toggle = toggle_item_sp(
       title=tr("Enable Dynamic Experimental Control"),
-      description=tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
+      description=tr("Let the model choose between sunnypilot ACC and End to End Longitudinal. Stable stop trajectories can brake to a full stop and hold."),
       param="DynamicExperimentalControl")
 
     items = [

sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -13,6 +13,7 @@ from numpy import interp
 from opendbc.car import structs
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.stop_intent import StopConstraint, StopIntentController, StopIntentState
 
 ModeType = Literal['acc', 'blended']
 
@@ -69,7 +70,7 @@ class ModeTransitionManager:
 
   def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
     if immediate:
-      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames)
+      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) if mode == 'blended' else 0
       self._pending_mode = mode
       self._pending_count = 0
       self._switch_mode(mode)
@@ -128,6 +129,8 @@ class DynamicExperimentalController:
     self._urgency = 0.0
 
     self._mode_manager = ModeTransitionManager()
+    self._stop_intent = StopIntentController()
+    self._stop_constraint = StopConstraint()
 
     self._lead_tracker = HysteresisSignal(
       enter_threshold=WMACConstants.LEAD_PROB,
@@ -183,6 +186,9 @@ class DynamicExperimentalController:
   def active(self) -> bool:
     return self._active
 
+  def stop_constraint(self) -> StopConstraint:
+    return self._stop_constraint
+
   def set_mpc_fcw_crash_cnt(self) -> None:
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
 
@@ -270,6 +276,12 @@ class DynamicExperimentalController:
     return urgency
 
   def _desired_mode(self) -> tuple[ModeType, bool]:
+    if self._stop_constraint.state == StopIntentState.suppressed:
+      return 'acc', True
+
+    if self._stop_constraint.active:
+      return 'blended', True
+
     if not self._CP.radarUnavailable and self._has_radar_acc_lead:
       return 'acc', False
 
@@ -289,15 +301,34 @@ class DynamicExperimentalController:
 
     return 'acc', False
 
+  @staticmethod
+  def _model_observation_valid(sm: messaging.SubMaster) -> bool:
+    validity = getattr(sm, 'valid', None)
+    updated = getattr(sm, 'updated', None)
+    try:
+      return (validity is None or bool(validity['modelV2'])) and (updated is None or bool(updated['modelV2']))
+    except (KeyError, TypeError):
+      return False
+
   def update(self, sm: messaging.SubMaster) -> None:
     self._read_params()
+    self._active = sm['selfdriveState'].experimentalMode and self._enabled
     self.set_mpc_fcw_crash_cnt()
     self._update_calculations(sm)
 
+    gas_pressed = bool(sm['carState'].gasPressed)
+    stop_enabled = self._active and self._CP.openpilotLongitudinalControl and (bool(sm['carControl'].longActive) or gas_pressed)
+    self._stop_constraint = self._stop_intent.update(
+      sm['modelV2'],
+      sm['carState'],
+      enabled=stop_enabled,
+      lead_present=bool(sm['radarState'].leadOne.status),
+      observation_valid=self._model_observation_valid(sm),
+    )
+
     mode, immediate = self._desired_mode()
     self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
                                     cancel_hold=self._has_radar_acc_lead)
     self._mode_manager.update()
 
-    self._active = sm['selfdriveState'].experimentalMode and self._enabled
     self._frame += 1

sunnypilot/selfdrive/controls/lib/dec/stop_intent.py

@@ -0,0 +1,268 @@
+"""A compact fixed-world stop tracker for Dynamic Experimental Control."""
+
+from dataclasses import dataclass
+from enum import IntEnum
+import math
+
+from openpilot.common.realtime import DT_MDL
+
+
+class StopIntentState(IntEnum):
+  clear = 0
+  qualifying = 1
+  approach = 2
+  hold = 3
+  suppressed = 4
+
+
+@dataclass(frozen=True)
+class StopConstraint:
+  active: bool = False
+  distance_m: float = 0.0
+  hold: bool = False
+  state: StopIntentState = StopIntentState.clear
+  confidence: float = 0.0
+  accel_ceiling_mps2: float = math.inf
+
+
+@dataclass(frozen=True)
+class _Observation:
+  distance_m: float
+  evidence: bool
+
+
+class StopIntentController:
+  TRAJECTORY_SIZE = 33
+  QUALIFY_TIME = 0.75
+  QUALIFY_TRAVEL_MARGIN = 1.0
+  APPROACH_CLEAR_TIME = 0.5
+  HOLD_MIN_TIME = 1.0
+  HOLD_CLEAR_TIME = 0.5
+
+  MAX_OBSERVATION_DISTANCE = 170.0
+  MAX_APPROACH_SPEED = 20.0
+  MAX_REQUIRED_DECEL = 2.2
+  ACTUATION_LOOKAHEAD = 0.25
+  ENDPOINT_SETTLE_MARGIN = 2.5
+  HOLD_DISTANCE = 3.0
+  HOLD_SPEED = 0.7
+
+  ACTION_DECEL_MIN = 0.2
+  ACTION_DECEL_RATIO = 0.4
+  FREE_FLOW_HORIZON = 7.0
+  SHORT_TRAJECTORY_RATIO = 0.95
+  TERMINAL_SPEED_MAX = 0.75
+  TERMINAL_WINDOW_SIZE = 4
+  TERMINAL_WINDOW_SPEED_MAX = 2.0
+
+  WORLD_ERROR_MIN = 2.0
+  WORLD_ERROR_MAX = 3.0
+  WORLD_SPEED_MIN_TIME = 0.4
+  WORLD_SPEED_MAX = 0.75
+  ASSOCIATION_ERROR_MIN = 2.0
+
+  def __init__(self):
+    self.reset()
+
+  def reset(self) -> None:
+    self._state = StopIntentState.clear
+    self._distance_m = 0.0
+    self._start_distance = 0.0
+    self._travel_m = 0.0
+    self._elapsed = 0.0
+    self._evidence_time = 0.0
+    self._clear_time = 0.0
+    self._hold_time = 0.0
+    self._last_v_ego = 0.0
+
+  def update(self, model, car_state, *, enabled: bool, lead_present: bool, observation_valid: bool = True, dt: float = DT_MDL) -> StopConstraint:
+    dt = self._sanitize_dt(dt)
+    if not enabled:
+      self.reset()
+      return self._constraint()
+
+    v_ego = self._read_speed(car_state)
+    self._last_v_ego = v_ego
+    standstill = bool(getattr(car_state, 'standstill', False))
+    observation = self._read_observation(model, v_ego) if observation_valid else None
+    observation_step = min(dt, DT_MDL)
+
+    if bool(getattr(car_state, 'gasPressed', False)):
+      self.reset()
+      self._state = StopIntentState.suppressed
+      self._distance_m = observation.distance_m if observation is not None else 0.0
+      return self._constraint()
+    if self._state == StopIntentState.suppressed:
+      self.reset()
+
+    if self._state == StopIntentState.clear:
+      if lead_present or observation is None or not observation.evidence:
+        return self._constraint()
+      reserve = self._qualification_reserve(observation.distance_m, v_ego)
+      if not self._feasible(observation.distance_m, v_ego, reserve):
+        return self._constraint()
+      self._start_candidate(observation.distance_m, observation_step)
+      return self._constraint()
+
+    step_distance = v_ego * dt
+    if self._state == StopIntentState.qualifying:
+      self._distance_m -= step_distance
+      self._travel_m += step_distance
+      self._elapsed += dt
+      reserve = self._qualification_reserve(
+        self._start_distance,
+        v_ego,
+        evidence_time=self._evidence_time,
+        travel_m=self._travel_m,
+      )
+      if lead_present or observation is None or not observation.evidence or not self._feasible(self._distance_m, v_ego, reserve):
+        self.reset()
+        return self._constraint()
+
+      expected_distance = self._start_distance - self._travel_m
+      world_displacement = observation.distance_m - expected_distance
+      moving_target = self._elapsed >= self.WORLD_SPEED_MIN_TIME and abs(world_displacement) / self._elapsed > self.WORLD_SPEED_MAX
+      if abs(world_displacement) > self._world_tolerance(self._start_distance) or moving_target:
+        self._start_candidate(observation.distance_m, observation_step)
+        return self._constraint()
+
+      self._fuse(observation.distance_m)
+      self._evidence_time = min(self.QUALIFY_TIME, self._evidence_time + observation_step)
+      self._commit_if_ready(v_ego, standstill)
+      return self._constraint()
+
+    self._distance_m -= step_distance
+    if self._state == StopIntentState.hold:
+      self._hold_time += dt
+      self._clear_time = self._clear_time + observation_step if observation is not None and not observation.evidence else 0.0
+      if self._hold_time >= self.HOLD_MIN_TIME - 1e-6 and self._clear_time >= self.HOLD_CLEAR_TIME - 1e-6:
+        self.reset()
+      return self._constraint()
+
+    if observation is None:
+      self._clear_time = 0.0
+    elif observation.evidence:
+      self._clear_time = 0.0
+      if observation.distance_m < self._distance_m:
+        self._distance_m = observation.distance_m
+      elif observation.distance_m - self._distance_m <= self.ASSOCIATION_ERROR_MIN:
+        self._fuse(observation.distance_m)
+    else:
+      self._clear_time += observation_step
+      if self._clear_time >= self.APPROACH_CLEAR_TIME - 1e-6:
+        self.reset()
+        return self._constraint()
+
+    if standstill or (self._distance_m <= self.HOLD_DISTANCE and v_ego <= self.HOLD_SPEED):
+      self._state = StopIntentState.hold
+      self._hold_time = 0.0
+      self._clear_time = 0.0
+    return self._constraint()
+
+  def _start_candidate(self, distance_m: float, evidence_step: float) -> None:
+    self._state = StopIntentState.qualifying
+    self._distance_m = distance_m
+    self._start_distance = distance_m
+    self._travel_m = 0.0
+    self._elapsed = 0.0
+    self._evidence_time = evidence_step
+
+  def _commit_if_ready(self, v_ego: float, standstill: bool) -> None:
+    if self._evidence_time < self.QUALIFY_TIME:
+      return
+    if self._distance_m <= self.HOLD_DISTANCE and (standstill or v_ego <= self.HOLD_SPEED):
+      self._state = StopIntentState.hold
+      self._hold_time = 0.0
+      self._clear_time = 0.0
+    elif self._travel_m >= self._world_tolerance(self._start_distance) + self.QUALIFY_TRAVEL_MARGIN:
+      self._state = StopIntentState.approach
+
+  def _qualification_reserve(self, start_distance: float, v_ego: float, *, evidence_time: float = 0.0, travel_m: float = 0.0) -> float:
+    time_reserve = v_ego * max(0.0, self.QUALIFY_TIME - evidence_time)
+    travel_reserve = max(0.0, self._world_tolerance(start_distance) + self.QUALIFY_TRAVEL_MARGIN - travel_m)
+    return max(time_reserve, travel_reserve)
+
+  def _feasible(self, distance_m: float, v_ego: float, reserve_m: float = 0.0) -> bool:
+    if v_ego > self.MAX_APPROACH_SPEED or not 0.1 <= distance_m <= self.MAX_OBSERVATION_DISTANCE:
+      return False
+    if distance_m <= self.HOLD_DISTANCE and v_ego <= self.HOLD_SPEED:
+      return True
+    usable_distance = self._control_distance(distance_m, v_ego) - max(0.0, reserve_m)
+    return usable_distance > 0.0 and v_ego * v_ego / (2.0 * usable_distance) <= self.MAX_REQUIRED_DECEL
+
+  def _read_observation(self, model, v_ego: float) -> _Observation | None:
+    position = getattr(model, 'position', None)
+    xs, ys = getattr(position, 'x', ()), getattr(position, 'y', ())
+    velocity = getattr(model, 'velocity', None)
+    vxs, vys = getattr(velocity, 'x', ()), getattr(velocity, 'y', ())
+    if not all(len(values) == self.TRAJECTORY_SIZE for values in (xs, ys, vxs, vys)):
+      return None
+
+    try:
+      points = [(float(x), float(y)) for x, y in zip(xs, ys, strict=True)]
+      terminal_speeds = [math.hypot(float(vx), float(vy)) for vx, vy in zip(vxs[-self.TERMINAL_WINDOW_SIZE :], vys[-self.TERMINAL_WINDOW_SIZE :], strict=True)]
+    except (TypeError, ValueError):
+      return None
+    if not all(math.isfinite(value) for point in points for value in point) or not all(math.isfinite(speed) for speed in terminal_speeds):
+      return None
+    if points[-1][0] <= 0.0:
+      return None
+
+    distance_m = math.hypot(*points[0]) + sum(math.hypot(x1 - x0, y1 - y0) for (x0, y0), (x1, y1) in zip(points, points[1:], strict=False))
+    if not 0.1 <= distance_m <= self.MAX_OBSERVATION_DISTANCE:
+      return None
+
+    terminal_profile = (
+      terminal_speeds[-1] <= self.TERMINAL_SPEED_MAX
+      and max(terminal_speeds) <= self.TERMINAL_WINDOW_SPEED_MAX
+      and all(next_speed <= speed + 0.25 for speed, next_speed in zip(terminal_speeds, terminal_speeds[1:], strict=False))
+    )
+    expected_distance = max(20.0, self.FREE_FLOW_HORIZON * v_ego)
+
+    action = getattr(model, 'action', None)
+    try:
+      desired_accel = float(getattr(action, 'desiredAcceleration', 0.0))
+    except (TypeError, ValueError):
+      return None
+    if not math.isfinite(desired_accel):
+      return None
+    control_distance = max(0.1, self._control_distance(distance_m, v_ego))
+    required_decel = v_ego * v_ego / (2.0 * control_distance)
+    action_threshold = max(self.ACTION_DECEL_MIN, self.ACTION_DECEL_RATIO * required_decel)
+    action_evidence = bool(getattr(action, 'shouldStop', False)) or desired_accel <= -action_threshold
+    evidence = action_evidence and terminal_profile and distance_m <= expected_distance * self.SHORT_TRAJECTORY_RATIO
+    return _Observation(distance_m, evidence)
+
+  def _fuse(self, observed_distance: float) -> None:
+    self._distance_m += max(-1.0, min(1.0, observed_distance - self._distance_m)) * 0.25
+
+  @classmethod
+  def _sanitize_dt(cls, dt: float) -> float:
+    try:
+      dt = float(dt)
+    except (TypeError, ValueError):
+      return DT_MDL
+    return max(0.01, min(0.5, dt)) if math.isfinite(dt) and dt > 0.0 else DT_MDL
+
+  @staticmethod
+  def _read_speed(car_state) -> float:
+    try:
+      v_ego = float(getattr(car_state, 'vEgo', 0.0))
+    except (TypeError, ValueError):
+      return 0.0
+    return max(0.0, v_ego) if math.isfinite(v_ego) else 0.0
+
+  def _world_tolerance(self, distance_m: float) -> float:
+    return min(self.WORLD_ERROR_MAX, max(self.WORLD_ERROR_MIN, 0.02 * distance_m))
+
+  def _control_distance(self, distance_m: float, v_ego: float) -> float:
+    return distance_m - self.ENDPOINT_SETTLE_MARGIN - v_ego * self.ACTUATION_LOOKAHEAD
+
+  def _constraint(self) -> StopConstraint:
+    active = self._state in (StopIntentState.approach, StopIntentState.hold)
+    accel_ceiling = math.inf
+    if active:
+      control_distance = max(0.1, self._control_distance(max(0.0, self._distance_m), self._last_v_ego))
+      accel_ceiling = -min(self.MAX_REQUIRED_DECEL, self._last_v_ego**2 / (2.0 * control_distance))
+    confidence = 1.0 if active else min(1.0, self._evidence_time / self.QUALIFY_TIME)
+    return StopConstraint(active, max(0.0, self._distance_m), self._state == StopIntentState.hold, self._state, confidence, accel_ceiling)

sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -1,6 +1,7 @@
 import pytest
 
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.stop_intent import StopIntentState
 
 
 class MockLeadOne:
@@ -16,10 +17,16 @@ class MockRadarState:
 
 
 class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
+  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False, gasPressed=False):
     self.vEgo = vEgo
     self.vCruise = vCruise
     self.standstill = standstill
+    self.gasPressed = gasPressed
+
+
+class MockCarControl:
+  def __init__(self, long_active=True):
+    self.longActive = long_active
 
 
 class MockAction:
@@ -29,13 +36,14 @@ class MockAction:
 
 
 class MockModelData:
-  def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False):
+  def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False, terminal_speed=20.0):
     position_size = 33 if valid else 10
     orientation_size = position_size if orientation_valid is None else (33 if orientation_valid else 10)
     position_x = [0.0] * position_size
     if position_x:
       position_x[-1] = endpoint_x
-    self.position = type("Pos", (), {"x": position_x})()
+    self.position = type("Pos", (), {"x": position_x, "y": [0.0] * position_size})()
+    self.velocity = type("Velocity", (), {"x": [terminal_speed] * position_size, "y": [0.0] * position_size})()
     self.orientation = type("Ori", (), {"x": [0.0] * orientation_size})()
     self.acceleration = type("Accel", (), {"x": [0.0] * position_size})()
     self.action = MockAction(desired_acceleration, should_stop)
@@ -51,14 +59,21 @@ class MockParams:
     return True
 
 
+class MockSubMaster(dict):
+  def __init__(self, *args, **kwargs):
+    super().__init__(*args, **kwargs)
+    self.valid = {'modelV2': True}
+
+
 @pytest.fixture
 def default_sm():
-  sm = {
+  sm = MockSubMaster({
+    'carControl': MockCarControl(),
     'carState': MockCarState(vEgo=10.0, vCruise=20.0),
     'radarState': MockRadarState(status=1.0),
     'modelV2': MockModelData(valid=True),
     'selfdriveState': MockSelfDriveState(experimentalMode=True),
-  }
+  })
   return sm
 
 
@@ -66,6 +81,7 @@ def default_sm():
 def mock_cp():
   class CP:
     radarUnavailable = False
+    openpilotLongitudinalControl = True
   return CP()
 
 
@@ -233,3 +249,68 @@ def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, defau
 
   assert controller._has_lead_filtered
   assert controller.mode() == "acc"
+
+
+def test_consistent_stop_intent_commits_and_survives_radar_acquisition(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+
+  for frame in range(20):
+    default_sm['modelV2'] = MockModelData(endpoint_x=55.0 - frame * 0.5, desired_acceleration=-1.0, terminal_speed=0.0)
+    controller.update(default_sm)
+
+  assert controller.stop_constraint().active
+  assert controller.stop_constraint().state == StopIntentState.approach
+  assert controller.mode() == "blended"
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  controller.update(default_sm)
+  assert controller.stop_constraint().active
+  assert controller.mode() == "blended"
+
+
+def test_gas_suppresses_committed_stop_and_returns_to_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+
+  for frame in range(20):
+    default_sm['modelV2'] = MockModelData(endpoint_x=50.0 - frame * 0.5, desired_acceleration=-1.0, terminal_speed=0.0)
+    controller.update(default_sm)
+  assert controller.stop_constraint().active
+
+  default_sm['carState'].gasPressed = True
+  controller.update(default_sm)
+
+  assert controller.stop_constraint().state == StopIntentState.suppressed
+  assert not controller.stop_constraint().active
+  assert controller.mode() == "acc"
+
+
+def test_vision_only_lead_blocks_stop_qualification(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0)
+
+  for frame in range(20):
+    default_sm['modelV2'] = MockModelData(endpoint_x=55.0 - frame * 0.5, desired_acceleration=-1.0, terminal_speed=0.0)
+    controller.update(default_sm)
+
+  assert controller.stop_constraint().state == StopIntentState.clear
+  assert not controller.stop_constraint().active
+
+
+def test_stop_qualification_requires_valid_model_and_longitudinal_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm.valid['modelV2'] = False
+
+  for frame in range(20):
+    default_sm['modelV2'] = MockModelData(endpoint_x=55.0 - frame * 0.5, desired_acceleration=-1.0, terminal_speed=0.0)
+    controller.update(default_sm)
+  assert controller.stop_constraint().state == StopIntentState.clear
+
+  default_sm.valid['modelV2'] = True
+  default_sm['carControl'].longActive = False
+  for frame in range(20):
+    default_sm['modelV2'] = MockModelData(endpoint_x=55.0 - frame * 0.5, desired_acceleration=-1.0, terminal_speed=0.0)
+    controller.update(default_sm)
+  assert controller.stop_constraint().state == StopIntentState.clear

sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_closed_loop.py

@@ -0,0 +1,135 @@
+from collections import deque
+
+import numpy as np
+import pytest
+
+from cereal import car, custom
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_CTRL, DT_MDL
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.stop_intent import StopIntentController, StopIntentState
+
+
+class CarState:
+  def __init__(self, v_ego: float):
+    self.vEgo = v_ego
+    self.aEgo = 0.0
+    self.standstill = False
+    self.gasPressed = False
+    self.brakePressed = False
+    self.cruiseState = type('CruiseState', (), {'standstill': False})()
+
+
+class StopModel:
+  def __init__(self, distance_m: float, v_ego: float, *, valid: bool = True):
+    size = StopIntentController.TRAJECTORY_SIZE if valid else 0
+    self.position = type(
+      'Position',
+      (),
+      {
+        'x': np.linspace(0.0, max(distance_m, 0.1), size).tolist(),
+        'y': np.zeros(size).tolist(),
+      },
+    )()
+    self.velocity = type(
+      'Velocity',
+      (),
+      {
+        'x': np.linspace(v_ego, 0.0, size).tolist(),
+        'y': np.zeros(size).tolist(),
+      },
+    )()
+    control_distance = max(0.1, distance_m - StopIntentController.ENDPOINT_SETTLE_MARGIN - v_ego * StopIntentController.ACTUATION_LOOKAHEAD)
+    required_decel = v_ego * v_ego / (2.0 * control_distance)
+    rolling_decel = min(1.5, max(StopIntentController.ACTION_DECEL_MIN, 0.45 * required_decel))
+    self.action = type(
+      'Action',
+      (),
+      {'shouldStop': valid and v_ego < 0.3, 'desiredAcceleration': -rolling_decel if valid else 0.0},
+    )()
+
+
+def run_stop_approach(*, initial_speed: float = 12.0, stop_line: float = 60.0, dropout: range = range(0), duration: float = 30.0):
+  distance = 0.0
+  v_ego = initial_speed
+  a_ego = 0.0
+  controller = StopIntentController()
+  car_state = CarState(v_ego)
+  CP = car.CarParams.new_message()
+  CP.stopAccel = -2.0
+  CP.stoppingDecelRate = 0.8
+  CP.vEgoStopping = 0.5
+  CP.vEgoStarting = 0.5
+  CP.longitudinalActuatorDelay = StopIntentController.ACTUATION_LOOKAHEAD
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.0]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.0]
+  long_control = LongControl(CP, custom.CarParamsSP.new_message())
+  actuator_delay = deque([0.0] * round(CP.longitudinalActuatorDelay / DT_CTRL))
+  trace = []
+
+  for frame in range(round(duration / DT_MDL)):
+    remaining = stop_line - distance
+    car_state.vEgo = v_ego
+    car_state.aEgo = a_ego
+    car_state.standstill = v_ego < 0.01
+    car_state.cruiseState.standstill = car_state.standstill
+    observation_valid = frame not in dropout
+    model = StopModel(remaining, v_ego, valid=observation_valid)
+    constraint = controller.update(
+      model,
+      car_state,
+      enabled=True,
+      lead_present=False,
+      observation_valid=observation_valid,
+      dt=DT_MDL,
+    )
+
+    a_target = min(model.action.desiredAcceleration, 0.0)
+    if constraint.active:
+      a_target = min(a_target, constraint.accel_ceiling_mps2)
+    should_stop = constraint.hold or (model.action.shouldStop and not constraint.active)
+    for _ in range(round(DT_MDL / DT_CTRL)):
+      car_state.vEgo = v_ego
+      car_state.aEgo = a_ego
+      car_state.standstill = v_ego < 0.01
+      car_state.cruiseState.standstill = car_state.standstill
+      commanded_accel = long_control.update(True, car_state, a_target, should_stop, (ACCEL_MIN, ACCEL_MAX))
+      actuator_delay.append(commanded_accel)
+      delayed_accel = actuator_delay.popleft()
+      a_ego += np.clip(delayed_accel - a_ego, -4.0 * DT_CTRL, 4.0 * DT_CTRL)
+      v_ego = max(0.0, v_ego + a_ego * DT_CTRL)
+      distance += v_ego * DT_CTRL
+    trace.append((distance, v_ego, a_ego, constraint))
+
+  return stop_line, trace
+
+
+@pytest.mark.parametrize(("initial_speed", "stop_line"), [(3.0, 15.0), (8.0, 35.0), (12.0, 60.0), (20.0, 150.0)])
+def test_stationary_constraint_stops_and_holds_at_target(initial_speed, stop_line):
+  stop_line, trace = run_stop_approach(initial_speed=initial_speed, stop_line=stop_line)
+  active_constraints = [constraint for _, _, _, constraint in trace if constraint.active]
+
+  assert active_constraints
+  assert all(not constraint.hold for constraint in active_constraints if constraint.state == StopIntentState.approach)
+  assert trace[-1][3].state == StopIntentState.hold
+  assert trace[-1][3].hold
+  assert trace[-1][1] < 0.02
+  assert 0.0 <= stop_line - trace[-1][0] < 3.0
+  assert min(a_ego for _, _, a_ego, _ in trace) >= -StopIntentController.MAX_REQUIRED_DECEL - 0.01
+
+  first_active = next(idx for idx, (*_, constraint) in enumerate(trace) if constraint.active)
+  active_speeds = [v_ego for _, v_ego, _, _ in trace[first_active:]]
+  assert all(next_speed <= speed + 0.01 for speed, next_speed in zip(active_speeds, active_speeds[1:], strict=False))
+
+
+def test_committed_stop_survives_model_dropout_without_reaccelerating():
+  stop_line, trace = run_stop_approach(dropout=range(30, 38))
+  dropout_trace = trace[30:38]
+
+  assert all(constraint.active for _, _, _, constraint in dropout_trace)
+  assert max(v_ego for _, v_ego, _, _ in dropout_trace) <= trace[29][1] + 0.05
+  assert trace[-1][3].hold
+  assert trace[-1][1] < 0.02
+  assert 0.0 <= stop_line - trace[-1][0] < 3.0

sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_intent.py

@@ -0,0 +1,227 @@
+import math
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.stop_intent import StopIntentController, StopIntentState
+
+
+class CarState:
+  def __init__(self, v_ego=10.0, *, standstill=False, gas_pressed=False):
+    self.vEgo = v_ego
+    self.standstill = standstill
+    self.gasPressed = gas_pressed
+
+
+class Model:
+  def __init__(
+    self,
+    distance=50.0,
+    *,
+    valid=True,
+    terminal_speed=0.0,
+    terminal_lateral_speed=0.0,
+    should_stop=False,
+    desired_accel=-1.0,
+    curve=0.0,
+  ):
+    size = StopIntentController.TRAJECTORY_SIZE if valid else 0
+    denominator = max(1, size - 1)
+    self.position = type(
+      'Position',
+      (),
+      {
+        'x': [distance * i / denominator for i in range(size)],
+        'y': [distance * curve * i / denominator for i in range(size)],
+      },
+    )()
+    self.velocity = type(
+      'Velocity',
+      (),
+      {
+        'x': [terminal_speed] * size,
+        'y': [terminal_lateral_speed] * size,
+      },
+    )()
+    self.action = type('Action', (), {'shouldStop': should_stop, 'desiredAcceleration': desired_accel})()
+
+
+def run_fixed_target(controller, car_state, start_distance=60.0, frames=20, **model_kwargs):
+  constraint = None
+  for frame in range(frames):
+    distance = max(0.1, start_distance - car_state.vEgo * frame * DT_MDL)
+    constraint = controller.update(Model(distance, **model_kwargs), car_state, enabled=True, lead_present=False)
+  return constraint
+
+
+def qualify(controller, car_state, start_distance=60.0):
+  constraint = run_fixed_target(controller, car_state, start_distance)
+  assert constraint.active
+  return constraint
+
+
+def test_fixed_world_endpoint_qualifies_and_sets_bounded_ceiling():
+  controller = StopIntentController()
+  car_state = CarState(v_ego=10.0)
+
+  constraint = qualify(controller, car_state)
+
+  control_distance = constraint.distance_m - controller.ENDPOINT_SETTLE_MARGIN - car_state.vEgo * controller.ACTUATION_LOOKAHEAD
+  assert constraint.state == StopIntentState.approach
+  assert constraint.accel_ceiling_mps2 == pytest.approx(-(car_state.vEgo**2) / (2.0 * control_distance))
+  assert -controller.MAX_REQUIRED_DECEL <= constraint.accel_ceiling_mps2 < 0.0
+
+
+def test_curved_path_uses_arc_length():
+  controller = StopIntentController()
+  constraint = controller.update(Model(10.0, curve=1.0), CarState(v_ego=1.0), enabled=True, lead_present=False)
+
+  assert constraint.state == StopIntentState.qualifying
+  assert constraint.distance_m == pytest.approx(math.sqrt(200.0))
+
+
+@pytest.mark.parametrize('target_world_speed', [1.0, 2.0, 4.0])
+def test_moving_world_endpoint_never_commits(target_world_speed):
+  controller = StopIntentController()
+  car_state = CarState(v_ego=10.0)
+
+  for frame in range(40):
+    distance = 60.0 - (car_state.vEgo - target_world_speed) * frame * DT_MDL
+    constraint = controller.update(Model(distance), car_state, enabled=True, lead_present=False)
+
+  assert not constraint.active
+
+
+@pytest.mark.parametrize('v_ego', [0.0, 2.0, 4.0])
+def test_rolling_constant_horizon_never_commits(v_ego):
+  controller = StopIntentController()
+  car_state = CarState(v_ego=v_ego, standstill=v_ego == 0.0)
+
+  for _ in range(80):
+    constraint = controller.update(Model(12.0), car_state, enabled=True, lead_present=False)
+
+  assert not constraint.active
+
+
+@pytest.mark.parametrize(('v_ego', 'terminal_speed'), [(10.0, 2.0), (20.0, 5.0)])
+def test_speed_reduction_trajectory_is_not_promoted_to_stop(v_ego, terminal_speed):
+  controller = StopIntentController()
+  constraint = run_fixed_target(controller, CarState(v_ego=v_ego), 5.0 * v_ego, 30, terminal_speed=terminal_speed)
+
+  assert constraint.state == StopIntentState.clear
+
+
+def test_terminal_velocity_dip_and_turn_are_rejected():
+  controller = StopIntentController()
+  car_state = CarState()
+  dip = Model(50.0, terminal_speed=5.0)
+  dip.velocity.x[-1] = 0.0
+
+  for _ in range(20):
+    dip_constraint = controller.update(dip, car_state, enabled=True, lead_present=False)
+  controller.reset()
+  for _ in range(20):
+    turn_constraint = controller.update(
+      Model(50.0, terminal_lateral_speed=8.0),
+      car_state,
+      enabled=True,
+      lead_present=False,
+    )
+
+  assert dip_constraint.state == StopIntentState.clear
+  assert turn_constraint.state == StopIntentState.clear
+
+
+def test_gentle_low_speed_stop_can_qualify():
+  controller = StopIntentController()
+  constraint = run_fixed_target(controller, CarState(v_ego=3.0), 15.0, 30, desired_accel=-0.35)
+
+  assert constraint.state == StopIntentState.approach
+
+
+def test_acquisition_reserves_qualification_and_actuator_distance():
+  controller = StopIntentController()
+  v_ego = 8.0
+  reserve = max(v_ego * controller.QUALIFY_TIME, controller.WORLD_ERROR_MIN + controller.QUALIFY_TRAVEL_MARGIN)
+  minimum = controller.ENDPOINT_SETTLE_MARGIN + v_ego * controller.ACTUATION_LOOKAHEAD + v_ego**2 / (2.0 * controller.MAX_REQUIRED_DECEL) + reserve
+
+  assert controller._feasible(minimum + 1e-6, v_ego, reserve)
+  assert not controller._feasible(minimum - 1e-6, v_ego, reserve)
+  assert not controller._feasible(150.0, controller.MAX_APPROACH_SPEED + 0.1, reserve)
+
+
+def test_lead_or_invalid_model_blocks_acquisition():
+  controller = StopIntentController()
+  car_state = CarState()
+
+  for frame in range(30):
+    distance = 60.0 - car_state.vEgo * frame * DT_MDL
+    lead_constraint = controller.update(Model(distance), car_state, enabled=True, lead_present=True)
+  controller.reset()
+  for _ in range(30):
+    invalid_constraint = controller.update(Model(valid=False), car_state, enabled=True, lead_present=False)
+
+  assert lead_constraint.state == StopIntentState.clear
+  assert invalid_constraint.state == StopIntentState.clear
+
+
+def test_committed_target_survives_dropout_and_accepts_closer_revision():
+  controller = StopIntentController()
+  car_state = CarState()
+  constraint = qualify(controller, car_state)
+  distance_before_dropout = constraint.distance_m
+
+  for _ in range(4):
+    constraint = controller.update(Model(), car_state, enabled=True, lead_present=False, observation_valid=False)
+  assert constraint.distance_m == pytest.approx(distance_before_dropout - 2.0)
+
+  closer_distance = constraint.distance_m - 8.0
+  constraint = controller.update(Model(closer_distance), car_state, enabled=True, lead_present=False)
+  assert constraint.distance_m == pytest.approx(closer_distance)
+
+
+def test_committed_target_cancels_only_on_sustained_clear_evidence():
+  controller = StopIntentController()
+  car_state = CarState()
+  constraint = qualify(controller, car_state)
+
+  for _ in range(9):
+    constraint = controller.update(
+      Model(max(0.1, constraint.distance_m - car_state.vEgo * DT_MDL), desired_accel=0.0),
+      car_state,
+      enabled=True,
+      lead_present=False,
+    )
+  assert constraint.active
+
+  constraint = controller.update(Model(constraint.distance_m, desired_accel=0.0), car_state, enabled=True, lead_present=False)
+  assert constraint.state == StopIntentState.clear
+
+
+def test_early_standstill_latches_hold_and_gap_does_not_release_it():
+  controller = StopIntentController()
+  car_state = CarState()
+  constraint = qualify(controller, car_state)
+  assert constraint.distance_m > controller.HOLD_DISTANCE
+
+  car_state.vEgo = 0.0
+  car_state.standstill = True
+  constraint = controller.update(Model(constraint.distance_m), car_state, enabled=True, lead_present=True)
+  assert constraint.state == StopIntentState.hold
+
+  for _ in range(20):
+    constraint = controller.update(Model(10.0, should_stop=True), car_state, enabled=True, lead_present=False)
+  constraint = controller.update(Model(10.0, desired_accel=0.0), car_state, enabled=True, lead_present=False, dt=0.5)
+  assert constraint.state == StopIntentState.hold
+
+
+def test_gas_immediately_suppresses_committed_stop():
+  controller = StopIntentController()
+  car_state = CarState()
+  qualify(controller, car_state)
+
+  car_state.gasPressed = True
+  constraint = controller.update(Model(), car_state, enabled=True, lead_present=False)
+
+  assert constraint.state == StopIntentState.suppressed
+  assert not constraint.active

sunnypilot/sunnylink/settings_ui.json

@@ -571,7 +571,7 @@
               "key": "DynamicExperimentalControl",
               "widget": "toggle",
               "title": "Dynamic Experimental Control",
-              "description": "Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal.",
+              "description": "Let the model choose between sunnypilot ACC and End to End Longitudinal. Stable stop trajectories can brake to a full stop and hold.",
               "visibility": [
                 {
                   "type": "capability",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -19,7 +19,7 @@ sections:
   - key: DynamicExperimentalControl
     widget: toggle
     title: Dynamic Experimental Control
-    description: Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal.
+    description: Let the model choose between sunnypilot ACC and End to End Longitudinal. Stable stop trajectories can brake to a full stop and hold.
     visibility:
     - $ref: '#/macros/longitudinal'
     enablement: