From 6e58a32b600cc89940676185a88dafe742e8bf5e Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 20 Aug 2026 13:36:49 -0700 Subject: [PATCH] dec: rewrite acc/blended --- openpilot/cereal/custom.capnp | 4 + .../controls/lib/longitudinal_planner.py | 1 + .../selfdrive/controls/lib/dec/constants.py | 17 - .../selfdrive/controls/lib/dec/dec.py | 441 +++++------------- .../lib/dec/tests/test_dynamic_controller.py | 328 ++++++++++--- .../controls/lib/longitudinal_planner.py | 8 +- .../test/longitudinal_maneuvers/plant.py | 47 +- .../tests/test_dec_maneuvers.py | 179 +++++++ 8 files changed, 612 insertions(+), 413 deletions(-) delete mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_dec_maneuvers.py diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 5c7766ed02..1f4e4a1524 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -209,6 +209,10 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { state @0 :DynamicExperimentalControlState; enabled @1 :Bool; active @2 :Bool; + decelIntent @3 :Float32; + curveDetected @4 :Bool; + wantBlended @5 :Bool; + leadVeto @6 :Bool; enum DynamicExperimentalControlState { acc @0; diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index daff74038e..003937f189 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -132,6 +132,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target) self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality) + self.update_dec(sm) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py deleted file mode 100644 index 4586afbc9f..0000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ /dev/null @@ -1,17 +0,0 @@ -class WMACConstants: - # Lead detection parameters - LEAD_WINDOW_SIZE = 6 # Stable detection window - LEAD_PROB = 0.45 # Balanced threshold for lead detection - - # Slow down detection parameters - SLOW_DOWN_WINDOW_SIZE = 5 # Responsive but stable - SLOW_DOWN_PROB = 0.3 # Balanced threshold for slow down scenarios - - # Optimized slow down distance curve - smooth and progressive - SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] - SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.] - - # Slowness detection parameters - SLOWNESS_WINDOW_SIZE = 10 # Stable slowness detection - SLOWNESS_PROB = 0.55 # Clear threshold for slowness - SLOWNESS_CRUISE_OFFSET = 1.025 # Conservative cruise speed offset diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py index fb854edae8..fe8e0540c3 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -4,192 +4,116 @@ Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -# Version = 2025-6-30 +from dataclasses import dataclass +from typing import Literal + +import numpy as np from openpilot.cereal import messaging from opendbc.car import structs -from numpy import interp from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants -from typing import Literal +from openpilot.selfdrive.modeld.constants import ModelConstants -# d-e2e, from modeldata.h -TRAJECTORY_SIZE = 33 -SET_MODE_TIMEOUT = 15 - -# Define the valid mode types ModeType = Literal['acc', 'blended'] +_DECEL_LOOKAHEAD_MIN_T = 1.0 +_DECEL_LOOKAHEAD_MAX_T = 6.0 +_T_IDXS = np.array(ModelConstants.T_IDXS) +_DECEL_IDX = np.where((_T_IDXS >= _DECEL_LOOKAHEAD_MIN_T) & (_T_IDXS <= _DECEL_LOOKAHEAD_MAX_T))[0] +_DECEL_INV_T = 1.0 / _T_IDXS[_DECEL_IDX] -class SmoothKalmanFilter: - """Enhanced Kalman filter with smoothing for stable decision making.""" +DECEL_INTENT_A_HINT = 0.35 +DECEL_INTENT_A_FULL = 1.30 +DECEL_INTENT_TRIGGER = 0.5 - def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01, - alpha=1.0, smoothing_factor=0.85): - self.x = initial_value - self.P = 1.0 - self.R = measurement_noise - self.Q = process_noise - self.alpha = alpha - self.smoothing_factor = smoothing_factor - self.initialized = False - self.history = [] - self.max_history = 10 - self.confidence = 0.0 +CURVE_Y_MAX = 5.0 - def add_data(self, measurement): - if len(self.history) >= self.max_history: - self.history.pop(0) - self.history.append(measurement) +LEAD_FUTURE_PROB_VANISH = 0.35 - if not self.initialized: - self.x = measurement - self.initialized = True - self.confidence = 0.1 - return +MODEL_DROP_TRUST_FULL = 5.0 +MODEL_DROP_TRUST_NONE = 30.0 +MODEL_TRUST_MIN = 0.5 - self.P = self.alpha * self.P + self.Q +CREEP_SPEED_ENTER = 2.0 +CREEP_SPEED_EXIT = 3.0 - K = self.P / (self.P + self.R) - effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1 +ENTER_FRAMES = 3 +EXIT_FRAMES = 16 +MIN_BLENDED_FRAMES = 20 - innovation = measurement - self.x - self.x = self.x + effective_K * innovation - self.P = (1 - effective_K) * self.P - - if abs(innovation) < 0.1: - self.confidence = min(1.0, self.confidence + 0.05) - else: - self.confidence = max(0.1, self.confidence - 0.02) - - def get_value(self): - return self.x if self.initialized else None - - def get_confidence(self): - return self.confidence - - def reset_data(self): - self.initialized = False - self.history = [] - self.confidence = 0.0 +PARAM_READ_FRAMES = 5 -class ModeTransitionManager: - """Manages smooth transitions between driving modes with hysteresis.""" +@dataclass +class DecSignals: + decel_intent: float = 0.0 + curve_detected: bool = False + model_trust: float = 1.0 + creeping: bool = False + +def should_blend(s: DecSignals) -> bool: + degraded = s.model_trust < MODEL_TRUST_MIN + slowdown_detected = not degraded and s.decel_intent >= DECEL_INTENT_TRIGGER and not s.curve_detected + return slowdown_detected or s.creeping + + +class ModeHysteresis: def __init__(self): - self.current_mode: ModeType = 'acc' - self.mode_confidence = {'acc': 1.0, 'blended': 0.0} - self.transition_timeout = 0 - self.min_mode_duration = 10 - self.mode_duration = 0 - self.emergency_override = False + self.mode: ModeType = 'acc' + self.above = 0 + self.below = 0 + self.blended_frames = 0 - def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False): - # Emergency override for critical situations (stops, collisions) - if emergency: - self.emergency_override = True - self.current_mode = mode - self.transition_timeout = SET_MODE_TIMEOUT - self.mode_duration = 0 - return + def update(self, want_blended: bool, override: bool, veto: bool) -> ModeType: + self.above = self.above + 1 if want_blended else 0 + self.below = 0 if want_blended else self.below + 1 - self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence) - for m in self.mode_confidence: - if m != mode: - self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05) + if override: + self.mode, self.blended_frames = 'blended', 0 + elif veto: + self.mode = 'acc' + elif self.mode == 'acc': + if self.above >= ENTER_FRAMES: + self.mode, self.blended_frames = 'blended', 0 + else: + self.blended_frames += 1 + if self.blended_frames >= MIN_BLENDED_FRAMES and self.below >= EXIT_FRAMES: + self.mode = 'acc' + return self.mode - # Require minimum duration in current mode (unless emergency) - if self.mode_duration < self.min_mode_duration and not self.emergency_override: - return - - # Hysteresis: higher threshold for mode changes - confidence_threshold = 0.6 if mode != self.current_mode else 0.3 # Lower threshold for faster response - - if self.mode_confidence[mode] > confidence_threshold: - if mode != self.current_mode and self.transition_timeout == 0: - self.transition_timeout = SET_MODE_TIMEOUT - self.current_mode = mode - self.mode_duration = 0 - - def update(self): - if self.transition_timeout > 0: - self.transition_timeout -= 1 - self.mode_duration += 1 - - # Reset emergency override after some time - if self.emergency_override and self.mode_duration > 20: - self.emergency_override = False - - # Gradual confidence decay - for mode in self.mode_confidence: - self.mode_confidence[mode] *= 0.98 - - def get_mode(self) -> ModeType: - return self.current_mode + def reset(self) -> None: + self.mode = 'acc' + self.above = 0 + self.below = 0 + self.blended_frames = 0 class DynamicExperimentalController: def __init__(self, CP: structs.CarParams, mpc, params=None): - self._CP = CP self._mpc = mpc self._params = params or Params() self._enabled: bool = self._params.get_bool("DynamicExperimentalControl") self._active: bool = False self._frame: int = 0 - self._urgency = 0.0 - self._mode_manager = ModeTransitionManager() + self._hysteresis = ModeHysteresis() + self._creeping = False - # Smooth filters for stable decision making with faster response for critical scenarios - self._lead_filter = SmoothKalmanFilter( - measurement_noise=0.15, - process_noise=0.05, - alpha=1.02, - smoothing_factor=0.8 - ) + self.signals = DecSignals() + self.want_blended = False + self.lead_veto = False - self._slow_down_filter = SmoothKalmanFilter( - measurement_noise=0.1, - process_noise=0.1, - alpha=1.05, - smoothing_factor=0.7 - ) - - self._slowness_filter = SmoothKalmanFilter( - measurement_noise=0.1, - process_noise=0.06, - alpha=1.015, - smoothing_factor=0.92 - ) - - self._mpc_fcw_filter = SmoothKalmanFilter( - measurement_noise=0.2, - process_noise=0.1, - alpha=1.1, - smoothing_factor=0.5 - ) - self._has_lead_filtered = False - self._has_slow_down = False - self._has_slowness = False - self._has_mpc_fcw = False - self._v_ego_kph = 0.0 - self._v_cruise_kph = 0.0 - self._has_standstill = False - self._mpc_fcw_crash_cnt = 0 - self._standstill_count = 0 - # debug - self._endpoint_x = float('inf') - self._expected_distance = 0.0 - self._trajectory_valid = False + def _update_creeping(self, v_ego: float) -> bool: + self._creeping = v_ego < CREEP_SPEED_EXIT if self._creeping else v_ego <= CREEP_SPEED_ENTER + return self._creeping def _read_params(self) -> None: - if self._frame % int(1. / DT_MDL) == 0: + if self._frame % PARAM_READ_FRAMES == 0: self._enabled = self._params.get_bool("DynamicExperimentalControl") def mode(self) -> str: - return self._mode_manager.get_mode() + return self._hysteresis.mode def enabled(self) -> bool: return self._enabled @@ -197,192 +121,61 @@ class DynamicExperimentalController: def active(self) -> bool: return self._active - def set_mpc_fcw_crash_cnt(self) -> None: - """Set MPC FCW crash count""" - self._mpc_fcw_crash_cnt = self._mpc.crash_cnt + @staticmethod + def _decel_intent(md) -> float: + v = np.asarray(md.velocity.x) + if len(v) != len(_T_IDXS): + return 0.0 + a_req = float(np.min((v[_DECEL_IDX] - v[0]) * _DECEL_INV_T)) + return float(np.interp(-a_req, [DECEL_INTENT_A_HINT, DECEL_INTENT_A_FULL], [0.0, 1.0])) - def _update_calculations(self, sm: messaging.SubMaster) -> None: - car_state = sm['carState'] - lead_one = sm['radarState'].leadOne - md = sm['modelV2'] + @staticmethod + def _curve_detected(md) -> bool: + y = md.position.y + if len(y) < 1: + return False + return abs(y[-1]) >= CURVE_Y_MAX - self._v_ego_kph = car_state.vEgo * 3.6 - self._v_cruise_kph = car_state.vCruise - self._has_standstill = car_state.standstill + @staticmethod + def _model_trust(md) -> float: + if len(md.velocity.x) != len(_T_IDXS): + return 0.0 + return float(np.interp(md.frameDropPerc, [MODEL_DROP_TRUST_FULL, MODEL_DROP_TRUST_NONE], [1.0, 0.0])) - # standstill detection - if self._has_standstill: - self._standstill_count = min(20, self._standstill_count + 1) - else: - self._standstill_count = max(0, self._standstill_count - 1) - - # Lead detection - self._lead_filter.add_data(float(lead_one.present)) - lead_value = self._lead_filter.get_value() or 0.0 - self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB - - # MPC FCW detection - fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0 - self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0)) - self._has_mpc_fcw = fcw_filtered_value > 0.5 - - # Slow down detection - self._calculate_slow_down(md) - - # Slowness detection - if not (self._standstill_count > 5) and not self._has_slow_down: - current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET)) - self._slowness_filter.add_data(current_slowness) - slowness_value = self._slowness_filter.get_value() or 0.0 - - # Hysteresis for slowness - threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1) - self._has_slowness = slowness_value > threshold - - def _calculate_slow_down(self, md): - """Calculate urgency based on trajectory endpoint vs expected distance.""" - - # Reset to safe defaults - urgency = 0.0 - self._endpoint_x = float('inf') - self._trajectory_valid = False - - #Require exact trajectory size - position_valid = len(md.position.x) == TRAJECTORY_SIZE - orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE - - if not (position_valid and orientation_valid): - # Invalid trajectory - this itself might indicate a stop scenario - # Apply moderate urgency for incomplete trajectories at speed - if self._v_ego_kph > 20.0: - urgency = 0.3 - - self._slow_down_filter.add_data(urgency) - urgency_filtered = self._slow_down_filter.get_value() or 0.0 - self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB - self._urgency = urgency_filtered - return - - # We have a valid full trajectory - self._trajectory_valid = True - - # Use the exact endpoint (33rd point, index 32) - endpoint_x = md.position.x[TRAJECTORY_SIZE - 1] - self._endpoint_x = endpoint_x - - # Get expected distance based on current speed using tuned constants - expected_distance = interp(self._v_ego_kph, - WMACConstants.SLOW_DOWN_BP, - WMACConstants.SLOW_DOWN_DIST) - self._expected_distance = expected_distance - - # Calculate urgency based on trajectory shortage - if endpoint_x < expected_distance: - shortage = expected_distance - endpoint_x - shortage_ratio = shortage / expected_distance - - # Base urgency on shortage ratio - urgency = min(1.0, shortage_ratio * 2.0) - - # Increase urgency for very short trajectories (imminent stops) - critical_distance = expected_distance * 0.3 - if endpoint_x < critical_distance: - urgency = min(1.0, urgency * 2.0) - - # Speed-based urgency adjustment - if self._v_ego_kph > 25.0: - speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0 - urgency = min(1.0, urgency * speed_factor) - - # Apply filtering but with less smoothing for stops - self._slow_down_filter.add_data(urgency) - urgency_filtered = self._slow_down_filter.get_value() or 0.0 - - # Update state with lower threshold for better stop detection - self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8) - self._urgency = urgency_filtered - - def _radarless_mode(self) -> None: - """Radarless mode decision logic with emergency handling.""" - - # EMERGENCY: MPC FCW - immediate blended mode - if self._has_mpc_fcw: - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - return - - # Standstill: use blended - if self._standstill_count > 3: - self._mode_manager.request_mode('blended', confidence=0.9) - return - - # Slow down scenarios: emergency for high urgency, normal for lower urgency - if self._has_slow_down: - if self._urgency > 0.7: - # Emergency: immediate blended mode for high urgency stops - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - else: - # Normal: blended with urgency-based confidence - confidence = min(1.0, self._urgency * 1.5) - self._mode_manager.request_mode('blended', confidence=confidence) - return - - # Driving slow: use ACC (but not if actively slowing down) - if self._has_slowness and not self._has_slow_down: - self._mode_manager.request_mode('acc', confidence=0.8) - return - - # Default: ACC - self._mode_manager.request_mode('acc', confidence=0.7) - - def _radar_mode(self) -> None: - """Radar mode with emergency handling.""" - - # EMERGENCY: MPC FCW - immediate blended mode - if self._has_mpc_fcw: - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - return - - # If lead detected and not in standstill: always use ACC - if self._has_lead_filtered and not (self._standstill_count > 3): - self._mode_manager.request_mode('acc', confidence=1.0) - return - - # Slow down scenarios: emergency for high urgency, normal for lower urgency - if self._has_slow_down: - if self._urgency > 0.7: - # Emergency: immediate blended mode for high urgency stops - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - else: - # Normal: blended with urgency-based confidence - confidence = min(1.0, self._urgency * 1.3) - self._mode_manager.request_mode('blended', confidence=confidence) - return - - # Standstill: use blended - if self._standstill_count > 3: - self._mode_manager.request_mode('blended', confidence=0.9) - return - - # Driving slow: use ACC (but not if actively slowing down) - if self._has_slowness and not self._has_slow_down: - self._mode_manager.request_mode('acc', confidence=0.8) - return - - # Default: ACC - self._mode_manager.request_mode('acc', confidence=0.7) + @staticmethod + def _lead_veto(radar_state, md) -> bool: + lead_one, lead_two = radar_state.leadOne, radar_state.leadTwo + lead_now = lead_one.present or lead_two.present + probs = md.leadsV3 + future = min(probs[1].prob, probs[2].prob) if len(probs) >= 3 else 1.0 + return bool(lead_now and future > LEAD_FUTURE_PROB_VANISH) def update(self, sm: messaging.SubMaster) -> None: self._read_params() - self.set_mpc_fcw_crash_cnt() + car_state = sm['carState'] + md = sm['modelV2'] + radar_state = sm['radarState'] - self._update_calculations(sm) + is_creeping = self._update_creeping(car_state.vEgo) + self.lead_veto = self._lead_veto(radar_state, md) - if self._CP.radarUnavailable: - self._radarless_mode() + self.signals = DecSignals( + decel_intent=self._decel_intent(md), + curve_detected=self._curve_detected(md), + model_trust=self._model_trust(md), + creeping=is_creeping, + ) + self.want_blended = should_blend(self.signals) + + crash_override = self._mpc.crash_cnt >= 1 + hard_brake_override = bool(md.meta.hardBrakePredicted) + override = (crash_override or hard_brake_override) and not self.lead_veto + + if self._enabled: + self._hysteresis.update(self.want_blended, override, self.lead_veto) else: - self._radar_mode() + self._hysteresis.reset() - self._mode_manager.update() self._active = sm['selfdriveState'].experimentalMode and self._enabled self._frame += 1 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index 4fec6eaa52..97ea0c7cdb 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -1,91 +1,285 @@ +import numpy as np + +from openpilot.cereal import messaging +from opendbc.car import structs from openpilot.common.test import OpenpilotTestCase -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ( + DecSignals, + DynamicExperimentalController, + ModeHysteresis, + should_blend, + ENTER_FRAMES, + MIN_BLENDED_FRAMES, +) -class MockLeadOne: - def __init__(self, present=0.0): - self.present = present +T_IDXS = np.array(ModelConstants.T_IDXS) -class MockRadarState: - def __init__(self, present=0.0): - self.leadOne = MockLeadOne(present=present) - -class MockCarState: - def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False): - self.vEgo = vEgo - self.vCruise = vCruise - self.standstill = standstill - -class MockModelData: - def __init__(self, valid=True): - size = 33 if valid else 10 # incomplete if invalid - self.position = type("Pos", (), {"x": [0.0] * size})() - self.orientation = type("Ori", (), {"x": [0.0] * size})() - -class MockSelfDriveState: - def __init__(self, experimentalMode=False): - self.experimentalMode = experimentalMode class MockParams: + def __init__(self, enabled=True): + self._enabled = enabled + def get_bool(self, name): - return True + return self._enabled -def default_sm(): - sm = { - 'carState': MockCarState(vEgo=10.0, vCruise=20.0), - 'radarState': MockRadarState(present=1.0), - 'modelV2': MockModelData(valid=True), - 'selfdriveState': MockSelfDriveState(experimentalMode=True), + +class MockMpc: + def __init__(self, crash_cnt=0): + self.crash_cnt = crash_cnt + + +def flat_velocity(v): + return [float(v)] * len(T_IDXS) + + +def decel_velocity(v0, a): + return [float(max(0.0, v0 + a * t)) for t in T_IDXS] + + +def make_car_state(v_ego=10.0, v_cruise=20.0): + msg = messaging.new_message('carState') + msg.carState.vEgo = v_ego + msg.carState.vCruise = v_cruise + return msg.carState.as_reader() + + +def make_selfdrive_state(experimental_mode=True): + msg = messaging.new_message('selfdriveState') + msg.selfdriveState.experimentalMode = experimental_mode + return msg.selfdriveState.as_reader() + + +def make_radar_state(lead_present=False, lead_radar=False, lead_two_present=False): + msg = messaging.new_message('radarState') + msg.radarState.leadOne.present = lead_present + msg.radarState.leadOne.radar = lead_radar + msg.radarState.leadTwo.present = lead_two_present + return msg.radarState.as_reader() + + +def make_model_v2(velocity=None, position_y=None, hard_brake=False, lead_probs=None, frame_drop_perc=0.0): + msg = messaging.new_message('modelV2') + msg.modelV2.velocity.x = velocity if velocity is not None else flat_velocity(0.0) + msg.modelV2.position.y = position_y if position_y is not None else [0.0] * len(T_IDXS) + msg.modelV2.frameDropPerc = frame_drop_perc + msg.modelV2.meta.hardBrakePredicted = hard_brake + if lead_probs is not None: + msg.modelV2.init('leadsV3', 3) + for i, (prob, prob_time) in enumerate(zip(lead_probs, (0.0, 2.0, 4.0), strict=True)): + msg.modelV2.leadsV3[i].prob = prob + msg.modelV2.leadsV3[i].probTime = prob_time + return msg.modelV2.as_reader() + + +def make_sm(v_ego=10.0, v_cruise=20.0, velocity=None, position_y=None, hard_brake=False, + lead_present=False, lead_radar=False, lead_two_present=False, lead_probs=None, + frame_drop_perc=0.0, experimental_mode=True): + return { + 'carState': make_car_state(v_ego, v_cruise), + 'radarState': make_radar_state(lead_present, lead_radar, lead_two_present), + 'modelV2': make_model_v2(velocity, position_y, hard_brake, lead_probs, frame_drop_perc), + 'selfdriveState': make_selfdrive_state(experimental_mode), } - return sm -def mock_cp(): - class CP: - radarUnavailable = False - return CP() -def mock_mpc(): - class MPC: - crash_cnt = 0 - return MPC() +def make_controller(cp=None, mpc=None, enabled=True): + return DynamicExperimentalController(cp or structs.CarParams(), mpc or MockMpc(), params=MockParams(enabled)) -# Fake Kalman Filter that always returns a given value -class FakeKalman: - def __init__(self, value=1.0): - self.value = value - def add_data(self, v): pass - def get_value(self): return self.value - def get_confidence(self): return 1.0 - def reset_data(self): pass class TestDynamicExperimentalController(OpenpilotTestCase): - def test_initial_mode_is_acc(self, mock_cp, mock_mpc): - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + def test_initial_mode_is_acc(self): + controller = make_controller() assert controller.mode() == "acc" - def test_standstill_triggers_blended(self, mock_cp, mock_mpc, default_sm): - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - default_sm['carState'].standstill = True + def test_flat_plan_never_blends_at_any_speed(self): + for v_ego in (2.5, 5.6, 8.3, 13.9, 22.2, 30.6): + controller = make_controller() + sm = make_sm(v_ego=v_ego, velocity=flat_velocity(v_ego)) + for _ in range(100): + controller.update(sm) + assert controller.mode() == "acc", f"false blend on a flat plan at v_ego={v_ego}" + + def test_highway_slowdown_without_lead_blends(self): + v0 = 110 / 3.6 + a = (70 / 3.6 - v0) / 6.0 + controller = make_controller() + sm = make_sm(v_ego=v0, velocity=decel_velocity(v0, a)) for _ in range(10): - controller.update(default_sm) + controller.update(sm) assert controller.mode() == "blended" - def test_emergency_blended_on_fcw(self, mock_cp, mock_mpc, default_sm): - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - mock_mpc.crash_cnt = 1 # simulate FCW - for _ in range(2): - controller.update(default_sm) + def test_curve_exclusion_prevents_false_blend(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), position_y=[6.0] * len(T_IDXS)) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + def test_any_lead_forces_acc_even_with_strong_model_signal(self): + for lead_radar in (True, False): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_radar=lead_radar, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(60): + controller.update(sm) + assert controller.mode() == "acc" + + def test_veto_releases_without_rebuild_lag(self): + controller = make_controller() + lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(30): + controller.update(lead_sm) + assert controller.mode() == "acc" + assert controller.lead_veto + + no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=False) + for _ in range(ENTER_FRAMES + 2): + controller.update(no_lead_sm) + if controller.mode() == "blended": + break assert controller.mode() == "blended" - def test_radarless_slowdown_triggers_blended(self, mock_cp, mock_mpc, default_sm): - mock_cp.radarUnavailable = True - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + def test_lead_gone_with_no_underlying_slowdown_stays_acc(self): + controller = make_controller() + lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(30): + controller.update(lead_sm) + assert controller.mode() == "acc" - # Force conditions to simulate slowdown - controller._slow_down_filter = FakeKalman(value=1.0) # ty: ignore[invalid-assignment] - controller._v_ego_kph = 35.0 - default_sm['modelV2'] = MockModelData(valid=False) # Incomplete trajectory + no_lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False) + for _ in range(20): + controller.update(no_lead_sm) + assert controller.mode() == "acc" - for _ in range(3): - controller.update(default_sm) + def test_creep_does_not_release_lead_veto(self): + controller = make_controller() + sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(10): + controller.update(sm) + assert controller.mode() == "acc" + assert controller.lead_veto + def test_creep_hysteresis_band_without_lead(self): + controller = make_controller() + controller.update(make_sm(v_ego=1.5, velocity=flat_velocity(1.5))) + assert controller.signals.creeping + + controller.update(make_sm(v_ego=2.5, velocity=flat_velocity(2.5))) + assert controller.signals.creeping, "a small excursion above CREEP_SPEED_ENTER should not exit creeping" + + controller.update(make_sm(v_ego=5.0, velocity=flat_velocity(5.0))) + assert not controller.signals.creeping, "should exit creeping once genuinely above CREEP_SPEED_EXIT" + + def test_crash_cnt_override_inert_while_lead_present(self): + mpc = MockMpc(crash_cnt=0) + controller = make_controller(mpc=mpc) + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + mpc.crash_cnt = 1 + controller.update(sm) + assert controller.mode() == "acc" + + def test_crash_cnt_blends_within_one_frame_without_lead(self): + mpc = MockMpc(crash_cnt=1) + controller = make_controller(mpc=mpc) + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False) + controller.update(sm) assert controller.mode() == "blended" + + def test_hard_brake_predicted_blends_within_one_frame_without_lead(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True, lead_present=False) + controller.update(sm) + assert controller.mode() == "blended" + + def test_hard_brake_override_inert_while_lead_present(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True, + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + controller.update(sm) + assert controller.mode() == "acc" + + def test_degraded_model_does_not_blend(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0), frame_drop_perc=60.0) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + def test_short_plan_arrays_do_not_blend(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=[20.0] * 5) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + def test_disabled_param_holds_acc(self): + controller = make_controller(enabled=False) + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0)) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + +class TestModeHysteresis(OpenpilotTestCase): + def test_entry_requires_enter_frames(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES - 1): + assert h.update(want_blended=True, override=False, veto=False) == "acc" + assert h.update(want_blended=True, override=False, veto=False) == "blended" + + def test_override_beats_veto(self): + h = ModeHysteresis() + assert h.update(want_blended=False, override=True, veto=True) == "blended" + + def test_veto_forces_acc_even_when_reason_active(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES + 5): + assert h.update(want_blended=True, override=False, veto=True) == "acc" + + def test_counter_accumulates_under_veto_then_releases_instantly(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES + 5): + h.update(want_blended=True, override=False, veto=True) + assert h.mode == "acc" + assert h.update(want_blended=True, override=False, veto=False) == "blended" + + def test_exit_requires_min_dwell_and_sustained_absence(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES): + h.update(want_blended=True, override=False, veto=False) + assert h.mode == "blended" + for _ in range(MIN_BLENDED_FRAMES - 1): + assert h.update(want_blended=False, override=False, veto=False) == "blended" + assert h.update(want_blended=False, override=False, veto=False) == "acc" + + def test_no_flapping_on_alternating_reason(self): + h = ModeHysteresis() + changes = 0 + prev = h.mode + for i in range(200): + mode = h.update(want_blended=i % 2 == 0, override=False, veto=False) + changes += mode != prev + prev = mode + assert changes == 0 + + +class TestShouldBlend(OpenpilotTestCase): + def test_slowdown_detected_triggers(self): + assert should_blend(DecSignals(decel_intent=1.0)) + assert not should_blend(DecSignals(decel_intent=0.0)) + + def test_curve_exclusion_suppresses_slowdown(self): + assert not should_blend(DecSignals(decel_intent=1.0, curve_detected=True)) + + def test_degraded_model_suppresses_model_based_reasons(self): + s = DecSignals(decel_intent=1.0, model_trust=0.0) + assert not should_blend(s) + + def test_creep_bypasses_everything(self): + assert should_blend(DecSignals(model_trust=0.0, creeping=True)) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 0e69ece9f6..f254b17f2d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -93,9 +93,11 @@ class LongitudinalPlannerSP: def update(self, sm: messaging.SubMaster) -> None: self.accel_controller.update(sm) self.events_sp.clear() - self.dec.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp) + def update_dec(self, sm: messaging.SubMaster) -> None: + self.dec.update(sm) + def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') @@ -112,6 +114,10 @@ class LongitudinalPlannerSP: dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc dec.enabled = self.dec.enabled() dec.active = self.dec.active() + dec.decelIntent = float(self.dec.signals.decel_intent) + dec.curveDetected = bool(self.dec.signals.curve_detected) + dec.wantBlended = bool(self.dec.want_blended) + dec.leadVeto = bool(self.dec.lead_veto) accel_controller = longitudinalPlanSP.accelController accel_controller.enabled = self.accel_controller.is_enabled() diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py index 58389842f6..ff23e3b3bf 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -7,7 +7,7 @@ See the LICENSE.md file in the root directory for more details. from collections import deque from collections.abc import Callable -from dataclasses import dataclass +from dataclasses import asdict, dataclass import math import time from typing import Any @@ -28,6 +28,11 @@ LeadObservation = dict[str, Any] LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None] ModelActionFn = Callable[[float, float, float], tuple[float, bool]] EgoObservationFn = Callable[[float, float, float], tuple[float, float]] +ModelPlanFn = Callable[[float, float, float], list[float]] +ModelMetaFn = Callable[[float], tuple[list[float], bool, float]] +LeadFutureProbsFn = Callable[[float], tuple[float, float, float]] +PositionYFn = Callable[[float], list[float]] +ExperimentalModeFn = Callable[[float], bool] @dataclass(frozen=True) @@ -85,6 +90,11 @@ class PlantSP(Plant): lead_observation_fn: LeadObservationFn | None = None, model_action_fn: ModelActionFn | None = None, ego_observation_fn: EgoObservationFn | None = None, + model_plan_fn: ModelPlanFn | None = None, + model_meta_fn: ModelMetaFn | None = None, + lead_future_probs_fn: LeadFutureProbsFn | None = None, + position_y_fn: PositionYFn | None = None, + experimental_mode_fn: ExperimentalModeFn | None = None, actuator_delay: float | None = None, actuator_lag: float = 0.0, actuator_model: ActuatorModel | None = None, @@ -129,6 +139,11 @@ class PlantSP(Plant): self.lead_observation_fn = lead_observation_fn self.model_action_fn = model_action_fn self.ego_observation_fn = ego_observation_fn + self.model_plan_fn = model_plan_fn + self.model_meta_fn = model_meta_fn + self.lead_future_probs_fn = lead_future_probs_fn + self.position_y_fn = position_y_fn + self.experimental_mode_fn = experimental_mode_fn self.actuator_model = actuator_model self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay @@ -282,6 +297,10 @@ class PlantSP(Plant): # does not predict slowdown in e2e mode position = log.XYZTData.new_message() position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] + if self.position_y_fn is None: + position.y = [0.0] * len(ModelConstants.T_IDXS) + else: + position.y = [float(y) for y in self.position_y_fn(self.current_time)] model.modelV2.position = position if self.model_action_fn is None: model_acceleration, model_should_stop = self.acceleration + 0.5, False @@ -290,17 +309,34 @@ class PlantSP(Plant): model.modelV2.action.desiredAcceleration = float(model_acceleration) model.modelV2.action.shouldStop = bool(model_should_stop) velocity = log.XYZTData.new_message() - velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] - velocity.x[0] = float(self.speed) # always start at current speed + if self.model_plan_fn is None: + velocity_plan = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] + velocity_plan[0] = float(self.speed) # always start at current speed + else: + velocity_plan = [float(x) for x in self.model_plan_fn(self.current_time, self.speed, self.acceleration)] + velocity.x = velocity_plan model.modelV2.velocity = velocity acceleration = log.XYZTData.new_message() acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] model.modelV2.acceleration = acceleration model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] + if self.model_meta_fn is None: + brake3_probs, hard_brake_predicted, frame_drop_perc = [0.0] * 5, False, 0.0 + else: + brake3_probs, hard_brake_predicted, frame_drop_perc = self.model_meta_fn(self.current_time) + model.modelV2.meta.disengagePredictions.brake3MetersPerSecondSquaredProbs = [float(p) for p in brake3_probs] + model.modelV2.meta.hardBrakePredicted = bool(hard_brake_predicted) + model.modelV2.frameDropPerc = float(frame_drop_perc) + if self.lead_future_probs_fn is not None: + model.modelV2.init('leadsV3', 3) + lead_future_probs = self.lead_future_probs_fn(self.current_time) + for i, (prob, prob_time) in enumerate(zip(lead_future_probs, (0.0, 2.0, 4.0), strict=True)): + model.modelV2.leadsV3[i].prob = float(prob) + model.modelV2.leadsV3[i].probTime = prob_time control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else ( LongCtrlState.pid if self.enabled else LongCtrlState.off) - ss.selfdriveState.experimentalMode = self.e2e + ss.selfdriveState.experimentalMode = self.e2e if self.experimental_mode_fn is None else bool(self.experimental_mode_fn(self.current_time)) ss.selfdriveState.personality = self.personality control.controlsState.forceDecel = self.force_decel true_v_ego = self.speed @@ -383,6 +419,9 @@ class PlantSP(Plant): "fcw": fcw, "mpc_source": self.planner.mpc.source, "dec_mode": self.planner.dec.mode(), + "dec_want_blended": self.planner.dec.want_blended, + "dec_signals": asdict(self.planner.dec.signals), + "dec_lead_veto": self.planner.dec.lead_veto, "controller_active": self.planner.accel_controller_active, "model_action": { "desiredAcceleration": float(model_acceleration), diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_dec_maneuvers.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_dec_maneuvers.py new file mode 100644 index 0000000000..839e8c3846 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_dec_maneuvers.py @@ -0,0 +1,179 @@ +import numpy as np + +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.common.test import OpenpilotTestCase +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ENTER_FRAMES, MIN_BLENDED_FRAMES +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP + +T_IDXS = np.array(ModelConstants.T_IDXS) + + +def decel_plan(a): + def fn(_current_time, speed, _acceleration): + return [float(max(0.0, speed + a * t)) for t in T_IDXS] + return fn + + +def flat_plan(): + def fn(_current_time, speed, _acceleration): + return [float(speed)] * len(T_IDXS) + return fn + + +def alternating_plan(a): + def fn(current_time, speed, _acceleration): + frame_a = a if round(current_time / DT_MDL) % 2 == 0 else 0.0 + return [float(max(0.0, speed + frame_a * t)) for t in T_IDXS] + return fn + + +def persistent_lead_probs(_current_time): + return (1.0, 0.95, 0.9) + + +def _run(plant, steps, v_lead=0.0, v_cruise=50.0): + solver_failures = 0 + original_reset = plant.planner.mpc.reset + + def counting_reset(*args, **kw): + nonlocal solver_failures + if plant.planner.mpc.solution_status != 0: + solver_failures += 1 + return original_reset(*args, **kw) + + plant.planner.mpc.reset = counting_reset + return [plant.step(v_lead=v_lead, v_cruise=v_cruise) for _ in range(steps)], solver_failures + + +def mode_changes(results): + modes = [r["dec_mode"] for r in results] + return sum(a != b for a, b in zip(modes, modes[1:], strict=False)) + + +class TestDecManeuvers(OpenpilotTestCase): + def setUp(self): + super().setUp() + self.params = Params() + self.params.put_bool("DynamicExperimentalControl", True, block=True) + + def test_s1_lead_clears_with_underlying_slowdown_blends_quickly(self): + clear_t = 1.0 + + def lead_obs(current_time, _lead_name, truth): + return None if current_time >= clear_t else dict(truth) + + plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True, + lead_observation_fn=lead_obs, model_plan_fn=decel_plan(-2.5), + lead_future_probs_fn=persistent_lead_probs) + clear_frame = round(clear_t / DT_MDL) + results, _ = _run(plant, steps=clear_frame + ENTER_FRAMES + 5, v_lead=20.0, v_cruise=20.0) + + assert all(r["dec_mode"] == "acc" for r in results[:clear_frame]) + assert all(r["dec_lead_veto"] for r in results[:clear_frame]) + post_clear = [r["dec_mode"] for r in results[clear_frame:clear_frame + ENTER_FRAMES + 2]] + assert "blended" in post_clear + + def test_s1b_lead_clears_with_no_underlying_slowdown_stays_acc(self): + clear_t = 1.0 + + def lead_obs(current_time, _lead_name, truth): + return None if current_time >= clear_t else dict(truth) + + plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True, + lead_observation_fn=lead_obs, model_plan_fn=flat_plan(), + lead_future_probs_fn=persistent_lead_probs) + clear_frame = round(clear_t / DT_MDL) + results, _ = _run(plant, steps=clear_frame + MIN_BLENDED_FRAMES, v_lead=20.0, v_cruise=20.0) + + assert all(r["dec_mode"] == "acc" for r in results) + + def test_s2_steady_highway_following_never_blends(self): + v = 80.0 / 3.6 + plant = PlantSP(lead_relevancy=True, speed=v, distance_lead=40.0, e2e=True, only_radar=True, + model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs) + results, failures = _run(plant, steps=100, v_lead=v, v_cruise=v) + + assert failures <= 1 + assert all(r["dec_mode"] == "acc" for r in results) + + def test_s3_low_speed_cruise_no_lead_never_blends(self): + v = 15.0 / 3.6 + plant = PlantSP(lead_relevancy=False, speed=v, e2e=True, model_plan_fn=flat_plan()) + results, _ = _run(plant, steps=100, v_cruise=v) + + assert all(r["dec_mode"] == "acc" for r in results) + + def test_s4_highway_slowdown_without_lead_blends(self): + v0 = 110.0 / 3.6 + a = (70.0 / 3.6 - v0) / 6.0 + plant = PlantSP(lead_relevancy=False, speed=v0, e2e=True, model_plan_fn=decel_plan(a)) + results, _ = _run(plant, steps=10, v_cruise=v0) + + assert any(r["dec_mode"] == "blended" for r in results) + + def test_s5_stop_then_depart_with_lead_present_stays_acc_throughout(self): + def departing_lead(current_time): + return 0.0 if current_time < 1.0 else min(15.0, 3.0 * (current_time - 1.0)) + + plant = PlantSP(lead_relevancy=True, speed=0.0, distance_lead=6.0, e2e=True) + results = [] + solver_failures = 0 + original_reset = plant.planner.mpc.reset + + def counting_reset(*args, **kw): + nonlocal solver_failures + if plant.planner.mpc.solution_status != 0: + solver_failures += 1 + return original_reset(*args, **kw) + + plant.planner.mpc.reset = counting_reset + for _ in range(200): + results.append(plant.step(v_lead=departing_lead(plant.current_time), v_cruise=15.0)) + + assert solver_failures <= 1 + assert all(r["dec_mode"] == "acc" for r in results) + assert all(r["dec_lead_veto"] for r in results) + + def test_s6_creep_cycles_behind_lead_stay_acc(self): + def creep_cycle_lead(current_time): + return 1.5 + 1.5 * np.sin(current_time * 2.0) + + plant = PlantSP(lead_relevancy=True, speed=1.0, distance_lead=8.0, e2e=True, only_radar=True, + model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs) + results = [plant.step(v_lead=creep_cycle_lead(plant.current_time), v_cruise=5.0) for _ in range(200)] + + assert all(r["dec_mode"] == "acc" for r in results) + + def test_s7_oscillating_near_threshold_demand_does_not_flap(self): + plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=alternating_plan(-2.5)) + results, _ = _run(plant, steps=200, v_cruise=20.0) + + assert mode_changes(results) <= 2 + + def test_s8_degraded_model_holds_acc_through_a_slowdown(self): + def degraded_meta(_current_time): + return [0.0] * 5, False, 60.0 + + plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-3.0), model_meta_fn=degraded_meta) + results, _ = _run(plant, steps=30, v_cruise=20.0) + + assert all(r["dec_mode"] == "acc" for r in results) + + def test_s9_curve_exclusion_prevents_false_blend_on_a_bend(self): + plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-2.5), + position_y_fn=lambda _t: [6.0] * len(T_IDXS)) + results, _ = _run(plant, steps=30, v_cruise=20.0) + + assert all(r["dec_mode"] == "acc" for r in results) + + def test_s10_hard_brake_override_inert_while_lead_present(self): + def hard_brake_meta(_current_time): + return [0.0] * 5, True, 0.0 + + plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True, + model_plan_fn=flat_plan(), model_meta_fn=hard_brake_meta, lead_future_probs_fn=persistent_lead_probs) + results, _ = _run(plant, steps=10, v_lead=20.0, v_cruise=20.0) + + assert all(r["dec_mode"] == "acc" for r in results)