From b1039ef1c3afe2afe4091459c5ecd94276458c36 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 16 Jul 2026 13:08:12 -0700 Subject: [PATCH] Make DEC reliably complete model-predicted stops --- .../controls/lib/longitudinal_planner.py | 5 +- .../ui/sunnypilot/layouts/settings/cruise.py | 2 +- sunnypilot/selfdrive/controls/lib/dec/dec.py | 35 ++- .../selfdrive/controls/lib/dec/stop_intent.py | 268 ++++++++++++++++++ .../lib/dec/tests/test_dynamic_controller.py | 91 +++++- .../lib/dec/tests/test_stop_closed_loop.py | 135 +++++++++ .../lib/dec/tests/test_stop_intent.py | 227 +++++++++++++++ sunnypilot/sunnylink/settings_ui.json | 2 +- .../settings_ui_src/pages/cruise.yaml | 2 +- 9 files changed, 756 insertions(+), 11 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/dec/stop_intent.py create mode 100644 sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_closed_loop.py create mode 100644 sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_intent.py diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ea1fddc034..39946830c3 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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]) diff --git a/selfdrive/ui/sunnypilot/layouts/settings/cruise.py b/selfdrive/ui/sunnypilot/layouts/settings/cruise.py index 671174ac7a..7e3d80364b 100644 --- a/selfdrive/ui/sunnypilot/layouts/settings/cruise.py +++ b/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 = [ diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index a10a8fc916..ceb24bc12f 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/dec/stop_intent.py b/sunnypilot/selfdrive/controls/lib/dec/stop_intent.py new file mode 100644 index 0000000000..a2404bb97f --- /dev/null +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index d4fa748f7a..b7d733eb73 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_closed_loop.py b/sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_closed_loop.py new file mode 100644 index 0000000000..95479f2025 --- /dev/null +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_intent.py b/sunnypilot/selfdrive/controls/lib/dec/tests/test_stop_intent.py new file mode 100644 index 0000000000..297ec7b46e --- /dev/null +++ b/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 diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 4205697617..4a451cd617 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/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", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 11688c306b..3325e178fc 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/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: