From df61e0da78de1f061d5c5e482fa4580f7c77f705 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Fri, 17 Jul 2026 14:32:22 -0700 Subject: [PATCH] Make longitudinal pacing responsive without sacrificing smoothness --- .../lib/longitudinal_mpc_lib/long_mpc.py | 67 +- .../controls/lib/longitudinal_planner.py | 7 +- .../lib/accel_personality/accel_controller.py | 950 ++++------ .../tests/test_accel_controller.py | 1652 ++++++----------- .../tests/test_accel_controller_interfaces.py | 40 +- .../controls/lib/longitudinal_planner.py | 23 +- .../test_accel_controller_closed_loop.py | 1195 +++++------- 7 files changed, 1442 insertions(+), 2492 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b2005aa788..d5dafe3f65 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -57,6 +57,8 @@ COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 CRUISE_MIN_ACCEL = -1.2 CRUISE_MAX_ACCEL = 1.6 +CUSTOM_ACCEL_TRANSITION_FRAMES = 3 +CUSTOM_ACCEL_TRANSITION_MAX_SPEED = 0.3 MIN_X_LEAD_FACTOR = 0.5 def get_jerk_factor(personality=log.LongitudinalPersonality.standard): @@ -243,8 +245,8 @@ class LongitudinalMpc: self.last_cloudlog_t = 0 self.status = False self.crash_cnt = 0.0 - self.lead_obstacle_weights = np.ones(2) self.solution_status = 0 + self.custom_accel_frames = 0 # timers self.solve_time = 0.0 self.time_qp_solution = 0.0 @@ -284,6 +286,24 @@ class LongitudinalMpc: for i in range(N+1): self.solver.set(i, 'x', self.x0) + def _seed_stock_transition(self): + previous_bound = np.clip(self.params[:, 1], 0.0, CRUISE_MAX_ACCEL) + a_guess = previous_bound + (CRUISE_MAX_ACCEL - previous_bound) * (1.0 - np.exp(-T_IDXS)) + a_guess[0] = self.x0[2] + v_guess = np.zeros(N + 1) + x_guess = np.zeros(N + 1) + v_guess[0] = max(self.x0[1], 0.0) + x_guess[0] = self.x0[0] + for i in range(1, N + 1): + dt = T_IDXS[i] - T_IDXS[i - 1] + v_guess[i] = max(0.0, v_guess[i - 1] + 0.5 * (a_guess[i - 1] + a_guess[i]) * dt) + x_guess[i] = x_guess[i - 1] + 0.5 * (v_guess[i - 1] + v_guess[i]) * dt + for i in range(N + 1): + self.solver.set(i, "x", np.array([x_guess[i], v_guess[i], a_guess[i]])) + for i in range(N): + dt = T_IDXS[i + 1] - T_IDXS[i] + self.solver.set(i, "u", np.array([(a_guess[i + 1] - a_guess[i]) / dt])) + @staticmethod def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) @@ -317,7 +337,7 @@ class LongitudinalMpc: def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, accel_max: float | tuple[float, ...] | np.ndarray | None = None, shape_accel_max_in_cruise: bool = False, - lead_obstacle_weights: tuple[float, float] | np.ndarray | None = None): + apply_accel_max_constraint: bool = True): t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -328,50 +348,36 @@ class LongitudinalMpc: # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - raw_lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - raw_lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) - custom_accel_max = False + custom_accel = False accel_max_traj = ACCEL_MAX * np.ones(N + 1) if accel_max is not None: accel_max_input = np.asarray(accel_max, dtype=float) if accel_max_input.ndim == 0: accel_max_input = np.full(N + 1, float(accel_max_input)) - custom_accel_max = accel_max_input.shape == (N + 1,) and np.all(np.isfinite(accel_max_input)) - if custom_accel_max: + custom_accel = accel_max_input.shape == (N + 1,) and np.all(np.isfinite(accel_max_input)) + if custom_accel: accel_max_traj = np.clip(accel_max_input, ACCEL_MIN, ACCEL_MAX) + custom_accel_active = custom_accel and (shape_accel_max_in_cruise or apply_accel_max_constraint) + if (not custom_accel_active and 0 < self.custom_accel_frames < CUSTOM_ACCEL_TRANSITION_FRAMES + and v_ego < CUSTOM_ACCEL_TRANSITION_MAX_SPEED and self.source == LongitudinalPlanSource.cruise): + self._seed_stock_transition() + # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) # TODO does this make sense when max_a is negative? - if custom_accel_max and shape_accel_max_in_cruise: - cruise_accel_max_traj = np.minimum(accel_max_traj, CRUISE_MAX_ACCEL) - v_upper = v_ego + (np.cumsum(T_DIFFS * cruise_accel_max_traj) * 1.05) + if custom_accel and shape_accel_max_in_cruise: + cruise_accel_traj = np.clip(accel_max_traj, 0.0, CRUISE_MAX_ACCEL) + v_upper = v_ego + (np.cumsum(T_DIFFS * cruise_accel_traj) * 1.05) else: v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) - # The acceleration controller may gradually introduce a benign newly - # acquired obstacle to avoid a one-frame optimizer/source discontinuity. - # Raw lead trajectories remain untouched for FCW below, and missing or - # invalid weights preserve stock behavior exactly. - self.lead_obstacle_weights = np.ones(2) - if lead_obstacle_weights is not None: - weight_input = np.asarray(lead_obstacle_weights, dtype=float) - if weight_input.shape == (2,) and np.all(np.isfinite(weight_input)): - self.lead_obstacle_weights = np.clip(weight_input, 0.0, 1.0) - if np.array_equal(self.lead_obstacle_weights, np.ones(2)): - # Preserve the original arrays bit-for-bit on every bypass. Even an - # algebraically equivalent subtract/add can perturb the one-iteration - # solver at a standstill. - lead_0_obstacle = raw_lead_0_obstacle - lead_1_obstacle = raw_lead_1_obstacle - else: - lead_0_obstacle = cruise_obstacle + self.lead_obstacle_weights[0] * (raw_lead_0_obstacle - cruise_obstacle) - lead_1_obstacle = cruise_obstacle + self.lead_obstacle_weights[1] * (raw_lead_1_obstacle - cruise_obstacle) - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = MPC_SOURCES[np.argmin(x_obstacles[0])] @@ -381,7 +387,7 @@ class LongitudinalMpc: self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) self.params[:,0] = ACCEL_MIN - if custom_accel_max: + if custom_accel and apply_accel_max_constraint: self.params[:,1] = accel_max_traj self.params[0,1] = max(accel_max_traj[0], self.x0[2]) else: @@ -392,6 +398,7 @@ class LongitudinalMpc: self.params[:,5] = LEAD_DANGER_FACTOR self.run() + self.custom_accel_frames = self.custom_accel_frames + 1 if custom_accel_active and self.last_solution_status == 0 else 0 if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and radarstate.leadOne.modelProb > 0.9): self.crash_cnt += 1 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4624c66510..28ea14dd04 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -145,18 +145,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP): if force_slow_decel: v_cruise = 0.0 - if self.accel_controller_result.reset_mpc: - # Urgent-entry MPC reset must not erase stock FCW evidence. - crash_cnt = self.mpc.crash_cnt - self.mpc.reset() - self.mpc.crash_cnt = crash_cnt self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.update( sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality, accel_max=self.accel_controller_result.mpc_accel_max, shape_accel_max_in_cruise=self.accel_controller_result.mpc_shape_cruise, - lead_obstacle_weights=self.accel_controller_result.lead_obstacle_weights, + apply_accel_max_constraint=self.accel_controller_result.mpc_apply_accel_constraint, ) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 69f47f97c5..9d29fcc6e5 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -7,7 +7,7 @@ import math import numpy as np from cereal import log -from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from opendbc.car.interfaces import ACCEL_MAX from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( LongitudinalMpc, @@ -39,79 +39,67 @@ class ProfileConfig: comfort_decel: float release_rate: float release_confirm: float + departure_rate: float PROFILE_CONFIGS = { - AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=0.90, release_confirm=0.50), - AccelProfile.normal: ProfileConfig(comfort_decel=0.335, release_rate=1.15, release_confirm=0.35), - AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_rate=1.45, release_confirm=0.20), + AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=1.55, release_confirm=0.50, departure_rate=12.0), + AccelProfile.normal: ProfileConfig(comfort_decel=0.32, release_rate=1.75, release_confirm=0.35, departure_rate=16.0), + AccelProfile.sport: ProfileConfig(comfort_decel=0.38, release_rate=2.00, release_confirm=0.20, departure_rate=20.0), } -ACCEL_PROFILE_MAX_BP = [0.0, 10.0, 25.0, 40.0] -# Pre-MPC profile requests; stock output limits remain authoritative. +# Keep launch responsive, then separate the profiles above walking speed. +ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0] ACCEL_PROFILE_MAX_V = { - AccelProfile.eco: [1.55, 0.85, 0.45, 0.25], - AccelProfile.normal: [1.65, 1.10, 0.70, 0.45], - AccelProfile.sport: [2.00, 1.70, 1.20, 0.90], + AccelProfile.eco: [1.55, 1.25, 0.85, 0.50, 0.30], + AccelProfile.normal: [1.70, 1.40, 1.05, 0.65, 0.45], + AccelProfile.sport: [2.00, 1.90, 1.70, 1.20, 0.90], } -LAUNCH_DELTA_V = 3.0 -LAUNCH_TARGET_HANDOFF_SPEED = 1.0 CAP_FILTER_FRAMES = 5 RESTRICT_DEADBAND = 0.15 +RESTRICT_EXIT_DEADBAND = 0.05 RELIEF_DEADBAND = 0.35 +PACE_ACQUISITION_MARGIN = 0.45 +EARLY_APPROACH_HEADWAY = 5.0 +EARLY_APPROACH_MIN_SPEED = 8.0 +CLOSING_SPEED_ENTER = 0.25 +CLOSING_SPEED_EXIT = 0.10 +MOVING_LEAD_DECEL_RATE = 0.15 +MOVING_LEAD_DECEL_CONFIRM_FRAMES = 3 +MOVING_LEAD_DECEL_ENTER_REQUIRED_DECEL = 0.15 +MOVING_LEAD_DECEL_ACCEL_MAX = -0.42 +MOVING_LEAD_DECEL_ACCEL_SLEW_RATE = 0.50 +MOVING_LEAD_DECEL_EXIT_REQUIRED_DECEL = 0.05 +MOVING_LEAD_DECEL_EXIT_FRAMES = 4 STOP_HOLD_EGO_SPEED = 0.30 STOP_HOLD_CAP = 0.50 +STOP_HOLD_ACCEL_MAX = 0.0 +STOP_HOLD_GUARD_ACCEL_MAX = -0.42 +STOP_HOLD_GUARD_EXTRA = 0.20 STOPPED_LEAD_SPEED = 0.30 -LEAD_DEPARTURE_SPEED = 0.30 STOP_HOLD_EXIT_FRAMES = 4 -LAUNCH_PROFILE_HANDOFF_SPEED = 0.05 +MAX_LEAD_ACCEL_TAU = 10.0 VEGO_NOISE_TOLERANCE = 0.10 -ACCEL_LIMIT_JERK = 1.0 -DECEL_LIMIT_JERK = 1.10 -LAUNCH_ACCEL_RATE = 5.0 -CLEAR_LAUNCH_ACCEL_RATE = 3.0 -INITIAL_LAUNCH_ACCEL_MAX = 0.95 -BREAKAWAY_ACCEL_MAX = 1.15 -HOLD_ACCEL_MAX = 0.10 -MATCHED_LEAD_PROFILE_ACCEL = -0.90 -MATCHED_LEAD_ACCEL_TAPER_SPEED = 1.25 -MATCHED_LEAD_ACCEL_GAIN = 0.80 -NO_PROPULSION_CLOSING_SPEED = 0.02 -MATERIAL_CLOSING_DECEL_ENTER = 0.08 -MATERIAL_CLOSING_DECEL_EXIT = 0.03 -MATERIAL_CLOSING_SPEED_ENTER = 0.25 -MATERIAL_CLOSING_SPEED_EXIT = 0.10 -MATERIAL_CLOSING_EXIT_FRAMES = 3 -LAUNCH_PACE_RATE = 5.0 -LEAD_ACQUISITION_INITIAL_AUTHORITY = 0.20 -LEAD_ACQUISITION_TIME = 0.30 -LEAD_ACQUISITION_MIN_DISTANCE = 60.0 -LEAD_ACQUISITION_MIN_HEADWAY = 3.0 -LEAD_ACQUISITION_MIN_TTC = 10.0 -LEAD_ACQUISITION_MAX_DECEL = 0.25 -LEAD_ACQUISITION_MIN_LEAD_ACCEL = -0.50 -LEAD_ACQUISITION_MIN_PLANNER_ACCEL = -0.10 -LEAD_DEPARTURE_INITIAL_AUTHORITY = 0.0 -LEAD_DEPARTURE_HANDOFF_TIME = 0.50 -RELATIVE_PACE_PREVIEW_TIME = 3.0 -URGENT_BYPASS_REQUIRED_DECEL = 0.45 -URGENT_BYPASS_MIN_SPEED = 5.0 -URGENT_RELEASE_REQUIRED_DECEL = 0.35 -URGENT_LOW_SPEED_LEAD_BYPASS = 5.0 -URGENT_REJOIN_ACCEL_MAX = -0.15 -URGENT_REJOIN_ACCEL_RATE = 5.0 +LEAD_SPEED_NOISE_TOLERANCE = 0.10 +ACCEL_LIMIT_TIGHTEN_RATE = 2.0 +ACCEL_LIMIT_RAISE_RATE = 1.0 +ACCEL_LIMIT_BYPASS_RATE = 2.0 +ACCEL_BOUND_HORIZON_RATE = 1.0 +STANDSTILL_MPC_ACCEL_SEED = 0.95 +STANDSTILL_ACCEL_LIMIT_RAISE_RATE = 2.0 +ACCEL_SHAPE_WARMUP_FRAMES = 3 @dataclass(frozen=True) class EnergyEnvelope: cap: float = math.inf selected_lead: int = -1 + selected_lead_speed: float = math.inf departure_lead_speed: float = math.inf usable_gap: float = math.inf closing_speed: float = 0.0 required_decel: float = 0.0 - conservative_required_decel: float = 0.0 has_nearly_stopped_lead: bool = False @@ -127,8 +115,7 @@ class AccelControllerResult: effective_accel_max: float mpc_accel_max: tuple[float, ...] | None mpc_shape_cruise: bool - reset_mpc: bool - lead_obstacle_weights: tuple[float, float] + mpc_apply_accel_constraint: bool state: AccelControllerState shadow_state: AccelControllerState base_speed: float @@ -151,20 +138,12 @@ class _PacePath: relief_time: float = 0.0 departure_frames: int = 0 departing_from_stop: bool = False - launch_target_active: bool = False - departure_handoff_active: bool = False - stop_departure_confirmed: bool = False - stopped_lead_hold: bool = False - accel_limit: float | None = None - decel_limit_active: bool = False - urgent_bypass_active: bool = False - urgent_recovery_active: bool = False - urgent_dropout_frames: int = 0 - material_closing: bool = False - material_relief_frames: int = 0 - lead_seen: list[bool] = field(default_factory=lambda: [False, False]) - lead_track_ids: list[int] = field(default_factory=lambda: [-1, -1]) - lead_obstacle_weights: list[float] = field(default_factory=lambda: [1.0, 1.0]) + filtered_lead_active: bool = False + previous_lead_speed: float | None = None + lead_decel_frames: int = 0 + moving_lead_decel: bool = False + moving_lead_relief_frames: int = 0 + moving_lead_accel_max: float | None = None def reset(self) -> None: self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES) @@ -173,32 +152,20 @@ class _PacePath: self.relief_time = 0.0 self.departure_frames = 0 self.departing_from_stop = False - self.launch_target_active = False - self.departure_handoff_active = False - self.stop_departure_confirmed = False - self.stopped_lead_hold = False - self.accel_limit = None - self.decel_limit_active = False - self.urgent_bypass_active = False - self.urgent_recovery_active = False - self.urgent_dropout_frames = 0 - self.material_closing = False - self.material_relief_frames = 0 - self.lead_seen = [False, False] - self.lead_track_ids = [-1, -1] - self.lead_obstacle_weights = [1.0, 1.0] + self.filtered_lead_active = False + self.previous_lead_speed = None + self.lead_decel_frames = 0 + self.moving_lead_decel = False + self.moving_lead_relief_frames = 0 + self.moving_lead_accel_max = None def update_filter(self, cap: float) -> float: self.cap_samples.append(cap) return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2] - @property - def filtered_cap(self) -> float: - return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2] - class AccelController: - """A relative-pace governor with a pre-MPC acceleration comfort ceiling.""" + """Pre-MPC acceleration profile and relative-pace cruise governor.""" def __init__(self, CP, dt: float = DT_MDL): if not math.isfinite(dt) or dt <= 0.0: @@ -208,6 +175,29 @@ class AccelController: self.dt = dt self.live = _PacePath() self.shadow = _PacePath() + self._reset_accel_limit() + self._clear_recovery_state() + + def _reset_accel_limit(self, value: float | None = None) -> None: + self.live_accel_max = value + self.live_accel_bypass = False + self.live_accel_rejoin = False + self.live_accel_warmup_frames = 0 + + def _clear_recovery_state(self) -> None: + self.recovery_available = False + self.recovery_target_speed = math.inf + self.recovery_accel_max: tuple[float, ...] | None = None + self.recovery_shape_cruise = False + self.recovery_apply_accel_constraint = False + + def _cache_recovery_state(self, target_speed: float, accel_max: tuple[float, ...] | None, + shape_cruise: bool, apply_accel_constraint: bool) -> None: + self.recovery_available = True + self.recovery_target_speed = target_speed + self.recovery_accel_max = accel_max + self.recovery_shape_cruise = shape_cruise + self.recovery_apply_accel_constraint = apply_accel_constraint @staticmethod def _profile(profile: int | AccelProfile) -> AccelProfile: @@ -218,12 +208,11 @@ class AccelController: @classmethod def get_profile_accel_max(cls, profile: int | AccelProfile, v_ego: float) -> float: - """Return the profile's positive-acceleration ceiling at the current speed.""" if not math.isfinite(v_ego): return math.nan - profile = cls._profile(profile) - return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile])) + selected_profile = cls._profile(profile) + return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[selected_profile])) def _delay(self) -> float: try: @@ -236,20 +225,32 @@ class AccelController: if a_ego < 0.0: stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0 if stop_time <= delay: - return -v_ego * v_ego / (2.0 * a_ego) if v_ego > 0.0 else 0.0, 0.0 + distance = -v_ego * v_ego / (2.0 * a_ego) if v_ego > 0.0 else 0.0 + return distance, 0.0 - return max(v_ego * delay + 0.5 * a_ego * delay * delay, 0.0), max(v_ego + a_ego * delay, 0.0) + distance = v_ego * delay + 0.5 * a_ego * delay * delay + return max(distance, 0.0), max(v_ego + a_ego * delay, 0.0) @staticmethod def _valid_lead(lead) -> bool: - return bool(lead.status) and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau)) + try: + d_rel, v_lead, a_lead, a_lead_tau = (float(lead.dRel), float(lead.vLeadK), float(lead.aLeadK), float(lead.aLeadTau)) + status = bool(lead.status) + except (AttributeError, TypeError, ValueError, OverflowError): + return False - def calculate_energy_envelope( - self, radar_state, v_ego: float, a_ego: float, profile: int | AccelProfile, follow_personality=log.LongitudinalPersonality.standard - ) -> EnergyEnvelope: - """Calculate the unfiltered relative-energy speed cap without mutating radar state.""" - profile = self._profile(profile) - config = PROFILE_CONFIGS[profile] + return ( + status + and all(math.isfinite(value) for value in (d_rel, v_lead, a_lead, a_lead_tau)) + and d_rel >= 0.0 + and v_lead >= -LEAD_SPEED_NOISE_TOLERANCE + and 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU + ) + + def calculate_energy_envelope(self, radar_state, v_ego: float, a_ego: float, profile: int | AccelProfile, + follow_personality=log.LongitudinalPersonality.standard) -> EnergyEnvelope: + selected_profile = self._profile(profile) + config = PROFILE_CONFIGS[selected_profile] delay = self._delay() if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0: return EnergyEnvelope() @@ -260,54 +261,41 @@ class AccelController: t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard) x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay) - candidates: list[EnergyEnvelope] = [] - departure_candidates: list[tuple[float, float]] = [] - + candidates = [] + departure_candidates = [] for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)): if not self._valid_lead(lead): continue - x_lead = float(lead.dRel) - v_lead = float(lead.vLeadK) - a_lead = np.clip(float(lead.aLeadK), -10.0, 5.0) - a_lead_tau = float(lead.aLeadTau) - lead_xv = LongitudinalMpc.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) - x_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 0])) - v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1])) - departure_candidates.append((x_lead_delay + float(get_stopped_equivalence_factor(v_lead_delay)), v_lead_delay)) + try: + lead_xv = LongitudinalMpc.extrapolate_lead( + float(lead.dRel), + float(lead.vLeadK), + float(np.clip(lead.aLeadK, -10.0, 5.0)), + float(lead.aLeadTau), + ) + x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0])) + v_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 1])) + if not all(math.isfinite(value) and value >= 0.0 for value in (x_lead, v_lead)): + continue - match_gap = STOP_DISTANCE + t_follow * v_lead_delay - usable_gap = max(x_lead_delay - x_ego - match_gap, 0.0) - closing_speed = max(v_ego_delay - v_lead_delay, 0.0) - if closing_speed == 0.0: - required_decel = 0.0 - elif usable_gap == 0.0: - required_decel = math.inf - else: - required_decel = closing_speed * closing_speed / (2.0 * usable_gap) + match_gap = STOP_DISTANCE + t_follow * v_lead + usable_gap = max(x_lead - x_ego - match_gap, 0.0) + closing_speed = max(v_ego_delay - v_lead, 0.0) + required_decel = 0.0 if closing_speed == 0.0 else math.inf if usable_gap == 0.0 else closing_speed**2 / (2.0 * usable_gap) + cap = v_lead + math.sqrt(2.0 * config.comfort_decel * usable_gap) + departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead)) + except (FloatingPointError, OverflowError, TypeError, ValueError): + continue - # Positive aLeadK spikes can hide urgent acquisition. The zero-accel projection is urgency-only; raw lead data remains unchanged. - conservative_required_decel = required_decel - if a_lead > 0.0: - conservative_lead_xv = LongitudinalMpc.extrapolate_lead(x_lead, v_lead, 0.0, a_lead_tau) - conservative_x_lead = float(np.interp(delay, T_IDXS, conservative_lead_xv[:, 0])) - conservative_v_lead = float(np.interp(delay, T_IDXS, conservative_lead_xv[:, 1])) - conservative_gap = max(conservative_x_lead - x_ego - STOP_DISTANCE - t_follow * conservative_v_lead, 0.0) - conservative_closing = max(v_ego_delay - conservative_v_lead, 0.0) - if conservative_closing == 0.0: - conservative_required_decel = 0.0 - elif conservative_gap == 0.0: - conservative_required_decel = math.inf - else: - conservative_required_decel = conservative_closing * conservative_closing / (2.0 * conservative_gap) - conservative_required_decel = max(required_decel, conservative_required_decel) + if not all(math.isfinite(value) for value in (usable_gap, closing_speed, cap, departure_distance)) or math.isnan(required_decel) or required_decel < 0.0: + continue - anticipated_gap = max(usable_gap - closing_speed * RELATIVE_PACE_PREVIEW_TIME, 0.0) - cap = v_lead_delay + math.sqrt(2.0 * config.comfort_decel * anticipated_gap) candidates.append(EnergyEnvelope( - cap=cap, selected_lead=lead_index, usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, - conservative_required_decel=conservative_required_decel, + cap=cap, selected_lead=lead_index, selected_lead_speed=v_lead, departure_lead_speed=v_lead, + usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, )) + departure_candidates.append((departure_distance, v_lead)) if not candidates: return EnergyEnvelope() @@ -315,355 +303,157 @@ class AccelController: selected = min(candidates, key=lambda candidate: candidate.cap) departure_lead_speed = min(departure_candidates, key=lambda candidate: candidate[0])[1] return EnergyEnvelope( - cap=selected.cap, selected_lead=selected.selected_lead, departure_lead_speed=departure_lead_speed, usable_gap=selected.usable_gap, - closing_speed=selected.closing_speed, required_decel=selected.required_decel, - conservative_required_decel=selected.conservative_required_decel, - has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED, + cap=selected.cap, selected_lead=selected.selected_lead, selected_lead_speed=selected.selected_lead_speed, + departure_lead_speed=departure_lead_speed, usable_gap=selected.usable_gap, closing_speed=selected.closing_speed, + required_decel=selected.required_decel, has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED, ) - def _lead_acquisition_is_benign(self, lead, v_ego: float, a_ego: float, planner_accel: float, follow_personality) -> bool: - """Return whether a new lead can enter the optimizer gradually without delaying needed braking.""" - if not self._valid_lead(lead) or planner_accel <= LEAD_ACQUISITION_MIN_PLANNER_ACCEL: - return False - - try: - t_follow = get_T_FOLLOW(follow_personality) - except (NotImplementedError, TypeError, ValueError): - t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard) - - delay = self._delay() - x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay) - lead_xv = LongitudinalMpc.extrapolate_lead( - float(lead.dRel), float(lead.vLeadK), float(np.clip(lead.aLeadK, -10.0, 5.0)), float(lead.aLeadTau), - ) - x_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 0])) - v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1])) - separation = max(x_lead_delay - x_ego, 0.0) - closing_speed = max(v_ego_delay - v_lead_delay, 0.0) - ttc = separation / closing_speed if closing_speed > 0.0 else math.inf - usable_gap = max(separation - STOP_DISTANCE - t_follow * v_lead_delay, 0.0) - if closing_speed == 0.0: - required_decel = 0.0 - elif usable_gap == 0.0: - required_decel = math.inf - else: - required_decel = closing_speed * closing_speed / (2.0 * usable_gap) - - return ( - separation > max(LEAD_ACQUISITION_MIN_DISTANCE, LEAD_ACQUISITION_MIN_HEADWAY * v_ego_delay) - and ttc > LEAD_ACQUISITION_MIN_TTC - and required_decel < LEAD_ACQUISITION_MAX_DECEL - and float(lead.aLeadK) > LEAD_ACQUISITION_MIN_LEAD_ACCEL - ) - - def _update_lead_obstacle_weights( - self, path: _PacePath, radar_state, v_ego: float, a_ego: float, planner_accel: float, follow_personality, *, allow_blend: bool, - ) -> tuple[float, float]: - """Ramp benign new obstacles into MPC while making every urgent lead immediate.""" - authority_step = (1.0 - LEAD_ACQUISITION_INITIAL_AUTHORITY) * self.dt / LEAD_ACQUISITION_TIME - departure_step = (1.0 - LEAD_DEPARTURE_INITIAL_AUTHORITY) * self.dt / LEAD_DEPARTURE_HANDOFF_TIME - for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)): - if not self._valid_lead(lead): - path.lead_seen[lead_index] = False - path.lead_track_ids[lead_index] = -1 - path.lead_obstacle_weights[lead_index] = 1.0 - continue - - track_id = int(getattr(lead, "radarTrackId", -1)) - previous_track_id = path.lead_track_ids[lead_index] - positive_track_change = track_id >= 0 and previous_track_id >= 0 and track_id != previous_track_id - new_lead = not path.lead_seen[lead_index] or positive_track_change - benign = self._lead_acquisition_is_benign(lead, v_ego, a_ego, planner_accel, follow_personality) - safe_departure_handoff = path.departure_handoff_active and float(lead.vLeadK) > LEAD_DEPARTURE_SPEED and v_ego <= float(lead.vLeadK) - - if path.departure_handoff_active: - if safe_departure_handoff: - # Clear standstill deadband, then ramp raw-lead authority. - previous_weight = path.lead_obstacle_weights[lead_index] - if previous_weight <= LEAD_DEPARTURE_INITIAL_AUTHORITY and v_ego < LAUNCH_PROFILE_HANDOFF_SPEED: - path.lead_obstacle_weights[lead_index] = LEAD_DEPARTURE_INITIAL_AUTHORITY - else: - path.lead_obstacle_weights[lead_index] = min(1.0, max(previous_weight, LEAD_DEPARTURE_INITIAL_AUTHORITY) + departure_step) - else: - path.lead_obstacle_weights[lead_index] = 1.0 - elif new_lead: - path.lead_obstacle_weights[lead_index] = LEAD_ACQUISITION_INITIAL_AUTHORITY if allow_blend and benign else 1.0 - elif path.lead_obstacle_weights[lead_index] < 1.0: - if benign: - path.lead_obstacle_weights[lead_index] = min(1.0, path.lead_obstacle_weights[lead_index] + authority_step) - else: - path.lead_obstacle_weights[lead_index] = 1.0 - - path.lead_seen[lead_index] = True - path.lead_track_ids[lead_index] = track_id - - if path.state == AccelControllerState.stopHold: - try: - v_ego_stopping = float(self.CP.vEgoStopping) - except (AttributeError, TypeError, ValueError): - v_ego_stopping = STOP_HOLD_EGO_SPEED - if not math.isfinite(v_ego_stopping) or v_ego_stopping < 0.0: - v_ego_stopping = STOP_HOLD_EGO_SPEED - - # Use zero-speed cruise below stock's shouldStop threshold; retain raw authority above it for close stopped leads. - hold_weight = 0.0 if v_ego < v_ego_stopping else 1.0 - path.lead_obstacle_weights = [hold_weight, hold_weight] - elif path.departure_handoff_active and not path.departing_from_stop and all( - not seen or weight >= 1.0 for seen, weight in zip(path.lead_seen, path.lead_obstacle_weights, strict=True) - ): - path.departure_handoff_active = False - - return tuple(path.lead_obstacle_weights) - - def reset(self) -> None: - self.live.reset() - self.shadow.reset() - @staticmethod def _lead_source(source) -> bool: return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1) - def _update_path( - self, path: _PacePath, envelope: EnergyEnvelope, base_speed: float, v_ego: float, config: ProfileConfig, previous_mpc_source, - planner_speed: float, previous_should_stop: bool, selected_lead_speed: float, - ) -> float: + def _update_moving_lead_decel(self, path: _PacePath, envelope: EnergyEnvelope, base_speed: float, v_ego: float) -> None: + lead_speed = envelope.selected_lead_speed + speed_falling = ( + path.previous_lead_speed is not None + and math.isfinite(path.previous_lead_speed) + and math.isfinite(lead_speed) + and lead_speed < path.previous_lead_speed - MOVING_LEAD_DECEL_RATE * self.dt + ) + path.lead_decel_frames = path.lead_decel_frames + 1 if speed_falling else 0 + if ( + not path.moving_lead_decel + and path.lead_decel_frames >= MOVING_LEAD_DECEL_CONFIRM_FRAMES + and path.filtered_lead_active + and envelope.cap < base_speed - RESTRICT_DEADBAND + and envelope.required_decel >= MOVING_LEAD_DECEL_ENTER_REQUIRED_DECEL + and v_ego >= EARLY_APPROACH_MIN_SPEED + and envelope.closing_speed > CLOSING_SPEED_ENTER + ): + path.moving_lead_decel = True + path.moving_lead_accel_max = None + + matched_moving_lead = ( + path.moving_lead_decel + and lead_speed > STOPPED_LEAD_SPEED + and envelope.closing_speed <= CLOSING_SPEED_EXIT + and envelope.required_decel <= MOVING_LEAD_DECEL_EXIT_REQUIRED_DECEL + ) + path.moving_lead_relief_frames = path.moving_lead_relief_frames + 1 if matched_moving_lead else 0 + if path.moving_lead_relief_frames >= MOVING_LEAD_DECEL_EXIT_FRAMES or envelope.selected_lead < 0 or path.state == AccelControllerState.stopHold: + path.moving_lead_decel = False + path.moving_lead_relief_frames = 0 + path.moving_lead_accel_max = None + + path.previous_lead_speed = lead_speed if math.isfinite(lead_speed) else None + + def _update_path(self, path: _PacePath, envelope: EnergyEnvelope, base_speed: float, v_ego: float, config: ProfileConfig, + previous_mpc_source, planner_speed: float, previous_should_stop: bool) -> float: raw_cap = envelope.cap - closing_speed = envelope.closing_speed - required_decel = envelope.required_decel filtered_cap = path.update_filter(raw_cap) + filtered_lead_active = math.isfinite(filtered_cap) + acquired_restrictive_lead = filtered_lead_active and not path.filtered_lead_active and envelope.closing_speed > 0.0 + path.filtered_lead_active = filtered_lead_active - # Ignore near-zero track-switch chatter without delaying material closing. - if math.isfinite(raw_cap): - restrictive_evidence = required_decel >= MATERIAL_CLOSING_DECEL_ENTER or closing_speed >= MATERIAL_CLOSING_SPEED_ENTER - relief_evidence = required_decel <= MATERIAL_CLOSING_DECEL_EXIT and closing_speed <= MATERIAL_CLOSING_SPEED_EXIT - if restrictive_evidence: - path.material_closing = True - path.material_relief_frames = 0 - elif path.material_closing and relief_evidence: - path.material_relief_frames += 1 - if path.material_relief_frames >= MATERIAL_CLOSING_EXIT_FRAMES: - path.material_closing = False - path.material_relief_frames = 0 - else: - path.material_relief_frames = 0 - elif not math.isfinite(filtered_cap): - path.material_closing = False - path.material_relief_frames = 0 - - just_initialized = path.pace is None - if just_initialized: - # Clear road seeds base cruise; a present lead seeds ego pace. - path.pace = base_speed if not math.isfinite(raw_cap) else min(base_speed, v_ego) + if path.pace is None: + path.pace = base_speed path.state = AccelControllerState.free - if just_initialized and v_ego < STOP_HOLD_EGO_SPEED and not math.isfinite(raw_cap) and not previous_should_stop: - path.pace = base_speed - path.state = AccelControllerState.release - path.relief_time = 0.0 - path.departing_from_stop = True - path.launch_target_active = True - return filtered_cap + early_approach = envelope.usable_gap >= EARLY_APPROACH_HEADWAY * max(v_ego, 1.0) + if acquired_restrictive_lead and filtered_cap < path.pace - RESTRICT_DEADBAND: + if not early_approach or v_ego >= EARLY_APPROACH_MIN_SPEED: + acquisition_margin = PACE_ACQUISITION_MARGIN if early_approach else 0.0 + acquisition_pace = max(envelope.selected_lead_speed, v_ego - acquisition_margin) + path.pace = min(path.pace, acquisition_pace) - # A lower non-controller target is authoritative, and is also the correct seed if it later clears. path.pace = min(path.pace, base_speed) if self._lead_source(previous_mpc_source) and not math.isfinite(raw_cap) and planner_speed < path.pace: path.pace = max(planner_speed, 0.0) - if v_ego < STOP_HOLD_EGO_SPEED and (filtered_cap < STOP_HOLD_CAP or envelope.has_nearly_stopped_lead): - path.stopped_lead_hold = True + if path.state == AccelControllerState.stopHold: + moving_lead = envelope.departure_lead_speed > STOPPED_LEAD_SPEED and math.isfinite(raw_cap) + confirmed_lead_loss = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap) + path.departure_frames = path.departure_frames + 1 if moving_lead or confirmed_lead_loss else 0 + if path.departure_frames < STOP_HOLD_EXIT_FRAMES: + path.pace = 0.0 + return filtered_cap - clear_road_launch_complete = path.departing_from_stop and not path.stopped_lead_hold and v_ego >= LAUNCH_PROFILE_HANDOFF_SPEED - if v_ego >= STOP_HOLD_EGO_SPEED or clear_road_launch_complete: - path.departing_from_stop = False - path.stopped_lead_hold = False - if v_ego >= STOP_HOLD_EGO_SPEED: - path.stop_departure_confirmed = False + departure_ceiling = min(base_speed, filtered_cap) if math.isfinite(filtered_cap) else base_speed + path.pace = max(departure_ceiling, 0.0) + path.state = AccelControllerState.release + path.relief_time = 0.0 + path.departure_frames = 0 + path.departing_from_stop = True + return filtered_cap - if path.launch_target_active: - if v_ego >= LAUNCH_TARGET_HANDOFF_SPEED: - # Preserve launch preview across the base-cruise handoff. - path.pace = min(base_speed, max(path.pace, v_ego + LAUNCH_DELTA_V)) - path.launch_target_active = False - elif envelope.has_nearly_stopped_lead or closing_speed > MATERIAL_CLOSING_SPEED_EXIT or required_decel >= MATERIAL_CLOSING_DECEL_ENTER: - path.launch_target_active = False - - renewed_stop_evidence = filtered_cap < STOP_HOLD_CAP or envelope.has_nearly_stopped_lead - stale_plan_stop = previous_should_stop and not path.departing_from_stop and not path.stop_departure_confirmed - enter_stop_hold = v_ego < STOP_HOLD_EGO_SPEED and (renewed_stop_evidence or stale_plan_stop) - if enter_stop_hold and path.state != AccelControllerState.stopHold: + stopped_lead_is_close = envelope.has_nearly_stopped_lead and raw_cap < STOP_HOLD_CAP + stop_evidence = filtered_cap < STOP_HOLD_CAP or stopped_lead_is_close + clearly_moving_lead = math.isfinite(raw_cap) and envelope.departure_lead_speed > STOPPED_LEAD_SPEED + stale_plan_stop = previous_should_stop and not path.departing_from_stop and not clearly_moving_lead + if v_ego < STOP_HOLD_EGO_SPEED and (stop_evidence or stale_plan_stop): path.pace = 0.0 path.state = AccelControllerState.stopHold path.relief_time = 0.0 path.departure_frames = 0 path.departing_from_stop = False - path.launch_target_active = False - path.departure_handoff_active = False - path.stop_departure_confirmed = False return filtered_cap - if path.state == AccelControllerState.stopHold: - # Departure uses projected lead motion, independent of profile. Lead loss waits for the median's three-observation guard. - raw_departure = math.isfinite(envelope.departure_lead_speed) and envelope.departure_lead_speed > LEAD_DEPARTURE_SPEED - guarded_lead_loss = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap) - if raw_departure or guarded_lead_loss: - path.departure_frames += 1 - else: - path.departure_frames = 0 + if path.departing_from_stop and v_ego >= STOP_HOLD_EGO_SPEED: + path.departing_from_stop = False - if path.departure_frames < STOP_HOLD_EXIT_FRAMES: - path.pace = 0.0 - return filtered_cap - - path.state = AccelControllerState.release - path.relief_time = config.release_confirm - path.departure_frames = 0 - path.departing_from_stop = True - path.launch_target_active = True - path.departure_handoff_active = True - path.stop_departure_confirmed = True - path.stopped_lead_hold = False - path.pace = min(base_speed, filtered_cap, v_ego + LAUNCH_DELTA_V) + transient_relief = math.isfinite(filtered_cap) and (not math.isfinite(raw_cap) or raw_cap > filtered_cap + RELIEF_DEADBAND) + if transient_relief and not path.departing_from_stop: + path.state = AccelControllerState.hold + path.relief_time = 0.0 return filtered_cap - ceiling = min(base_speed, filtered_cap) - matched_moving_lead = (math.isfinite(selected_lead_speed) and closing_speed <= MATERIAL_CLOSING_SPEED_EXIT - and v_ego <= selected_lead_speed + MATERIAL_CLOSING_SPEED_EXIT) - if matched_moving_lead: - # Cap matched traffic at lead speed; upward changes still use the release ramp. - matched_ceiling = min(base_speed, max(selected_lead_speed, 0.0)) - path.pace = min(path.pace, matched_ceiling) - ceiling = min(ceiling, matched_ceiling) - if math.isfinite(raw_cap) and path.material_closing: - ceiling = min(ceiling, path.pace) - if ceiling <= path.pace - RESTRICT_DEADBAND: + ceiling = base_speed if path.departing_from_stop else min(base_speed, filtered_cap) + restrict_deadband = RESTRICT_EXIT_DEADBAND if path.state == AccelControllerState.restrict else RESTRICT_DEADBAND + cap_restriction = ceiling <= path.pace - restrict_deadband + closing_threshold = CLOSING_SPEED_EXIT if path.state == AccelControllerState.restrict else CLOSING_SPEED_ENTER + closing_guard = math.isfinite(raw_cap) and ceiling < base_speed - RESTRICT_DEADBAND and envelope.closing_speed > closing_threshold + if cap_restriction: path.pace = max(ceiling, path.pace - config.comfort_decel * self.dt) path.state = AccelControllerState.restrict path.relief_time = 0.0 - path.departing_from_stop = False - path.launch_target_active = False - path.departure_handoff_active = False + return filtered_cap + + if closing_guard: + path.state = AccelControllerState.hold + path.relief_time = 0.0 return filtered_cap relief = ceiling - path.pace - confirmed_clear = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap) - relief_deadband = RESTRICT_DEADBAND if confirmed_clear else RELIEF_DEADBAND - release_allowed = path.state == AccelControllerState.release and relief > RESTRICT_DEADBAND - if relief >= relief_deadband and not release_allowed: - path.relief_time += self.dt - path.state = AccelControllerState.hold - release_allowed = path.relief_time >= config.release_confirm + release_active = path.state == AccelControllerState.release and relief > RESTRICT_DEADBAND + waiting_for_release = False + if relief > RELIEF_DEADBAND and not release_active: + if path.state != AccelControllerState.release: + path.relief_time += self.dt + path.state = AccelControllerState.hold + if path.relief_time >= config.release_confirm: + path.state = AccelControllerState.release + release_active = path.state == AccelControllerState.release + waiting_for_release = not release_active - if release_allowed: - pace_rate = LAUNCH_PACE_RATE if path.departing_from_stop else config.release_rate - path.pace = min(ceiling, path.pace + pace_rate * self.dt) - path.state = AccelControllerState.release - elif relief <= relief_deadband: + if release_active: + release_rate = config.departure_rate if path.departing_from_stop else config.release_rate + path.pace = min(ceiling, path.pace + release_rate * self.dt) + elif waiting_for_release: + pass + elif not math.isfinite(raw_cap) and not math.isfinite(filtered_cap) and 0.0 < relief <= RELIEF_DEADBAND: + path.pace = ceiling + path.state = AccelControllerState.free path.relief_time = 0.0 - if confirmed_clear: - # Close the final clear-road deadband so HOLD cannot persist without a lead. - path.pace = ceiling - path.state = AccelControllerState.free - else: - path.state = AccelControllerState.free if path.pace >= base_speed else AccelControllerState.hold + else: + path.relief_time = 0.0 + path.state = AccelControllerState.free if path.pace >= ceiling - RESTRICT_DEADBAND else AccelControllerState.hold return filtered_cap - def _update_accel_limit( - self, path: _PacePath, envelope: EnergyEnvelope, stock_accel_max: float, planner_accel: float, profile_accel_max: float, - config: ProfileConfig, v_ego: float, selected_lead_speed: float, - ) -> tuple[float, float]: - """Return telemetry effective max and the controller's pre-MPC upper bound.""" - profile_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX)) - lead_present = envelope.selected_lead >= 0 - - if path.state == AccelControllerState.stopHold: - # Pin the full MPC horizon at standstill; departure logic reopens it. - path.accel_limit = 0.0 - path.decel_limit_active = False - path.urgent_recovery_active = False - return min(stock_accel_max, 0.0), 0.0 - - if path.departing_from_stop: - path.decel_limit_active = False - path.urgent_recovery_active = False - planner_seed = max(0.0, planner_accel) - if path.departure_handoff_active: - # A common bounded breakaway ramp starts motion; each profile may then continue toward its table ceiling. - launch_target = min(ACCEL_MAX, max(BREAKAWAY_ACCEL_MAX, profile_limit, planner_seed)) - previous_limit = path.accel_limit if path.accel_limit is not None else 0.0 - path.accel_limit = min(launch_target, previous_limit + LAUNCH_ACCEL_RATE * self.dt) - else: - # Seed below the standstill solver edge; the profile table takes over after the first few centimeters. - if path.accel_limit is None: - path.accel_limit = min(INITIAL_LAUNCH_ACCEL_MAX, BREAKAWAY_ACCEL_MAX) - else: - path.accel_limit = min(BREAKAWAY_ACCEL_MAX, path.accel_limit + CLEAR_LAUNCH_ACCEL_RATE * self.dt) - return min(stock_accel_max, path.accel_limit), path.accel_limit - - if path.urgent_recovery_active: - # Exit on current matched-lead evidence or confirmed relief; missing leads keep the no-gas guard until then. - if (lead_present and envelope.closing_speed <= 0.10) or path.state in (AccelControllerState.free, AccelControllerState.release): - path.urgent_recovery_active = False - else: - previous_limit = path.accel_limit if path.accel_limit is not None else ACCEL_MAX - rejoin_target = URGENT_REJOIN_ACCEL_MAX if path.state == AccelControllerState.restrict else HOLD_ACCEL_MAX - max_step = URGENT_REJOIN_ACCEL_RATE * self.dt - path.accel_limit = float(np.clip(rejoin_target, previous_limit - max_step, previous_limit + max_step)) - path.decel_limit_active = path.state == AccelControllerState.restrict - return min(stock_accel_max, path.accel_limit), path.accel_limit - - if path.state == AccelControllerState.restrict and (not lead_present or path.material_closing): - # Keep a negative horizon while materially closing or through missing-lead restriction; matched traffic recovers the ceiling. - requested_limit = -config.comfort_decel - if not path.decel_limit_active: - # Retain an existing horizon on restriction entry; reseeding from scalar planner accel can cause a one-frame drop. - if path.accel_limit is None: - path.accel_limit = max(0.0, planner_accel) - path.decel_limit_active = True - elif lead_present and path.material_closing: - requested_limit = HOLD_ACCEL_MAX - path.decel_limit_active = False - elif lead_present and envelope.closing_speed > NO_PROPULSION_CLOSING_SPEED: - # Tiny closing uses the +0.10 hold ceiling instead of material-closing deceleration. - requested_limit = HOLD_ACCEL_MAX - path.decel_limit_active = False - elif lead_present and math.isfinite(selected_lead_speed) and selected_lead_speed - v_ego <= MATCHED_LEAD_ACCEL_TAPER_SPEED: - # Taper propulsion before matching lead speed to absorb acceleration already in the planner and actuator. - speed_error = max(selected_lead_speed - v_ego, 0.0) - requested_limit = min(profile_limit, max(HOLD_ACCEL_MAX, MATCHED_LEAD_ACCEL_GAIN * speed_error)) - path.decel_limit_active = False - elif path.state in (AccelControllerState.restrict, AccelControllerState.hold): - # A clearly faster lead may recover the profile ceiling; otherwise hold near zero through relief confirmation. - requested_limit = profile_limit if lead_present and planner_accel >= MATCHED_LEAD_PROFILE_ACCEL else HOLD_ACCEL_MAX - path.decel_limit_active = False - else: - requested_limit = profile_limit - path.decel_limit_active = False - - if path.accel_limit is None: - # Seed from the current positive command; global and stock output limits still apply. - path.accel_limit = min(ACCEL_MAX, max(requested_limit, max(0.0, planner_accel))) - else: - transition_jerk = DECEL_LIMIT_JERK if path.decel_limit_active or path.accel_limit < 0.0 else ACCEL_LIMIT_JERK - max_step = transition_jerk * self.dt - path.accel_limit = float(np.clip(requested_limit, path.accel_limit - max_step, path.accel_limit + max_step)) - - effective_limit = min(stock_accel_max, path.accel_limit) - return effective_limit, path.accel_limit - - def _build_mpc_accel_max(self, accel_limit: float | None) -> tuple[float, ...] | None: - """Build the controller's pre-MPC acceleration upper-bound trajectory.""" - if accel_limit is None or not math.isfinite(accel_limit): - return None - - bounded_limit = float(np.clip(accel_limit, ACCEL_MIN, ACCEL_MAX)) - return tuple(bounded_limit for _ in T_IDXS) - @staticmethod - def _valid_context( - base_speed: float, v_ego: float, a_ego: float, planner_speed: float, stock_accel_max: float, planner_accel: float, delay: float, - engaged: bool, cruise_initialized: bool, controller_fault: bool, - ) -> bool: + def _valid_context(base_speed: float, v_ego: float, a_ego: float, planner_speed: float, stock_accel_max: float, + planner_accel: float, delay: float, engaged: bool, cruise_initialized: bool, controller_fault: bool) -> bool: + values = (base_speed, v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay) return ( engaged and cruise_initialized @@ -672,184 +462,148 @@ class AccelController: and v_ego >= -VEGO_NOISE_TOLERANCE and planner_speed >= 0.0 and delay >= 0.0 - and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay)) + and all(math.isfinite(value) for value in values) ) - def update( - self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, profile: int | AccelProfile, follow_personality, enabled: bool, - acc_selected: bool, engaged: bool, cruise_initialized: bool, previous_mpc_source, planner_speed: float, stock_accel_max: float, - planner_accel: float, previous_should_stop: bool, controller_fault: bool = False, - ) -> AccelControllerResult: - """Update live and shadow acceleration controllers and return the target and additive telemetry.""" - profile = self._profile(profile) - # Clamp Toyota standstill wheel-speed noise without accepting materially negative speed. + def _build_mpc_accel_max(self, profile_accel_max: float, stock_accel_max: float, planner_accel: float, v_ego: float, + lead_present: bool, closing_speed: float, stop_hold: bool) -> tuple[float, tuple[float, ...] | None, bool, bool]: + def build_trajectory(limit: float) -> tuple[float, ...]: + initial_limit = max(limit, min(planner_accel, ACCEL_MAX)) + return tuple(max(limit, initial_limit - ACCEL_BOUND_HORIZON_RATE * t) for t in T_IDXS) + + requested_accel_max = min(profile_accel_max, stock_accel_max) + if stop_hold: + self._reset_accel_limit() + guard_time = self._delay() + STOP_HOLD_GUARD_EXTRA + hold_accel_max = tuple(STOP_HOLD_GUARD_ACCEL_MAX if t <= guard_time else ACCEL_MAX for t in T_IDXS) + return min(requested_accel_max, STOP_HOLD_ACCEL_MAX), hold_accel_max, True, True + + if self.live.departing_from_stop: + self._reset_accel_limit(requested_accel_max) + return requested_accel_max, None, False, False + + if self.live.moving_lead_decel and lead_present: + if self.live.moving_lead_accel_max is None: + self.live.moving_lead_accel_max = max(min(planner_accel, 0.0), MOVING_LEAD_DECEL_ACCEL_MAX) + self.live.moving_lead_accel_max = max( + MOVING_LEAD_DECEL_ACCEL_MAX, + self.live.moving_lead_accel_max - MOVING_LEAD_DECEL_ACCEL_SLEW_RATE * self.dt, + ) + moving_lead_accel_max = min(stock_accel_max, self.live.moving_lead_accel_max) + return moving_lead_accel_max, build_trajectory(moving_lead_accel_max), True, True + + if requested_accel_max <= 0.0: + self._reset_accel_limit() + return requested_accel_max, None, False, False + + requested_accel_max = float(np.clip(requested_accel_max, 0.0, ACCEL_MAX)) + closing_threshold = CLOSING_SPEED_EXIT if self.live_accel_bypass else CLOSING_SPEED_ENTER + if lead_present and closing_speed > closing_threshold: + self.live_accel_bypass = True + self.live_accel_rejoin = False + elif self.live_accel_bypass and (not lead_present or closing_speed <= CLOSING_SPEED_EXIT): + self.live_accel_bypass = False + self.live_accel_rejoin = self.live_accel_max is not None and self.live_accel_max < ACCEL_MAX + + if self.live_accel_max is None and not self.live_accel_bypass and v_ego >= STOP_HOLD_EGO_SPEED: + if self.live_accel_warmup_frames < ACCEL_SHAPE_WARMUP_FRAMES: + self.live_accel_warmup_frames += 1 + return requested_accel_max, None, False, False + + if self.live_accel_max is None: + self.live_accel_max = min(requested_accel_max, STANDSTILL_MPC_ACCEL_SEED) if v_ego < STOP_HOLD_EGO_SPEED else ACCEL_MAX + self.live_accel_warmup_frames = 0 + if v_ego < STOP_HOLD_EGO_SPEED and not self.live_accel_bypass: + return self.live_accel_max, build_trajectory(self.live_accel_max), True, True + + if self.live_accel_bypass or self.live_accel_rejoin: + self.live_accel_max = min(ACCEL_MAX, self.live_accel_max + ACCEL_LIMIT_BYPASS_RATE * self.dt) + if self.live_accel_max < ACCEL_MAX: + return self.live_accel_max, build_trajectory(self.live_accel_max), True, False + if self.live_accel_bypass: + return requested_accel_max, None, False, False + self.live_accel_rejoin = False + return self.live_accel_max, build_trajectory(self.live_accel_max), True, True + + if requested_accel_max >= self.live_accel_max: + raise_rate = STANDSTILL_ACCEL_LIMIT_RAISE_RATE if v_ego < STOP_HOLD_EGO_SPEED else ACCEL_LIMIT_RAISE_RATE + self.live_accel_max = min(requested_accel_max, self.live_accel_max + raise_rate * self.dt) + else: + self.live_accel_max = max(requested_accel_max, self.live_accel_max - ACCEL_LIMIT_TIGHTEN_RATE * self.dt) + + self.live_accel_max = float(np.clip(self.live_accel_max, 0.0, ACCEL_MAX)) + return self.live_accel_max, build_trajectory(self.live_accel_max), True, True + + def reset(self) -> None: + self.live.reset() + self.shadow.reset() + self._reset_accel_limit() + self._clear_recovery_state() + + def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, profile: int | AccelProfile, follow_personality, + enabled: bool, acc_selected: bool, engaged: bool, cruise_initialized: bool, previous_mpc_source, + planner_speed: float, stock_accel_max: float, planner_accel: float, previous_should_stop: bool, + controller_fault: bool = False) -> AccelControllerResult: + selected_profile = self._profile(profile) sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego - config = PROFILE_CONFIGS[profile] - profile_accel_max = self.get_profile_accel_max(profile, sanitized_v_ego) + profile_accel_max = self.get_profile_accel_max(selected_profile, sanitized_v_ego) delay = self._delay() - valid_context = self._valid_context( - base_speed, sanitized_v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay, engaged, cruise_initialized, controller_fault, - ) - - envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope() - selected_lead_speed = math.inf - if envelope.selected_lead in (0, 1): - selected_lead_speed = float(getattr((radar_state.leadOne, radar_state.leadTwo)[envelope.selected_lead], "vLeadK", math.inf)) + valid_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, + delay, engaged, cruise_initialized, controller_fault) + recovery_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, + delay, engaged, cruise_initialized, False) + envelope = (self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, selected_profile, follow_personality) + if valid_context else EnergyEnvelope()) + config = PROFILE_CONFIGS[selected_profile] if valid_context: - shadow_filtered_cap = self._update_path( - self.shadow, envelope, base_speed, sanitized_v_ego, config, previous_mpc_source, planner_speed, previous_should_stop, selected_lead_speed, - ) - self._update_accel_limit( - self.shadow, envelope, stock_accel_max, planner_accel, profile_accel_max, config, sanitized_v_ego, selected_lead_speed, - ) + shadow_filtered_cap = self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, config, previous_mpc_source, + planner_speed, previous_should_stop) + self._update_moving_lead_decel(self.shadow, envelope, base_speed, sanitized_v_ego) shadow_active = True else: self.shadow.reset() shadow_filtered_cap = math.inf shadow_active = False - reset_mpc = False live_active = valid_context and bool(enabled) and bool(acc_selected) if live_active: - live_was_initialized = self.live.pace is not None - established_selected_lead = False - if envelope.selected_lead in (0, 1): - selected_lead = (radar_state.leadOne, radar_state.leadTwo)[envelope.selected_lead] - selected_track_id = int(getattr(selected_lead, "radarTrackId", -1)) - previous_track_id = self.live.lead_track_ids[envelope.selected_lead] - positive_track_change = selected_track_id >= 0 and previous_track_id >= 0 and selected_track_id != previous_track_id - established_selected_lead = self.live.lead_seen[envelope.selected_lead] and not positive_track_change - live_filtered_cap = self._update_path( - self.live, envelope, base_speed, sanitized_v_ego, config, previous_mpc_source, planner_speed, previous_should_stop, selected_lead_speed, + if self.live.pace is None and self.recovery_available: + self.live.pace = min(base_speed, self.recovery_target_speed) + live_filtered_cap = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, config, previous_mpc_source, + planner_speed, previous_should_stop) + self._update_moving_lead_decel(self.live, envelope, base_speed, sanitized_v_ego) + target_speed = base_speed if self.live.departing_from_stop else min(base_speed, self.live.pace if self.live.pace is not None else base_speed) + effective_accel_max, mpc_accel_max, mpc_shape_cruise, mpc_apply_accel_constraint = self._build_mpc_accel_max( + profile_accel_max, stock_accel_max, planner_accel, sanitized_v_ego, envelope.selected_lead >= 0, + envelope.closing_speed, self.live.state == AccelControllerState.stopHold, ) - lead_obstacle_weights = self._update_lead_obstacle_weights( - self.live, radar_state, sanitized_v_ego, a_ego, planner_accel, follow_personality, allow_blend=live_was_initialized, - ) - urgent_was_active = self.live.urgent_bypass_active - selected_has_full_authority = envelope.selected_lead in (0, 1) and lead_obstacle_weights[envelope.selected_lead] >= 1.0 - urgent_required_decel = envelope.required_decel - if not live_was_initialized or not established_selected_lead: - urgent_required_decel = max(urgent_required_decel, envelope.conservative_required_decel) - low_speed_urgent_lead = selected_lead_speed < URGENT_LOW_SPEED_LEAD_BYPASS and self.live.state != AccelControllerState.stopHold - urgent_trigger = ( - (sanitized_v_ego >= URGENT_BYPASS_MIN_SPEED or low_speed_urgent_lead) - and urgent_required_decel >= URGENT_BYPASS_REQUIRED_DECEL - and (not live_was_initialized or established_selected_lead or selected_has_full_authority) - ) - if urgent_was_active and envelope.selected_lead < 0: - self.live.urgent_dropout_frames += 1 - else: - self.live.urgent_dropout_frames = 0 - urgent_dropout_guard = urgent_was_active and envelope.selected_lead < 0 and self.live.urgent_dropout_frames <= 2 - urgent_bypass = urgent_trigger or ( - urgent_was_active - and envelope.selected_lead >= 0 - and ( - envelope.required_decel > URGENT_RELEASE_REQUIRED_DECEL - or (selected_lead_speed < URGENT_LOW_SPEED_LEAD_BYPASS and self.live.state != AccelControllerState.stopHold) - ) - ) - # Latch urgency across two missing observations, retaining scalar state but never stale lead geometry. - self.live.urgent_bypass_active = urgent_bypass or urgent_dropout_guard - if (urgent_was_active and not urgent_bypass and not urgent_dropout_guard and envelope.selected_lead >= 0 - and self.live.state != AccelControllerState.stopHold): - # Rejoin from stock's achieved speed, not the stale pre-urgent pace. - rejoin_speed = min(planner_speed, sanitized_v_ego) - if envelope.closing_speed <= 0.10 and math.isfinite(selected_lead_speed): - # A matched moving lead is the lower pace reference; seeding below it reinforces residual braking. - rejoin_speed = max(rejoin_speed, min(base_speed, selected_lead_speed)) - self.live.pace = rejoin_speed - else: - self.live.pace = min(self.live.pace if self.live.pace is not None else rejoin_speed, rejoin_speed) - self.live.state = AccelControllerState.hold - self.live.relief_time = 0.0 - # Rejoin from the global ceiling so the pre-MPC slew tightens gradually. - self.live.accel_limit = ACCEL_MAX - self.live.urgent_recovery_active = True - if urgent_dropout_guard: - # Two-frame dropout holds pace and a nonpositive ceiling without stale lead geometry. - held_limit = self.live.accel_limit if self.live.accel_limit is not None else planner_accel - self.live.accel_limit = float(np.clip(min(held_limit, 0.0), ACCEL_MIN, ACCEL_MAX)) - self.live.decel_limit_active = self.live.accel_limit < 0.0 - self.live.urgent_recovery_active = False - effective_accel_max = min(stock_accel_max, self.live.accel_limit) - mpc_accel_max = self._build_mpc_accel_max(self.live.accel_limit) - mpc_shape_cruise = True - lead_obstacle_weights = (1.0, 1.0) - target_speed = min(base_speed, self.live.pace if self.live.pace is not None else sanitized_v_ego) - elif urgent_bypass: - # Urgent entry restores raw leads and stock bounds, then resets MPC before planner state/update. - reset_mpc = reset_mpc or not urgent_was_active - self.live.launch_target_active = False - self.live.accel_limit = None - self.live.decel_limit_active = False - self.live.urgent_recovery_active = False - if self.live.pace is not None: - self.live.pace = min(self.live.pace, planner_speed, sanitized_v_ego) - effective_accel_max = stock_accel_max - mpc_accel_max = None - mpc_shape_cruise = False - lead_obstacle_weights = (1.0, 1.0) - target_speed = base_speed - else: - recovery_was_active = self.live.urgent_recovery_active - effective_accel_max, controller_accel_max = self._update_accel_limit( - self.live, envelope, stock_accel_max, planner_accel, profile_accel_max, config, sanitized_v_ego, selected_lead_speed, - ) - if ( - recovery_was_active - and not self.live.urgent_recovery_active - and envelope.closing_speed <= 0.10 - and math.isfinite(selected_lead_speed) - and self.live.pace is not None - ): - # Do not carry recovery pace below a matched moving lead. - self.live.pace = max(self.live.pace, min(base_speed, selected_lead_speed)) - mpc_accel_max = self._build_mpc_accel_max(controller_accel_max) - mpc_shape_cruise = mpc_accel_max is not None - if mpc_accel_max is None: - effective_accel_max = stock_accel_max - if self.live.state == AccelControllerState.stopHold: - # Pin cruise because some platforms assert shouldStop below 0.30 m/s while lead authority is muted. - target_speed = 0.0 - elif self.live.departing_from_stop or self.live.launch_target_active: - # Base cruise supplies launch incentive; renewed closing cancels it. - target_speed = base_speed - elif ( - envelope.selected_lead >= 0 - and math.isfinite(selected_lead_speed) - and selected_lead_speed > STOPPED_LEAD_SPEED - and envelope.closing_speed <= MATERIAL_CLOSING_SPEED_EXIT - and envelope.required_decel <= MATERIAL_CLOSING_DECEL_EXIT - and planner_accel < -0.20 - and not self.live.material_closing - ): - # Base cruise unwinds residual braking after matching a moving lead. Raw-lead authority and mpc_accel_max remain active. - target_speed = base_speed - mpc_shape_cruise = False - lead_obstacle_weights = (1.0, 1.0) - elif self.live.urgent_recovery_active: - # Keep raw-lead authority and synchronized pace while the ceiling rejoins. - target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed) - lead_obstacle_weights = (1.0, 1.0) - else: - target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed) + self._cache_recovery_state(target_speed, mpc_accel_max, mpc_shape_cruise, mpc_apply_accel_constraint) else: self.live.reset() + self._reset_accel_limit() live_filtered_cap = math.inf - # Preserve the stock target bit-for-bit on every bypass, including stock's own invalid-value handling. target_speed = base_speed - effective_accel_max = math.inf - mpc_accel_max = None - mpc_shape_cruise = False - lead_obstacle_weights = (1.0, 1.0) + retain_recovery_state = bool(controller_fault) and recovery_context and self.recovery_available + if retain_recovery_state: + target_speed = min(base_speed, self.recovery_target_speed) + mpc_accel_max = (tuple(min(value, stock_accel_max, ACCEL_MAX) for value in self.recovery_accel_max) + if self.recovery_accel_max is not None else None) + mpc_shape_cruise = self.recovery_shape_cruise if mpc_accel_max is not None else False + mpc_apply_accel_constraint = self.recovery_apply_accel_constraint if mpc_accel_max is not None else False + effective_accel_max = min(mpc_accel_max) if mpc_accel_max is not None else math.inf + else: + self._clear_recovery_state() + effective_accel_max = math.inf + mpc_accel_max = None + mpc_shape_cruise = False + mpc_apply_accel_constraint = False return AccelControllerResult( target_speed=target_speed, enabled=bool(enabled), active=live_active, shadow_active=shadow_active, - launching=live_active and self.live.departing_from_stop, profile=profile, + launching=live_active and self.live.departing_from_stop, profile=selected_profile, profile_accel_max=profile_accel_max if live_active else math.inf, effective_accel_max=effective_accel_max, - mpc_accel_max=mpc_accel_max, mpc_shape_cruise=mpc_shape_cruise, reset_mpc=reset_mpc, lead_obstacle_weights=lead_obstacle_weights, + mpc_accel_max=mpc_accel_max, mpc_shape_cruise=mpc_shape_cruise, mpc_apply_accel_constraint=mpc_apply_accel_constraint, state=self.live.state, shadow_state=self.shadow.state, base_speed=base_speed, raw_energy_cap=envelope.cap, live_filtered_cap=live_filtered_cap, shadow_filtered_cap=shadow_filtered_cap, live_pace=self.live.pace if self.live.pace is not None else math.inf, diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index a113bea06c..930dc9b207 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 import math from types import SimpleNamespace @@ -6,35 +5,38 @@ import numpy as np import pytest from cereal import log -from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from opendbc.car.interfaces import ACCEL_MAX from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource, STOP_DISTANCE, T_IDXS, get_T_FOLLOW from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import ( - ACCEL_LIMIT_JERK, + ACCEL_LIMIT_BYPASS_RATE, + ACCEL_LIMIT_RAISE_RATE, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, - BREAKAWAY_ACCEL_MAX, - CLEAR_LAUNCH_ACCEL_RATE, - DECEL_LIMIT_JERK, - HOLD_ACCEL_MAX, - INITIAL_LAUNCH_ACCEL_MAX, - LAUNCH_ACCEL_RATE, - LAUNCH_DELTA_V, - RELATIVE_PACE_PREVIEW_TIME, - URGENT_BYPASS_REQUIRED_DECEL, - URGENT_REJOIN_ACCEL_MAX, - URGENT_REJOIN_ACCEL_RATE, - URGENT_RELEASE_REQUIRED_DECEL, + ACCEL_SHAPE_WARMUP_FRAMES, + CAP_FILTER_FRAMES, + MOVING_LEAD_DECEL_ACCEL_MAX, + MOVING_LEAD_DECEL_ACCEL_SLEW_RATE, + MOVING_LEAD_DECEL_CONFIRM_FRAMES, + MOVING_LEAD_DECEL_EXIT_FRAMES, + STOP_HOLD_EXIT_FRAMES, AccelController, AccelControllerState, AccelProfile, + EnergyEnvelope, PROFILE_CONFIGS, ) -def make_lead(*, status: bool = False, d_rel: float = 0.0, v_lead_k: float = 0.0, a_lead_k: float = 0.0, a_lead_tau: float = 1.5, - radar_track_id: int = -1): +def make_lead( + *, + status: bool = False, + d_rel: float = 0.0, + v_lead_k: float = 0.0, + a_lead_k: float = 0.0, + a_lead_tau: float = 1.5, + radar_track_id: int = -1, +): return SimpleNamespace( status=status, dRel=d_rel, @@ -49,14 +51,14 @@ def make_radar(lead_one=None, lead_two=None): return SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead()) -def make_governor(delay: float = 0.10): +def make_controller(delay: float = 0.10): return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay)) -def update(governor, radar_state=None, **overrides): +def update(controller, radar_state=None, **overrides): args = { "base_speed": 20.0, - "v_ego": 20.0, + "v_ego": 10.0, "a_ego": 0.0, "profile": AccelProfile.normal, "follow_personality": log.LongitudinalPersonality.standard, @@ -65,1109 +67,673 @@ def update(governor, radar_state=None, **overrides): "engaged": True, "cruise_initialized": True, "previous_mpc_source": LongitudinalPlanSource.cruise, - "planner_speed": 20.0, - "stock_accel_max": 2.0, + "planner_speed": 10.0, + "stock_accel_max": 1.2, "planner_accel": 0.0, "previous_should_stop": False, } args.update(overrides) - return governor.update(radar_state or make_radar(), **args) + return controller.update(radar_state or make_radar(), **args) -def assert_profile_trajectory(result, expected: float) -> None: - assert result.mpc_accel_max is not None - np.testing.assert_array_equal(result.mpc_accel_max, expected) - - -class TestAccelProfileLimits: - def test_profile_table_matches_tuned_values(self): - assert ACCEL_PROFILE_MAX_BP == [0.0, 10.0, 25.0, 40.0] +class TestAccelProfile: + def test_lookup_table_values(self): + assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0] assert ACCEL_PROFILE_MAX_V == { - AccelProfile.eco: [1.55, 0.85, 0.45, 0.25], - AccelProfile.normal: [1.65, 1.10, 0.70, 0.45], - AccelProfile.sport: [2.00, 1.70, 1.20, 0.90], + AccelProfile.eco: [1.55, 1.25, 0.85, 0.50, 0.30], + AccelProfile.normal: [1.70, 1.40, 1.05, 0.65, 0.45], + AccelProfile.sport: [2.00, 1.90, 1.70, 1.20, 0.90], } @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_profile_accel_max_matches_lookup_table(self, profile): + def test_lookup_table_interpolates_and_clamps(self, profile): for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True): assert AccelController.get_profile_accel_max(profile, speed) == expected - @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_profile_accel_max_interpolates_and_clamps(self, profile): - expected_midpoint = (ACCEL_PROFILE_MAX_V[profile][1] + ACCEL_PROFILE_MAX_V[profile][2]) / 2.0 - - assert AccelController.get_profile_accel_max(profile, 17.5) == pytest.approx(expected_midpoint) assert AccelController.get_profile_accel_max(profile, -1.0) == ACCEL_PROFILE_MAX_V[profile][0] assert AccelController.get_profile_accel_max(profile, 50.0) == ACCEL_PROFILE_MAX_V[profile][-1] - @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP) - def test_profile_accel_max_order_is_distinct(self, speed): - limits = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile] - - assert limits[AccelProfile.eco] < limits[AccelProfile.normal] < limits[AccelProfile.sport] - @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_profile_table_stays_within_global_accel_limit(self, profile): - for step in range(161): - speed = step * 0.25 + def test_lookup_table_stays_within_global_limit(self, profile): + for speed in np.linspace(0.0, 40.0, 161): assert 0.0 <= AccelController.get_profile_accel_max(profile, speed) <= ACCEL_MAX + def test_launch_knots_are_prompt_and_profiles_separate_after_walking_speed(self): + launch_limits = [AccelController.get_profile_accel_max(profile, 0.0) for profile in AccelProfile] + rolling_limits = [AccelController.get_profile_accel_max(profile, 10.0) for profile in AccelProfile] + + assert min(launch_limits) >= 1.55 + assert rolling_limits[0] < rolling_limits[1] < rolling_limits[2] + + def test_active_profile_is_clipped_to_stock_before_mpc(self): + controller = make_controller() + results = [update(controller, profile=AccelProfile.sport, stock_accel_max=1.2) for _ in range(12)] + + assert all(result.profile_accel_max == 1.7 for result in results) + assert all(result.mpc_accel_max is None for result in results[:ACCEL_SHAPE_WARMUP_FRAMES]) + assert results[-1].effective_accel_max == 1.2 + assert results[-1].mpc_accel_max == tuple(1.2 for _ in T_IDXS) + assert results[-1].mpc_shape_cruise + assert results[-1].mpc_apply_accel_constraint + assert max(results[-1].mpc_accel_max) <= min(1.2, ACCEL_MAX) + @pytest.mark.parametrize("profile", list(AccelProfile)) - @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP) - def test_stock_dynamic_output_limit_remains_authoritative(self, profile, speed): - governor = make_governor() - stock_limit = get_max_accel(speed) - - result = update(governor, profile=profile, v_ego=speed, planner_speed=speed, stock_accel_max=stock_limit) - - assert result.effective_accel_max <= stock_limit - - @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP[1:]) - def test_effective_profiles_remain_distinct_below_stock_output_limit(self, speed): - stock_limit = get_max_accel(speed) - limits = [] - for profile in AccelProfile: - governor = make_governor() - result = update(governor, profile=profile, v_ego=speed, planner_speed=speed, stock_accel_max=stock_limit) - limits.append(result.effective_accel_max) - - assert limits[AccelProfile.eco] < limits[AccelProfile.normal] < limits[AccelProfile.sport] - - def test_active_result_exposes_profile_accel_max(self): - governor = make_governor() - - result = update(governor, profile=AccelProfile.eco, v_ego=17.5, planner_speed=17.5) - - assert result.profile_accel_max == pytest.approx(0.65) - - def test_clear_road_profile_is_a_separate_pre_mpc_trajectory(self): - governor = make_governor() - - result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.4) + @pytest.mark.parametrize("stock_accel_max", [0.6, 1.2, ACCEL_MAX]) + def test_settled_mpc_limit_respects_profile_stock_and_global_ceiling(self, profile, stock_accel_max): + controller = make_controller() + result = [update(controller, profile=profile, stock_accel_max=stock_accel_max) for _ in range(40)][-1] + expected = min(AccelController.get_profile_accel_max(profile, 10.0), stock_accel_max, ACCEL_MAX) + assert result.effective_accel_max == pytest.approx(expected) assert result.mpc_accel_max is not None - assert result.mpc_shape_cruise - assert len(result.mpc_accel_max) == N + 1 - assert_profile_trajectory(result, 1.10) + assert max(result.mpc_accel_max) <= expected + assert result.mpc_apply_accel_constraint - def test_ordinary_closing_lead_uses_no_gas_pre_mpc_bound(self): - governor = make_governor() - radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0)) + def test_profiles_produce_distinct_effective_limits(self): + results = [update(make_controller(), profile=profile, stock_accel_max=1.2) for profile in AccelProfile] - result = update(governor, radar_state) + assert [result.effective_accel_max for result in results] == [0.85, 1.05, 1.2] + assert all(result.mpc_accel_max is None for result in results) - assert result.active - assert result.selected_lead == 0 - assert_profile_trajectory(result, 0.10) - assert result.mpc_shape_cruise + def test_nonpositive_stock_limit_does_not_create_custom_braking_horizon(self): + result = update(make_controller(), stock_accel_max=-0.2) - def test_filtered_lead_history_keeps_profile_bound_through_two_dropouts(self): - governor = make_governor() - radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0)) - for _ in range(3): - update(governor, radar_state) - - dropouts = [update(governor), update(governor)] - - assert all(math.isfinite(result.live_filtered_cap) for result in dropouts) - assert all(result.mpc_accel_max is not None for result in dropouts) - assert all(result.mpc_shape_cruise for result in dropouts) - - def test_stop_hold_pins_zero_target_with_coherent_zero_accel_horizon(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - - held = update(governor, stopped, base_speed=5.0, v_ego=0.1, planner_speed=0.1) - confirming = update(governor, moving, base_speed=5.0, v_ego=0.1, planner_speed=0.1) - - assert held.state == AccelControllerState.stopHold - assert held.target_speed == 0.0 - assert held.effective_accel_max == 0.0 - assert_profile_trajectory(held, 0.0) - assert held.mpc_shape_cruise - assert held.lead_obstacle_weights == (0.0, 0.0) - assert confirming.state == AccelControllerState.stopHold - assert confirming.target_speed == 0.0 - assert confirming.effective_accel_max == 0.0 - assert_profile_trajectory(confirming, 0.0) - assert confirming.mpc_shape_cruise - assert confirming.lead_obstacle_weights == (0.0, 0.0) - - def test_stop_hold_keeps_raw_lead_above_vehicle_should_stop_threshold(self): - governor = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.10, vEgoStopping=0.25)) - stopped = make_radar(make_lead(status=True, d_rel=2.0, v_lead_k=0.0)) - - held = update(governor, stopped, base_speed=5.0, v_ego=0.28, planner_speed=0.28) - - assert held.state == AccelControllerState.stopHold - assert held.target_speed == 0.0 - assert held.lead_obstacle_weights == (1.0, 1.0) - - def test_normal_active_limits_are_bounded_by_stock_and_profile(self): - governor = make_governor() - - result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40) - - assert result.profile_accel_max == 1.10 - assert result.effective_accel_max == 1.10 - assert_profile_trajectory(result, 1.10) - - def test_first_enable_seeds_from_positive_planner_accel_within_stock(self): - governor = make_governor() - - first = update( - governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20 - ) - second = update( - governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20 - ) - - assert first.effective_accel_max == 1.20 - assert second.effective_accel_max == pytest.approx(first.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL) - - def test_first_enable_seed_preserves_current_plan_but_effective_limit_stays_stock_bounded(self): - governor = make_governor() - - result = update( - governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.10, planner_accel=1.80 - ) - - assert result.effective_accel_max == 1.10 - assert_profile_trajectory(result, 1.80) - - def test_profile_switch_slews_at_one_meter_per_second_cubed(self): - governor = make_governor() - sport = update(governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0) - - eco = update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0) - - assert sport.effective_accel_max == 1.70 - assert eco.effective_accel_max == pytest.approx(sport.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL) - assert eco.effective_accel_max > eco.profile_accel_max - - def test_dynamic_stock_tightening_does_not_enter_controller_comfort_state(self): - governor = make_governor() - update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40) - - tightened = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=0.40) - released = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40) - - assert tightened.effective_accel_max == 0.40 - assert_profile_trajectory(tightened, 1.10) - assert released.effective_accel_max == 1.10 - assert_profile_trajectory(released, 1.10) - - def test_negative_stock_max_remains_authoritative_outside_the_mpc_profile_bound(self): - governor = make_governor() - - result = update(governor, v_ego=10.0, planner_speed=10.0, stock_accel_max=-0.20, planner_accel=1.0) - - assert result.effective_accel_max == -0.20 - assert_profile_trajectory(result, 1.10) - - def test_profile_tightening_can_converge_below_positive_planner_accel(self): - governor = make_governor() - update( - governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15 - ) - - results = [ - update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15) - for _ in range(30) - ] - - assert results[-1].effective_accel_max == pytest.approx(ACCEL_PROFILE_MAX_V[AccelProfile.eco][1]) - assert results[-1].effective_accel_max < 1.15 - assert all(result.mpc_accel_max is not None for result in results) - - @pytest.mark.parametrize("bypass", [{"enabled": False}, {"acc_selected": False}]) - def test_bypass_does_not_expose_an_active_accel_limit(self, bypass): - governor = make_governor() - - result = update(governor, **bypass) - - assert math.isinf(result.profile_accel_max) - assert math.isinf(result.effective_accel_max) + assert result.effective_accel_max == -0.2 assert result.mpc_accel_max is None assert not result.mpc_shape_cruise - @pytest.mark.parametrize("invalid", [{"stock_accel_max": math.nan}, {"planner_accel": math.nan}]) - def test_invalid_accel_input_bypasses_and_resets_limits(self, invalid): - governor = make_governor() - update(governor) + def test_profile_switch_reaches_new_limit_without_a_step(self): + controller = make_controller() + eco = [update(controller, profile=AccelProfile.eco) for _ in range(20)] + sport = [update(controller, profile=AccelProfile.sport) for _ in range(8)] + limits = np.array([eco[-1].effective_accel_max, *(result.effective_accel_max for result in sport)]) - result = update(governor, **invalid) + assert eco[-1].effective_accel_max == 0.85 + assert sport[-1].effective_accel_max == 1.2 + assert np.all(np.diff(limits) >= 0.0) + assert np.max(np.diff(limits)) <= ACCEL_LIMIT_RAISE_RATE * DT_MDL + 1e-9 - assert not result.active - assert governor.live.accel_limit is None - assert math.isinf(result.effective_accel_max) - assert not result.mpc_shape_cruise + def test_lead_transitions_preserve_profile_without_constraining_a_closing_lead(self): + controller = make_controller() + clear = [update(controller, profile=AccelProfile.eco) for _ in range(20)] + faster_lead = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0)) + nonclosing = update(controller, faster_lead, profile=AccelProfile.eco) + assert clear[-1].effective_accel_max == pytest.approx(ACCEL_PROFILE_MAX_V[AccelProfile.eco][2]) + assert nonclosing.effective_accel_max == clear[-1].effective_accel_max + assert nonclosing.mpc_apply_accel_constraint -class TestLeadObstacleAcquisition: - benign_lead = make_lead(status=True, d_rel=126.0, v_lead_k=17.0, radar_track_id=7) + closing_lead = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=5.0)) + bypass = [update(controller, closing_lead, profile=AccelProfile.eco) for _ in range(20)] - def test_lead_already_present_at_enable_has_full_authority(self): - governor = make_governor() + transition_bounds = [result.mpc_accel_max[0] for result in bypass if result.mpc_accel_max is not None] + assert transition_bounds[0] == pytest.approx(nonclosing.effective_accel_max + ACCEL_LIMIT_BYPASS_RATE * DT_MDL) + assert np.max(np.diff(transition_bounds)) <= ACCEL_LIMIT_BYPASS_RATE * DT_MDL + 1e-9 + assert all(not result.mpc_apply_accel_constraint for result in bypass) + assert bypass[-1].mpc_accel_max is None + assert not bypass[-1].mpc_shape_cruise - result = update(governor, make_radar(self.benign_lead), base_speed=30.0, v_ego=20.0, planner_speed=20.0) + rejoin = [update(controller, profile=AccelProfile.eco) for _ in range(20)] + rejoin_bounds = np.array([result.mpc_accel_max[0] for result in rejoin]) - assert result.lead_obstacle_weights == (1.0, 1.0) - - def test_benign_new_lead_reaches_full_authority_in_point_three_seconds(self): - governor = make_governor() - args = {"base_speed": 30.0, "v_ego": 20.0, "planner_speed": 20.0} - update(governor, **args) - - results = [update(governor, make_radar(self.benign_lead), **args) for _ in range(7)] - - expected = np.linspace(0.2, 1.0, 7) - np.testing.assert_allclose([result.lead_obstacle_weights[0] for result in results], expected, atol=1e-12, rtol=0.0) - assert all(result.mpc_accel_max is not None for result in results) - - def test_route_shaped_urgent_acquisition_is_immediate(self): - governor = make_governor() - args = {"base_speed": 40.0, "v_ego": 34.8, "planner_speed": 34.8} - update(governor, **args) - route_lead = make_lead(status=True, d_rel=93.6, v_lead_k=23.4, radar_track_id=22) - - result = update(governor, make_radar(route_lead), **args) - - assert result.required_decel > 1.0 - assert result.lead_obstacle_weights == (1.0, 1.0) - assert result.reset_mpc - - @pytest.mark.parametrize( - "lead", - [ - make_lead(status=True, d_rel=25.0, v_lead_k=0.0), - make_lead(status=True, d_rel=126.0, v_lead_k=17.0, a_lead_k=-0.6), - ], - ) - def test_close_or_braking_new_lead_is_immediate(self, lead): - governor = make_governor() - args = {"base_speed": 30.0, "v_ego": 20.0, "planner_speed": 20.0} - update(governor, **args) - - result = update(governor, make_radar(lead), **args) - - assert result.lead_obstacle_weights == (1.0, 1.0) - - def test_dropout_discards_authority_state_and_reacquisition_starts_fresh(self): - governor = make_governor() - args = {"base_speed": 30.0, "v_ego": 20.0, "planner_speed": 20.0} - update(governor, **args) - first = update(governor, make_radar(self.benign_lead), **args) - dropout = update(governor, **args) - reacquired = update(governor, make_radar(self.benign_lead), **args) - - assert first.lead_obstacle_weights[0] == pytest.approx(0.2) - assert dropout.lead_obstacle_weights == (1.0, 1.0) - assert reacquired.lead_obstacle_weights[0] == pytest.approx(0.2) - - @pytest.mark.parametrize("bypass", [{"enabled": False}, {"acc_selected": False}]) - def test_non_actuating_mode_always_requests_raw_lead_authority(self, bypass): - governor = make_governor() - update(governor) - - result = update(governor, make_radar(self.benign_lead), **bypass) - - assert result.lead_obstacle_weights == (1.0, 1.0) + assert all(result.mpc_apply_accel_constraint for result in rejoin) + assert rejoin_bounds[0] == pytest.approx(ACCEL_MAX - ACCEL_LIMIT_BYPASS_RATE * DT_MDL) + assert np.max(np.abs(np.diff(rejoin_bounds))) <= ACCEL_LIMIT_BYPASS_RATE * DT_MDL + 1e-9 + assert rejoin_bounds[-1] == ACCEL_PROFILE_MAX_V[AccelProfile.eco][2] + assert np.max(rejoin_bounds) <= ACCEL_MAX class TestEnergyEnvelope: - def test_correct_relative_energy_formula_and_lead_selection(self): - governor = make_governor() - lead_one = make_lead(status=True, d_rel=60.0, v_lead_k=10.0) - lead_two = make_lead(status=True, d_rel=100.0, v_lead_k=15.0) - radar_state = make_radar(lead_one, lead_two) + def test_relative_energy_formula(self): + controller = make_controller() + lead = make_lead(status=True, d_rel=60.0, v_lead_k=10.0) + envelope = controller.calculate_energy_envelope(make_radar(lead), 20.0, 0.0, AccelProfile.normal) - envelope = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal) - - delay = governor.CP.longitudinalActuatorDelay + DT_MDL + delay = controller.CP.longitudinalActuatorDelay + DT_MDL x_ego = 20.0 * delay - x_lead = lead_one.dRel + lead_one.vLeadK * delay - usable_gap = x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW() * lead_one.vLeadK - anticipated_gap = max(usable_gap - (20.0 - lead_one.vLeadK) * RELATIVE_PACE_PREVIEW_TIME, 0.0) - expected = lead_one.vLeadK + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * anticipated_gap) - incorrect_fixed_target_formula = math.sqrt( - lead_one.vLeadK**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * anticipated_gap, - ) + x_lead = lead.dRel + lead.vLeadK * delay + usable_gap = x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW() * lead.vLeadK + expected = lead.vLeadK + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * usable_gap) + incorrect = math.sqrt(lead.vLeadK**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * usable_gap) - assert envelope.selected_lead == 0 assert envelope.usable_gap == pytest.approx(usable_gap) assert envelope.cap == pytest.approx(expected) - assert envelope.cap != pytest.approx(incorrect_fixed_target_formula) + assert envelope.cap != pytest.approx(incorrect) - def test_lead_two_can_be_more_restrictive(self): - governor = make_governor() - lead_one = make_lead(status=True, d_rel=100.0, v_lead_k=18.0) - lead_two = make_lead(status=True, d_rel=35.0, v_lead_k=5.0) + def test_more_restrictive_lead_is_selected(self): + leads = make_radar( + make_lead(status=True, d_rel=100.0, v_lead_k=18.0), + make_lead(status=True, d_rel=35.0, v_lead_k=5.0), + ) - envelope = governor.calculate_energy_envelope(make_radar(lead_one, lead_two), 20.0, 0.0, AccelProfile.normal) + envelope = make_controller().calculate_energy_envelope(leads, 20.0, 0.0, AccelProfile.normal) assert envelope.selected_lead == 1 - assert envelope.cap < 10.0 - @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_profile_uses_its_comfort_deceleration(self, profile): - governor = make_governor() - radar_state = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0)) + def test_profiles_order_early_deceleration(self): + radar = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0)) + caps = [make_controller().calculate_energy_envelope(radar, 20.0, 0.0, profile).cap for profile in AccelProfile] - envelope = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, profile) - anticipated_gap = max(envelope.usable_gap - envelope.closing_speed * RELATIVE_PACE_PREVIEW_TIME, 0.0) - expected = 10.0 + math.sqrt(2.0 * PROFILE_CONFIGS[profile].comfort_decel * anticipated_gap) + assert caps[0] < caps[1] < caps[2] - assert envelope.cap == pytest.approx(expected) - - def test_profile_caps_order_eco_normal_sport(self): - governor = make_governor() - radar_state = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0)) - - caps = [governor.calculate_energy_envelope(radar_state, 20.0, 0.0, profile).cap for profile in AccelProfile] - - assert caps[AccelProfile.eco] < caps[AccelProfile.normal] < caps[AccelProfile.sport] - - def test_stock_follow_personality_is_independent(self): - governor = make_governor() - radar_state = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0)) - - aggressive = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal, log.LongitudinalPersonality.aggressive) - relaxed = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal, log.LongitudinalPersonality.relaxed) + def test_follow_personality_remains_stock_authority(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0)) + aggressive = controller.calculate_energy_envelope(radar, 20.0, 0.0, AccelProfile.normal, log.LongitudinalPersonality.aggressive) + relaxed = controller.calculate_energy_envelope(radar, 20.0, 0.0, AccelProfile.normal, log.LongitudinalPersonality.relaxed) assert relaxed.usable_gap < aggressive.usable_gap assert relaxed.cap < aggressive.cap - def test_lead_acceleration_is_clipped_before_extrapolation(self): - governor = make_governor(delay=0.30) + def test_lead_acceleration_is_clipped_before_projection(self): + controller = make_controller(delay=0.30) extreme = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0, a_lead_k=-100.0)) clipped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0, a_lead_k=-10.0)) - extreme_envelope = governor.calculate_energy_envelope(extreme, 20.0, 0.0, AccelProfile.normal) - clipped_envelope = governor.calculate_energy_envelope(clipped, 20.0, 0.0, AccelProfile.normal) + assert controller.calculate_energy_envelope(extreme, 20.0, 0.0, 1) == controller.calculate_energy_envelope(clipped, 20.0, 0.0, 1) - assert extreme_envelope == clipped_envelope + def test_ego_projection_stops_at_zero(self): + assert AccelController._project_ego(0.2, -4.0, 0.15) == pytest.approx((0.005, 0.0)) - def test_ego_projection_stops_at_zero_velocity(self): - x_ego, v_ego = AccelController._project_ego(0.2, -4.0, 0.15) + @pytest.mark.parametrize("field", ["dRel", "vLeadK", "aLeadK", "aLeadTau"]) + @pytest.mark.parametrize("invalid", [math.nan, math.inf, -math.inf]) + def test_nonfinite_lead_values_are_ignored(self, field, invalid): + lead = make_lead(status=True, d_rel=40.0, v_lead_k=10.0) + setattr(lead, field, invalid) - assert x_ego == pytest.approx(0.005) - assert v_ego == 0.0 + envelope = make_controller().calculate_energy_envelope(make_radar(lead), 20.0, 0.0, AccelProfile.normal) - def test_invalid_lead_is_ignored(self): - governor = make_governor() - radar_state = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=10.0)) - - envelope = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal) - - assert math.isinf(envelope.cap) - assert envelope.selected_lead == -1 - - -class TestAccelControllerState: - restrictive_lead = make_lead(status=True, d_rel=40.0, v_lead_k=5.0) - - @pytest.mark.parametrize("v_ego", [4.25, 9.39]) - def test_clear_road_rolling_engagement_immediately_targets_base_speed(self, v_ego): - governor = make_governor() - - result = update(governor, base_speed=20.0, v_ego=v_ego, planner_speed=v_ego) - - assert result.state == AccelControllerState.free - assert result.live_pace == result.base_speed - assert result.target_speed == result.base_speed - assert not result.launching - - def test_five_frame_median_requires_three_observations_and_holds_two_dropouts(self): - governor = make_governor() - restrictive_radar = make_radar(self.restrictive_lead) - - first, second, third = [update(governor, restrictive_radar) for _ in range(3)] - - assert math.isinf(first.live_filtered_cap) - assert math.isinf(second.live_filtered_cap) - assert math.isfinite(third.live_filtered_cap) - - dropout_one, dropout_two, dropout_three = [update(governor) for _ in range(3)] - - assert math.isfinite(dropout_one.live_filtered_cap) - assert math.isfinite(dropout_two.live_filtered_cap) - assert math.isinf(dropout_three.live_filtered_cap) - - def test_restriction_is_limited_by_profile_deceleration(self): - governor = make_governor() - # Restrictive enough for early comfort shaping but below the urgent stock-MPC threshold. - radar_state = make_radar(make_lead(status=True, d_rel=160.0, v_lead_k=10.0)) - - update(governor, radar_state) - update(governor, radar_state) - first_restriction = update(governor, radar_state) - next_restriction = update(governor, radar_state) - - expected_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL - assert first_restriction.live_pace == pytest.approx(20.0 - expected_step) - assert next_restriction.live_pace == pytest.approx(first_restriction.live_pace - expected_step) - assert next_restriction.state == AccelControllerState.restrict - assert_profile_trajectory(first_restriction, HOLD_ACCEL_MAX - DECEL_LIMIT_JERK * DT_MDL) - assert_profile_trajectory(next_restriction, first_restriction.mpc_accel_max[0] - DECEL_LIMIT_JERK * DT_MDL) - - def test_urgent_closing_bypasses_comfort_shaping_for_stock_mpc(self): - governor = make_governor() - result = update(governor, make_radar(self.restrictive_lead)) - - assert result.required_decel > URGENT_BYPASS_REQUIRED_DECEL - assert result.target_speed == result.base_speed - assert result.mpc_accel_max is None - assert result.lead_obstacle_weights == (1.0, 1.0) - - @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_e5_low_speed_urgency_hands_back_to_stock_immediately(self, profile): - governor = make_governor(delay=0.15) - established_lead = make_radar(make_lead(status=True, d_rel=18.68, v_lead_k=2.876, a_lead_k=-0.743, radar_track_id=7)) - established_args = { - "profile": profile, "base_speed": 23.056, "v_ego": 5.015, "a_ego": -0.949, - "planner_speed": 5.015, "planner_accel": -1.021, "stock_accel_max": 1.40, - } - - for _ in range(5): - established = update(governor, established_lead, **established_args) - - assert established.required_decel < URGENT_BYPASS_REQUIRED_DECEL - assert not governor.live.urgent_bypass_active - assert established.target_speed < established.base_speed - assert established.mpc_shape_cruise - - urgent_lead = make_radar(make_lead(status=True, d_rel=16.32, v_lead_k=1.89, a_lead_k=-1.16, radar_track_id=7)) - urgent_args = established_args | { - "v_ego": 4.499, "a_ego": -0.75, "planner_speed": 4.499, "planner_accel": -0.95, - } - entering = update(governor, urgent_lead, **urgent_args) - - assert entering.required_decel > URGENT_BYPASS_REQUIRED_DECEL - assert governor.live.urgent_bypass_active - stock_owned = update(governor, urgent_lead, **urgent_args) - assert governor.live.urgent_bypass_active - for result in (entering, stock_owned): - assert result.target_speed == result.base_speed - assert result.lead_obstacle_weights == (1.0, 1.0) - assert not result.mpc_shape_cruise - assert result.mpc_accel_max is None - assert result.effective_accel_max == urgent_args["stock_accel_max"] - - def test_positive_lead_accel_spike_cannot_delay_first_urgent_bypass(self): - governor = make_governor() - radar_state = make_radar(make_lead(status=True, d_rel=90.0, v_lead_k=14.0, a_lead_k=5.0)) - - result = update(governor, radar_state, base_speed=30.0, v_ego=22.0, planner_speed=22.0) - - assert result.required_decel < URGENT_BYPASS_REQUIRED_DECEL - assert governor.live.urgent_bypass_active - assert result.target_speed == result.base_speed - assert result.mpc_accel_max is None - assert result.lead_obstacle_weights == (1.0, 1.0) - - def test_urgent_entry_removes_existing_ceiling_on_first_frame(self): - governor = make_governor() - clear = update(governor, base_speed=40.0, v_ego=34.8, planner_speed=34.8) - urgent_radar = make_radar(make_lead(status=True, d_rel=94.0, v_lead_k=23.4, radar_track_id=22)) - - entering = update(governor, urgent_radar, base_speed=40.0, v_ego=34.8, planner_speed=34.8) - established = update(governor, urgent_radar, base_speed=40.0, v_ego=34.8, planner_speed=34.8) - - assert clear.mpc_accel_max is not None - assert entering.required_decel > URGENT_BYPASS_REQUIRED_DECEL - assert entering.mpc_accel_max is None - assert entering.reset_mpc - assert not entering.mpc_shape_cruise - assert entering.lead_obstacle_weights == (1.0, 1.0) - assert established.mpc_accel_max is None - assert not established.reset_mpc - assert not established.mpc_shape_cruise - - def test_urgent_dropout_holds_pace_and_nonpositive_ceiling_without_lead_geometry(self): - governor = make_governor() - urgent_radar = make_radar(self.restrictive_lead) - for _ in range(3): - update(governor, urgent_radar, planner_accel=-0.5) - - dropout_one = update(governor, planner_accel=-0.5) - dropout_two = update(governor, planner_accel=-0.5) - expired = update(governor, planner_accel=-0.5) - - for result in (dropout_one, dropout_two): - assert result.mpc_accel_max is not None - assert max(result.mpc_accel_max) <= 0.0 - assert result.mpc_shape_cruise - assert result.target_speed == result.live_pace - assert result.target_speed < result.base_speed - assert result.lead_obstacle_weights == (1.0, 1.0) - assert not governor.live.urgent_bypass_active - assert not governor.live.urgent_recovery_active - assert expired.mpc_accel_max is not None - assert expired.target_speed == expired.live_pace == expired.base_speed - assert expired.state == AccelControllerState.free - - def test_urgent_exit_slews_rejoin_ceiling_then_releases_after_speed_match(self): - governor = make_governor() - urgent_radar = make_radar(self.restrictive_lead) - for _ in range(3): - urgent = update(governor, urgent_radar) - assert urgent.required_decel > URGENT_BYPASS_REQUIRED_DECEL - assert governor.live.urgent_bypass_active - - moderate_lead = make_radar(make_lead(status=True, d_rel=180.0, v_lead_k=10.0)) - rejoin = update(governor, moderate_lead) - - assert rejoin.required_decel < URGENT_RELEASE_REQUIRED_DECEL - assert not governor.live.urgent_bypass_active - assert governor.live.urgent_recovery_active - assert rejoin.mpc_accel_max is not None - expected_first_ceiling = ACCEL_MAX - URGENT_REJOIN_ACCEL_RATE * DT_MDL - assert_profile_trajectory(rejoin, expected_first_ceiling) - assert rejoin.live_pace <= 20.0 - - recovery_limits = [rejoin.mpc_accel_max[0]] - while governor.live.urgent_recovery_active and len(recovery_limits) < 50: - result = update(governor, moderate_lead) - assert result.mpc_accel_max is not None - recovery_limits.append(result.mpc_accel_max[0]) - - max_rejoin_step = URGENT_REJOIN_ACCEL_RATE * DT_MDL - assert all(abs(current - previous) <= max_rejoin_step + 1e-9 for previous, current in zip(recovery_limits[:-1], recovery_limits[1:], strict=True)) - assert all(ACCEL_MIN <= limit <= ACCEL_MAX for limit in recovery_limits) - assert min(recovery_limits) == pytest.approx(URGENT_REJOIN_ACCEL_MAX) - - matched_lead = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=20.0)) - matched = update(governor, matched_lead) - assert not governor.live.urgent_recovery_active - assert matched.mpc_accel_max is not None - assert matched.mpc_accel_max[0] > URGENT_REJOIN_ACCEL_MAX - - def test_renewed_urgent_closing_cancels_rejoin_ceiling(self): - governor = make_governor() - urgent_radar = make_radar(self.restrictive_lead) - for _ in range(3): - update(governor, urgent_radar) - moderate_lead = make_radar(make_lead(status=True, d_rel=180.0, v_lead_k=10.0)) - update(governor, moderate_lead) - assert governor.live.urgent_recovery_active - - renewed = update(governor, urgent_radar) - - assert renewed.required_decel > URGENT_BYPASS_REQUIRED_DECEL - assert governor.live.urgent_bypass_active - assert not governor.live.urgent_recovery_active - assert renewed.mpc_accel_max is None - assert not renewed.mpc_shape_cruise - assert renewed.lead_obstacle_weights == (1.0, 1.0) - - established = update(governor, urgent_radar) - assert established.mpc_accel_max is None - - def test_release_waits_for_confirmation_then_uses_profile_rate(self): - governor = make_governor() - radar_state = make_radar(self.restrictive_lead) - for _ in range(30): - update(governor, radar_state) - - result = update(governor) - while math.isfinite(result.live_filtered_cap): - result = update(governor) - - held_pace = result.live_pace - assert result.state == AccelControllerState.hold - - confirmation_updates = 0 - while result.state != AccelControllerState.release: - assert result.live_pace == held_pace - result = update(governor) - confirmation_updates += 1 - assert confirmation_updates < 20 - - assert confirmation_updates >= 6 - expected_rate = PROFILE_CONFIGS[AccelProfile.normal].release_rate - assert result.live_pace == pytest.approx(held_pace + expected_rate * DT_MDL) - - def test_confirmed_clear_closes_sub_deadband_hold_to_base(self): - governor = make_governor() - base_speed = 20.0 - governor.live.pace = base_speed - 0.10 - governor.live.state = AccelControllerState.hold - governor.live.accel_limit = HOLD_ACCEL_MAX - - result = update(governor, base_speed=base_speed) - - assert math.isinf(result.raw_energy_cap) - assert math.isinf(result.live_filtered_cap) - assert result.state == AccelControllerState.free - assert result.live_pace == base_speed - assert result.target_speed == base_speed - - def test_live_state_never_adopts_shadow_history(self): - governor = make_governor() - radar_state = make_radar(self.restrictive_lead) - for _ in range(20): - active = update(governor, radar_state) - assert active.live_pace < 20.0 - - shadow_only = update(governor, radar_state, acc_selected=False) - assert shadow_only.target_speed == 20.0 - assert shadow_only.state == AccelControllerState.inactive - assert math.isinf(shadow_only.live_pace) - assert shadow_only.shadow_pace < active.shadow_pace - - reactivated = update(governor, radar_state) - assert reactivated.live_pace == 20.0 - assert reactivated.target_speed == 20.0 - assert reactivated.shadow_pace < 20.0 - - def test_previous_lead_plan_synchronizes_pace_downward(self): - governor = make_governor() - update(governor, base_speed=30.0, v_ego=20.0, planner_speed=20.0) - - result = update(governor, base_speed=30.0, v_ego=20.0, planner_speed=15.0, previous_mpc_source=LongitudinalPlanSource.lead0) - - assert result.live_pace == 15.0 - assert result.target_speed == 15.0 - - def test_stop_hold_requires_four_confirmed_departure_frames(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - stop_args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.1} - - for radar_state in (stopped,) * 3 + (moving,) * 3: - result = update(governor, radar_state, **stop_args) - assert result.state == AccelControllerState.stopHold - assert not result.launching - assert result.live_pace == 0.0 - assert result.target_speed == 0.0 - assert result.effective_accel_max == 0.0 - assert_profile_trajectory(result, 0.0) - assert result.lead_obstacle_weights == (0.0, 0.0) - - departed = update(governor, moving, **stop_args) - assert departed.state == AccelControllerState.release - assert departed.launching - assert departed.live_pace == pytest.approx(stop_args["v_ego"] + LAUNCH_DELTA_V) - assert departed.target_speed == stop_args["base_speed"] - assert departed.effective_accel_max == pytest.approx(LAUNCH_ACCEL_RATE * DT_MDL) - assert_profile_trajectory(departed, departed.effective_accel_max) - assert 0.0 < departed.lead_obstacle_weights[0] < 1.0 - assert departed.lead_obstacle_weights[1] == 1.0 - - def test_creeping_lead_departure_confirms_before_bounded_launch(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=7)) - creeping = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=0.6, radar_track_id=7)) - stop_args = { - "base_speed": 5.0, - "v_ego": 0.1, - "planner_speed": 0.1, - "previous_mpc_source": LongitudinalPlanSource.lead0, - "previous_should_stop": True, - } - - for _ in range(3): - update(governor, stopped, **stop_args) - - departure = [update(governor, creeping, **stop_args) for _ in range(4)] - - for held in departure[:3]: - assert held.state == AccelControllerState.stopHold - assert not held.launching - assert held.live_pace == 0.0 - assert held.target_speed == 0.0 - - launched = departure[3] - assert launched.state == AccelControllerState.release - assert launched.launching - assert launched.target_speed == launched.base_speed - assert launched.live_pace == pytest.approx(launched.live_filtered_cap) - assert launched.live_pace >= 0.6 - assert launched.live_pace < launched.base_speed - assert 0.0 < launched.lead_obstacle_weights[0] < 1.0 - - def test_far_irrelevant_stopped_lead_does_not_block_departure(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - mixed = make_radar( - make_lead(status=True, d_rel=20.0, v_lead_k=5.0), - make_lead(status=True, d_rel=200.0, v_lead_k=0.0), - ) - args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0} - update(governor, stopped, **args) - - results = [update(governor, mixed, **args) for _ in range(4)] - - assert all(result.raw_energy_cap > 0.8 for result in results) - assert all(result.state == AccelControllerState.stopHold for result in results[:3]) - assert all(not result.launching for result in results[:3]) - assert results[3].state == AccelControllerState.release - assert results[3].launching - assert governor.live.departure_frames == 0 - - def test_stock_relevant_stopped_lead_two_blocks_departure(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - mixed = make_radar( - make_lead(status=True, d_rel=20.0, v_lead_k=5.0), - make_lead(status=True, d_rel=5.0, v_lead_k=0.0), - ) - args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0} - update(governor, stopped, **args) - - results = [update(governor, mixed, **args) for _ in range(5)] - - assert all(result.state == AccelControllerState.stopHold for result in results) - assert all(not result.launching for result in results) - assert governor.live.departure_frames == 0 - - @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_confirmed_departure_ramps_to_common_breakaway_then_profiles(self, profile): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0, "stock_accel_max": 2.0, "profile": profile} - - for _ in range(3): - update(governor, stopped, **args) - departure = [update(governor, moving, **args) for _ in range(4)] - - assert [result.target_speed for result in departure[:3]] == [0.0] * 3 - assert all(result.effective_accel_max == 0.0 for result in departure[:3]) - assert all(result.mpc_accel_max is not None for result in departure[:3]) - expected_launch_pace = min(args["base_speed"], departure[-1].live_filtered_cap, args["v_ego"] + LAUNCH_DELTA_V) - assert departure[-1].live_pace == pytest.approx(expected_launch_pace) - assert departure[-1].target_speed == args["base_speed"] - assert departure[-1].effective_accel_max == pytest.approx(LAUNCH_ACCEL_RATE * DT_MDL) - assert_profile_trajectory(departure[-1], departure[-1].effective_accel_max) - - still_breaking_away = update(governor, moving, **(args | {"v_ego": 0.04, "planner_speed": 0.04})) - assert still_breaking_away.launching - assert still_breaking_away.target_speed == args["base_speed"] - assert still_breaking_away.effective_accel_max == pytest.approx(2.0 * LAUNCH_ACCEL_RATE * DT_MDL) - assert_profile_trajectory(still_breaking_away, still_breaking_away.effective_accel_max) - - moving_result = update(governor, moving, **(args | {"v_ego": 0.05, "planner_speed": 0.05})) - assert not moving_result.launching - assert moving_result.mpc_accel_max is not None - assert moving_result.mpc_shape_cruise - - @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_departure_confirmation_uses_controlling_lead_speed_not_energy_cap(self, profile): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - barely_moving = make_radar(make_lead(status=True, d_rel=5.0, v_lead_k=0.35)) - args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0, "profile": profile} - update(governor, stopped, **args) - - confirmation = [update(governor, barely_moving, **args) for _ in range(4)] - - assert all(result.raw_energy_cap < 0.8 for result in confirmation) - assert all(result.state == AccelControllerState.stopHold for result in confirmation[:3]) - assert confirmation[3].state == AccelControllerState.release - assert confirmation[3].launching - - def test_departure_confirmation_must_be_four_consecutive_frames(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0} - update(governor, stopped, **args) - - assert all(update(governor, moving, **args).state == AccelControllerState.stopHold for _ in range(2)) - interrupted = update(governor, stopped, **args) - assert interrupted.state == AccelControllerState.stopHold - assert governor.live.departure_frames == 0 - - confirmation = [update(governor, moving, **args) for _ in range(4)] - assert all(result.state == AccelControllerState.stopHold for result in confirmation[:3]) - assert confirmation[3].state == AccelControllerState.release - - def test_stopped_lead_departure_releases_while_mpc_source_remains_lead(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - lead_args = { - "base_speed": 5.0, - "v_ego": 0.1, - "planner_speed": 0.0, - "previous_mpc_source": LongitudinalPlanSource.lead0, - } - - for _ in range(3): - update(governor, stopped, **lead_args) - - departure = [update(governor, moving, **lead_args) for _ in range(4)] - assert [result.live_pace for result in departure[:3]] == [0.0] * 3 - assert departure[3].live_pace > 0.0 - assert [result.target_speed for result in departure[:3]] == [0.0] * 3 - assert departure[-1].target_speed == lead_args["base_speed"] - assert len(departure) * DT_MDL < 1.0 - - continued_release = update(governor, moving, **lead_args) - assert continued_release.state == AccelControllerState.release - assert continued_release.live_pace > departure[-1].live_pace - - def test_stale_should_stop_does_not_restart_departure_confirmation(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - stale_stop_args = { - "base_speed": 5.0, - "v_ego": 0.1, - "planner_speed": 0.0, - "previous_mpc_source": LongitudinalPlanSource.lead0, - "previous_should_stop": True, - } - - for _ in range(3): - update(governor, stopped, **stale_stop_args) - - departure = [update(governor, moving, **stale_stop_args) for _ in range(4)] - assert [result.live_pace for result in departure[:3]] == [0.0] * 3 - assert departure[3].live_pace > 0.0 - assert len(departure) * DT_MDL < 1.0 - - continued_paces = [update(governor, moving, **stale_stop_args).live_pace for _ in range(60)] - assert all(current >= previous for previous, current in zip(continued_paces[:-1], continued_paces[1:], strict=True)) - assert continued_paces[-1] > departure[-1].live_pace - - def test_renewed_stopped_lead_interrupts_confirmed_departure(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - stale_stop_args = { - "base_speed": 5.0, - "v_ego": 0.1, - "planner_speed": 0.0, - "previous_mpc_source": LongitudinalPlanSource.lead0, - "previous_should_stop": True, - } - - for _ in range(3): - update(governor, stopped, **stale_stop_args) - for _ in range(8): - departing = update(governor, moving, **stale_stop_args) - assert departing.target_speed > 0.0 - - renewed_stop = update(governor, stopped, **stale_stop_args) - assert renewed_stop.state == AccelControllerState.stopHold - assert renewed_stop.live_pace == 0.0 - assert renewed_stop.target_speed == 0.0 - assert renewed_stop.effective_accel_max == 0.0 - assert_profile_trajectory(renewed_stop, 0.0) - assert not governor.live.departing_from_stop - - def test_low_speed_moving_lead_never_bypasses_bounded_pace(self): - governor = make_governor() - noisy_moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5)) - - results = [update(governor, noisy_moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0) for _ in range(8)] - first, second = results[:2] - - assert first.selected_lead == 0 - assert first.live_pace == 0.0 - assert first.target_speed == first.live_pace - assert not governor.live.stopped_lead_hold - assert second.target_speed == second.live_pace - assert second.target_speed >= first.target_speed - assert results[-1].target_speed > 0.0 - assert results[-1].target_speed <= noisy_moving_lead.leadOne.vLeadK - - def test_real_stopped_evidence_latches_hold_after_noisy_first_frame(self): - governor = make_governor() - noisy_moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5)) - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - args = {"base_speed": 5.0, "v_ego": 0.0, "planner_speed": 0.0} - - initial_noise = update(governor, noisy_moving_lead, **args) - stopped_evidence = update(governor, stopped, **args) - repeated_noise = update(governor, noisy_moving_lead, **args) - - assert initial_noise.target_speed == initial_noise.live_pace - assert stopped_evidence.state == AccelControllerState.stopHold - assert governor.live.stopped_lead_hold - assert stopped_evidence.target_speed == 0.0 - assert repeated_noise.target_speed == 0.0 - assert_profile_trajectory(stopped_evidence, 0.0) - assert_profile_trajectory(repeated_noise, 0.0) - - def test_later_continuously_moving_lead_does_not_latch_stopped_hold(self): - governor = make_governor() - moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5)) - update(governor, base_speed=5.0, v_ego=1.0, planner_speed=1.0) - - observations = [update(governor, moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0) for _ in range(3)] - settled = observations[-1] - - assert settled.selected_lead == 0 - assert not governor.live.stopped_lead_hold - assert settled.target_speed == settled.live_pace - assert observations[0].target_speed == moving_lead.leadOne.vLeadK - assert settled.target_speed < settled.base_speed - - def test_stop_hold_dropout_pins_target_without_losing_hold_state(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - stop_args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.1} - for _ in range(3): - update(governor, stopped, **stop_args) - - dropout = update(governor, **stop_args) - - assert dropout.selected_lead == -1 - assert dropout.state == AccelControllerState.stopHold - assert dropout.live_pace == 0.0 - assert dropout.target_speed == 0.0 - assert_profile_trajectory(dropout, 0.0) - assert governor.live.stopped_lead_hold - - @pytest.mark.parametrize("profile", list(AccelProfile)) - def test_no_lead_start_uses_solver_safe_seed_then_common_breakaway_floor(self, profile): - governor = make_governor() - - args = {"base_speed": 5.0, "v_ego": 0.0, "planner_speed": 0.0, "profile": profile} - first, second, third = [update(governor, **args) for _ in range(3)] - - assert first.selected_lead == -1 - assert first.launching - assert first.live_pace == first.base_speed - assert first.target_speed == first.base_speed - assert first.effective_accel_max == INITIAL_LAUNCH_ACCEL_MAX - assert second.effective_accel_max == pytest.approx(INITIAL_LAUNCH_ACCEL_MAX + CLEAR_LAUNCH_ACCEL_RATE * DT_MDL) - assert third.effective_accel_max == BREAKAWAY_ACCEL_MAX - assert_profile_trajectory(first, INITIAL_LAUNCH_ACCEL_MAX) - assert_profile_trajectory(third, BREAKAWAY_ACCEL_MAX) - - def test_confirmed_departure_holds_base_target_then_hands_off_to_launch_preview(self): - governor = make_governor() - stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) - moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) - lead_args = { - "base_speed": 5.0, - "v_ego": 0.04, - "planner_speed": 0.0, - "previous_mpc_source": LongitudinalPlanSource.lead0, - "previous_should_stop": True, - } - for _ in range(3): - update(governor, stopped, **lead_args) - for _ in range(8): - departing = update(governor, moving, **lead_args) - assert governor.live.departing_from_stop - - handed_back = update(governor, moving, **(lead_args | {"v_ego": 0.05, "planner_speed": 0.05})) - - assert not governor.live.departing_from_stop - assert not governor.live.stopped_lead_hold - expected_step = PROFILE_CONFIGS[AccelProfile.normal].release_rate * DT_MDL - assert handed_back.live_pace == pytest.approx(departing.live_pace + expected_step) - assert handed_back.live_pace < min(handed_back.base_speed, handed_back.live_filtered_cap) - assert handed_back.target_speed == handed_back.base_speed - assert handed_back.mpc_accel_max is not None - assert handed_back.mpc_shape_cruise - - handoff = update(governor, moving, **(lead_args | {"v_ego": 1.0, "planner_speed": 1.0})) - - assert not governor.live.launch_target_active - assert min(handoff.base_speed, 1.0 + LAUNCH_DELTA_V) <= handoff.live_pace <= handoff.base_speed - assert handoff.target_speed == handoff.live_pace + assert envelope == EnergyEnvelope() @pytest.mark.parametrize( - "bypass", + "field, invalid", [ - {"enabled": False}, - {"acc_selected": False}, - {"engaged": False}, - {"cruise_initialized": False}, - {"controller_fault": True}, - {"a_ego": math.nan}, + ("dRel", -0.01), + ("vLeadK", -1.0), + ("aLeadTau", 0.0), + ("aLeadTau", -0.01), + ("aLeadTau", 1e6), ], ) - def test_bypass_returns_base_and_resets_live(self, bypass): - governor = make_governor() - radar_state = make_radar(self.restrictive_lead) - for _ in range(5): - update(governor, radar_state) + def test_physically_invalid_lead_values_are_ignored(self, field, invalid): + lead = make_lead(status=True, d_rel=40.0, v_lead_k=10.0) + setattr(lead, field, invalid) - result = update(governor, radar_state, **bypass) + envelope = make_controller().calculate_energy_envelope(make_radar(lead), 20.0, 0.0, AccelProfile.normal) + + assert envelope == EnergyEnvelope() + + @pytest.mark.parametrize( + "malformed_lead", + [ + SimpleNamespace(status=True, dRel=40.0, vLeadK=10.0, aLeadK=0.0), + SimpleNamespace(status=True, dRel="invalid", vLeadK=10.0, aLeadK=0.0, aLeadTau=1.5), + SimpleNamespace(status=True, dRel=40.0, vLeadK=None, aLeadK=0.0, aLeadTau=1.5), + ], + ) + def test_malformed_lead_does_not_hide_valid_second_lead(self, malformed_lead): + valid_lead = make_lead(status=True, d_rel=35.0, v_lead_k=5.0) + + envelope = make_controller().calculate_energy_envelope( + make_radar(malformed_lead, valid_lead), + 20.0, + 0.0, + AccelProfile.normal, + ) + + assert envelope.selected_lead == 1 + assert math.isfinite(envelope.cap) + + @pytest.mark.parametrize("invalid_column", [0, 1]) + @pytest.mark.parametrize("invalid", [math.nan, math.inf, -math.inf]) + def test_nonfinite_projected_lead_is_ignored(self, monkeypatch, invalid_column, invalid): + projected = np.column_stack((np.full_like(T_IDXS, 40.0), np.full_like(T_IDXS, 10.0))) + projected[:, invalid_column] = invalid + monkeypatch.setattr(LongitudinalMpc, "extrapolate_lead", staticmethod(lambda *_args: projected)) + + envelope = make_controller().calculate_energy_envelope( + make_radar(make_lead(status=True, d_rel=40.0, v_lead_k=10.0)), + 20.0, + 0.0, + AccelProfile.normal, + ) + + assert envelope == EnergyEnvelope() + + +class TestPaceGovernor: + restrictive_lead = make_lead(status=True, d_rel=40.0, v_lead_k=5.0) + nonrestrictive_lead = make_lead(status=True, d_rel=100.0, v_lead_k=15.0) + gentle_restrictive_lead = make_lead(status=True, d_rel=60.0, v_lead_k=8.0) + + @classmethod + def establish_gentle_restriction(cls, controller, frames=6): + for _ in range(CAP_FILTER_FRAMES // 2 + 1): + update(controller, make_radar(cls.nonrestrictive_lead)) + return [update(controller, make_radar(cls.gentle_restrictive_lead)) for _ in range(frames)] + + def test_clear_road_targets_base_immediately(self): + controller = make_controller() + result = update(controller, base_speed=25.0, v_ego=0.0, planner_speed=0.0, stock_accel_max=1.6) + settled = [update(controller, base_speed=25.0, v_ego=0.0, planner_speed=0.0, stock_accel_max=1.6) for _ in range(13)] + + assert result.target_speed == 25.0 + assert result.live_pace == 25.0 + assert result.effective_accel_max == 0.95 + assert settled[-1].effective_accel_max == 1.6 + assert all(sample.mpc_accel_max is not None for sample in [result, *settled]) + assert all(max(sample.mpc_accel_max) <= min(sample.profile_accel_max, 1.6, ACCEL_MAX) for sample in [result, *settled]) + assert all(sample.target_speed == 25.0 for sample in settled) + + def test_median_needs_three_leads_and_holds_two_dropouts(self): + controller = make_controller() + radar = make_radar(self.restrictive_lead) + acquisitions = [update(controller, radar) for _ in range(3)] + dropouts = [update(controller) for _ in range(3)] + + assert all(math.isinf(result.live_filtered_cap) for result in acquisitions[:2]) + assert math.isfinite(acquisitions[2].live_filtered_cap) + assert all(math.isfinite(result.live_filtered_cap) for result in dropouts[:2]) + assert math.isinf(dropouts[2].live_filtered_cap) + + def test_invalid_lead_expires_and_cannot_resurrect_filtered_geometry(self): + controller = make_controller() + valid = make_radar(self.restrictive_lead) + invalid = make_radar(make_lead(status=True, d_rel=-1.0, v_lead_k=5.0)) + + acquired = [update(controller, valid) for _ in range(CAP_FILTER_FRAMES // 2 + 1)] + expired = [update(controller, invalid) for _ in range(CAP_FILTER_FRAMES // 2 + 1)] + reacquired = [update(controller, valid) for _ in range(CAP_FILTER_FRAMES // 2 + 1)] + + assert math.isfinite(acquired[-1].live_filtered_cap) + assert math.isinf(expired[-1].live_filtered_cap) + assert all(math.isinf(result.live_filtered_cap) for result in reacquired[:-1]) + assert math.isfinite(reacquired[-1].live_filtered_cap) + + @pytest.mark.parametrize( + "field, invalid", + [ + ("dRel", -1.0), + ("vLeadK", -1.0), + ("aLeadTau", -1.0), + ("aLeadTau", math.inf), + ], + ) + def test_invalid_lead_never_produces_nan_telemetry(self, field, invalid): + lead = make_lead(status=True, d_rel=40.0, v_lead_k=5.0) + setattr(lead, field, invalid) + + result = update(make_controller(), make_radar(lead)) + + telemetry = ( + result.target_speed, + result.profile_accel_max, + result.effective_accel_max, + result.raw_energy_cap, + result.live_filtered_cap, + result.live_pace, + result.closing_speed, + result.required_decel, + ) + assert not any(math.isnan(value) for value in telemetry) + + def test_small_negative_stopped_lead_speed_is_treated_as_zero(self): + lead = make_lead(status=True, d_rel=6.0, v_lead_k=-0.01) + + result = update(make_controller(), make_radar(lead), base_speed=8.0, v_ego=0.1, planner_speed=0.1) + + assert result.selected_lead == 0 + assert result.raw_energy_cap == 0.0 + assert result.state == AccelControllerState.stopHold + + def test_restriction_changes_only_pace_at_comfort_rate(self): + controller = make_controller() + results = self.establish_gentle_restriction(controller, frames=3) + restricted = results[-1] + + assert all(result.live_pace == 20.0 for result in results[:2]) + assert restricted.live_pace == pytest.approx(20.0 - PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL) + assert restricted.state == AccelControllerState.restrict + assert restricted.mpc_accel_max is None + assert not restricted.mpc_shape_cruise + assert not restricted.mpc_apply_accel_constraint + + def test_far_slower_lead_acquisition_is_bounded_then_holds_for_mpc(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=200.0, v_lead_k=15.0)) + results = [update(controller, radar, base_speed=30.0, v_ego=25.0, planner_speed=25.0) for _ in range(3)] + + assert all(result.live_pace == 30.0 for result in results[:2]) + assert 24.5 <= results[-1].live_pace < 25.0 + assert results[-1].state == AccelControllerState.hold + assert results[-1].mpc_accel_max is None + assert not results[-1].mpc_apply_accel_constraint + + def test_material_closing_lead_returns_acceleration_authority_to_stock(self): + controller = make_controller() + radar = make_radar(self.restrictive_lead) + results = [update(controller, radar) for _ in range(20)] + + assert all(0.0 < result.effective_accel_max <= min(result.profile_accel_max, 1.2, ACCEL_MAX) for result in results) + assert all(result.mpc_accel_max is None for result in results) + assert all(not result.mpc_shape_cruise for result in results) + assert all(not result.mpc_apply_accel_constraint for result in results) + + def test_route_threshold_crossing_keeps_direct_stock_authority(self): + controller = make_controller() + leads = [ + make_radar(make_lead(status=True, d_rel=31.0, v_lead_k=5.0)), + make_radar(make_lead(status=True, d_rel=29.0, v_lead_k=5.0)), + ] + results = [update(controller, radar, v_ego=10.0, planner_speed=10.0) for radar in leads] + + assert all(result.mpc_accel_max is None for result in results) + assert all(not result.mpc_shape_cruise for result in results) + assert all(not result.mpc_apply_accel_constraint for result in results) + + def test_decelerating_moving_lead_uses_smooth_ceiling_until_confirmed_match(self): + controller = make_controller() + + def moving_lead(speed): + return make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=speed)) + + update(controller, moving_lead(15.0), base_speed=30.0, v_ego=20.0, planner_speed=20.0) + confirmation = [] + for frame in range(MOVING_LEAD_DECEL_CONFIRM_FRAMES): + confirmation.append( + update( + controller, + moving_lead(15.0 - 0.01 * (frame + 1)), + base_speed=30.0, + v_ego=20.0, + planner_speed=20.0, + ) + ) + if frame < MOVING_LEAD_DECEL_CONFIRM_FRAMES - 1: + assert not controller.live.moving_lead_decel + + assert controller.live.moving_lead_decel + assert confirmation[-1].effective_accel_max == pytest.approx(-MOVING_LEAD_DECEL_ACCEL_SLEW_RATE * DT_MDL) + assert confirmation[-1].mpc_apply_accel_constraint + + falling_speed = 15.0 - 0.01 * MOVING_LEAD_DECEL_CONFIRM_FRAMES + constrained = [update(controller, moving_lead(falling_speed - 0.01 * (frame + 1)), base_speed=30.0, v_ego=20.0, planner_speed=20.0) for frame in range(20)] + limits = np.array([result.effective_accel_max for result in [confirmation[-1], *constrained]]) + assert constrained[-1].effective_accel_max == MOVING_LEAD_DECEL_ACCEL_MAX + assert np.all(np.diff(limits) <= 0.0) + assert np.max(np.abs(np.diff(limits))) <= MOVING_LEAD_DECEL_ACCEL_SLEW_RATE * DT_MDL + 1e-9 + + matched = [update(controller, moving_lead(5.0), base_speed=30.0, v_ego=5.05, planner_speed=5.05) for _ in range(MOVING_LEAD_DECEL_EXIT_FRAMES)] + assert all(result.effective_accel_max <= 0.0 for result in matched[:-1]) + assert not controller.live.moving_lead_decel + assert controller.live.moving_lead_accel_max is None + assert matched[-1].effective_accel_max > 0.0 + + def test_far_or_noisy_decelerating_lead_does_not_force_braking(self): + far_controller = make_controller() + far_results = [ + update( + far_controller, + make_radar(make_lead(status=True, d_rel=500.0, v_lead_k=15.0 - 0.01 * frame)), + base_speed=30.0, + v_ego=20.0, + planner_speed=20.0, + ) + for frame in range(10) + ] + + noisy_controller = make_controller() + noisy_results = [ + update( + noisy_controller, + make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0 - 0.01 * (frame % 2))), + base_speed=30.0, + v_ego=20.0, + planner_speed=20.0, + ) + for frame in range(10) + ] + + assert not far_controller.live.moving_lead_decel + assert not noisy_controller.live.moving_lead_decel + assert all(result.effective_accel_max >= 0.0 for result in [*far_results, *noisy_results]) + + def test_release_waits_then_raises_pace_at_profile_rate(self): + controller = make_controller() + restricted = self.establish_gentle_restriction(controller, frames=30)[-1] + restricted_pace = restricted.live_pace + + confirmation_frames = math.ceil(PROFILE_CONFIGS[AccelProfile.normal].release_confirm / DT_MDL) + results = [update(controller) for _ in range(CAP_FILTER_FRAMES // 2 + confirmation_frames)] + released = results[-1] + + assert all(result.state == AccelControllerState.hold for result in results[:-1]) + assert all(result.live_pace == restricted_pace for result in results[:-1]) + assert released.state == AccelControllerState.release + assert released.live_pace > restricted_pace + assert released.live_pace == pytest.approx(restricted_pace + PROFILE_CONFIGS[AccelProfile.normal].release_rate * DT_MDL) + + def test_confirmed_clear_finishes_release_at_base(self): + controller = make_controller() + self.establish_gentle_restriction(controller, frames=30) + + results = [update(controller) for _ in range(200)] + + assert results[-1].state == AccelControllerState.free + assert results[-1].target_speed == results[-1].base_speed + + def test_cruise_source_dropout_holds_pace_without_a_target_lurch(self): + controller = make_controller() + restricted = self.establish_gentle_restriction(controller)[-1] + + dropout = update( + controller, + previous_mpc_source=LongitudinalPlanSource.cruise, + planner_speed=7.0, + v_ego=8.0, + ) + + assert math.isfinite(dropout.live_filtered_cap) + assert dropout.state == AccelControllerState.hold + assert dropout.live_pace == restricted.live_pace + assert dropout.target_speed == restricted.target_speed + + def test_far_geometry_jump_cannot_release_filtered_restriction(self): + controller = make_controller() + far = make_radar(make_lead(status=True, d_rel=200.0, v_lead_k=20.0)) + restricted = self.establish_gentle_restriction(controller)[-1] + + jumped = update(controller, far, planner_speed=8.0, v_ego=9.0) + + assert math.isfinite(jumped.live_filtered_cap) + assert jumped.state == AccelControllerState.hold + assert jumped.live_pace == restricted.live_pace + assert jumped.target_speed == restricted.target_speed + + def test_previous_lead_plan_synchronizes_pace_after_lead_loss(self): + controller = make_controller() + self.establish_gentle_restriction(controller) + + lost = update( + controller, + previous_mpc_source=LongitudinalPlanSource.lead0, + planner_speed=7.0, + ) + + assert lost.state == AccelControllerState.hold + assert lost.live_pace == 7.0 + assert lost.target_speed == 7.0 + + def test_existing_lead_plan_keeps_relative_pace_restriction(self): + controller = make_controller() + restricted = self.establish_gentle_restriction(controller)[-1] + radar = make_radar(self.gentle_restrictive_lead) + + handed_off = update( + controller, + radar, + previous_mpc_source=LongitudinalPlanSource.lead0, + planner_speed=restricted.live_pace, + ) + + assert restricted.live_pace < restricted.base_speed + assert handed_off.state == AccelControllerState.restrict + assert handed_off.live_pace <= restricted.live_pace + assert handed_off.target_speed < handed_off.base_speed + assert handed_off.mpc_accel_max is None + assert not handed_off.mpc_apply_accel_constraint + + def test_stop_hold_requires_four_moving_lead_frames_then_targets_base(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) + held = update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6) + waiting = [update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6) for _ in range(13)] + confirmations = [update(controller, moving, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6) for _ in range(STOP_HOLD_EXIT_FRAMES)] + released = update(controller, moving, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6) + + assert held.state == AccelControllerState.stopHold + assert held.target_speed == 0.0 + held_results = [held, *waiting, *confirmations[:-1]] + assert all(result.effective_accel_max == 0.0 for result in held_results) + assert all(result.mpc_accel_max[0] < 0.0 and result.mpc_accel_max[-1] == ACCEL_MAX for result in held_results) + assert all(result.mpc_shape_cruise for result in held_results) + assert all(result.mpc_apply_accel_constraint for result in held_results) + assert all(result.target_speed == 0.0 for result in confirmations[:-1]) + assert confirmations[-1].target_speed > 0.0 + assert confirmations[-1].effective_accel_max <= ACCEL_MAX + assert confirmations[-1].mpc_accel_max is None + assert confirmations[-1].launching + assert released.target_speed >= confirmations[-1].target_speed + assert released.launching + + def test_slow_queue_creep_exits_after_four_frames(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + creeping = make_radar(make_lead(status=True, d_rel=6.1, v_lead_k=0.5)) + update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1) + results = [update(controller, creeping, base_speed=8.0, v_ego=0.1, planner_speed=0.1) for _ in range(STOP_HOLD_EXIT_FRAMES)] + released = update(controller, creeping, base_speed=8.0, v_ego=0.1, planner_speed=0.1) + + assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) + assert results[-1].state == AccelControllerState.release + assert results[-1].target_speed > 0.0 + assert released.target_speed > 0.0 + + def test_far_stopped_lead_does_not_enter_stop_hold(self): + far_stopped = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=0.0)) + + result = update(make_controller(), far_stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1) + + assert result.state != AccelControllerState.stopHold + assert result.target_speed == 8.0 + + def test_departure_confirmation_must_be_consecutive(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) + update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1) + for _ in range(STOP_HOLD_EXIT_FRAMES - 1): + update(controller, moving, base_speed=8.0, v_ego=0.1, planner_speed=0.1) + interrupted = update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1) + + assert interrupted.state == AccelControllerState.stopHold + assert controller.live.departure_frames == 0 + + def test_stale_should_stop_does_not_reenter_after_confirmed_departure(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) + update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1, previous_should_stop=True) + for _ in range(STOP_HOLD_EXIT_FRAMES): + result = update( + controller, + moving, + base_speed=8.0, + v_ego=0.1, + planner_speed=0.1, + previous_should_stop=True, + ) + released = update( + controller, + moving, + base_speed=8.0, + v_ego=0.1, + planner_speed=0.1, + previous_should_stop=True, + ) + + assert result.state != AccelControllerState.stopHold + assert result.target_speed > 0.0 + assert released.state != AccelControllerState.stopHold + assert released.target_speed > 0.0 + + def test_live_never_adopts_shadow_history(self): + controller = make_controller() + for _ in range(CAP_FILTER_FRAMES // 2 + 1): + update(controller, make_radar(self.nonrestrictive_lead), enabled=False) + for _ in range(CAP_FILTER_FRAMES // 2 + 1): + shadow = update(controller, make_radar(self.gentle_restrictive_lead), enabled=False) + active = update(controller, enabled=True) + + assert shadow.shadow_state == AccelControllerState.restrict + assert active.target_speed == active.base_speed + assert active.live_pace == active.base_speed + + @pytest.mark.parametrize("bypass", [{"enabled": False}, {"acc_selected": False}, {"engaged": False}]) + def test_bypass_is_exact_base_and_resets_live(self, bypass): + controller = make_controller() + update(controller, make_radar(self.restrictive_lead)) + result = update(controller, **bypass) - assert result.target_speed == 20.0 assert not result.active - assert result.state == AccelControllerState.inactive - assert math.isinf(result.live_pace) - assert governor.live.accel_limit is None - assert math.isinf(result.effective_accel_max) + assert result.target_speed == result.base_speed assert result.mpc_accel_max is None - assert not result.mpc_shape_cruise + assert math.isinf(result.effective_accel_max) + assert controller.live.pace is None - def test_disabled_acc_mode_keeps_shadow_running(self): - governor = make_governor() - radar_state = make_radar(self.restrictive_lead) + @pytest.mark.parametrize("invalid", [{"base_speed": math.nan}, {"stock_accel_max": math.nan}, {"controller_fault": True}]) + def test_invalid_context_resets_without_changing_base(self, invalid): + controller = make_controller() + update(controller) + result = update(controller, **invalid) - results = [update(governor, radar_state, enabled=False) for _ in range(3)] + assert not result.active + assert controller.live.pace is None + if "base_speed" not in invalid: + assert result.target_speed == result.base_speed - assert all(not result.active for result in results) - assert all(result.shadow_active for result in results) - assert results[-1].shadow_state == AccelControllerState.restrict - assert math.isfinite(results[-1].shadow_filtered_cap) + recovered = update(controller) + assert recovered.active + assert recovered.live_pace == recovered.base_speed + + def test_fault_recovery_is_discarded_when_context_is_invalid(self): + controller = make_controller() + for _ in range(ACCEL_SHAPE_WARMUP_FRAMES + 1): + update(controller) + assert controller.recovery_accel_max is not None + + result = update(controller, controller_fault=True, base_speed=math.nan) + assert not result.active + assert result.mpc_accel_max is None + assert not controller.recovery_available def test_invalid_profile_defaults_to_normal(self): - governor = make_governor() - - result = update(governor, profile=99) + result = update(make_controller(), profile=99) assert result.profile == AccelProfile.normal - def test_small_negative_ego_speed_is_sanitized_without_resetting_state(self): - governor = make_governor() - update(governor) - cap_samples = governor.live.cap_samples - - result = update(governor, v_ego=-0.04, planner_speed=0.0) + def test_small_negative_ego_noise_is_sanitized(self): + result = update(make_controller(), v_ego=-0.05, planner_speed=0.0, stock_accel_max=1.6) assert result.active - assert math.isfinite(result.live_pace) - assert governor.live.cap_samples is cap_samples - - def test_negative_ego_speed_below_noise_tolerance_resets_live_state(self): - governor = make_governor() - update(governor) - cap_samples = governor.live.cap_samples - - result = update(governor, v_ego=-0.101, planner_speed=0.0) - - assert not result.active - assert math.isinf(result.live_pace) - assert governor.live.cap_samples is not cap_samples - - def test_invalid_delay_resets_and_bypasses(self): - governor = AccelController(SimpleNamespace(longitudinalActuatorDelay=None)) - - result = update(governor) - - assert result.target_speed == 20.0 - assert not result.active - assert not result.shadow_active - - def test_nonfinite_base_is_preserved_on_bypass(self): - governor = make_governor() - - result = update(governor, base_speed=math.nan) - - assert math.isnan(result.target_speed) - assert not result.active + assert result.profile_accel_max == ACCEL_PROFILE_MAX_V[AccelProfile.normal][0] def test_radar_input_is_not_mutated(self): - governor = make_governor() - lead = make_lead(status=True, d_rel=50.0, v_lead_k=10.0, a_lead_k=-2.0, a_lead_tau=1.2) - radar_state = make_radar(lead) - before = (lead.status, lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau) + lead = make_lead(status=True, d_rel=60.0, v_lead_k=10.0, a_lead_k=-2.0) + radar = make_radar(lead) + before = vars(lead).copy() - update(governor, radar_state) + update(make_controller(), radar) - assert (lead.status, lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau) == before + assert vars(lead) == before diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py index f2a6a7ca78..678d0e00ff 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py @@ -104,37 +104,11 @@ def test_mpc_missing_or_invalid_preshape_is_exact_stock(accel_max): np.testing.assert_array_equal(mpc.params, stock_params) -def test_mpc_benign_lead_weight_softens_only_optimization_obstacle(): +def test_mpc_profile_preshape_keeps_raw_lead_obstacle_authoritative(): radar_state = messaging.new_message('radarState').radarState radar_state.leadOne.status = True - radar_state.leadOne.dRel = 60.0 - radar_state.leadOne.vLead = 15.0 - radar_state.leadOne.vLeadK = 15.0 - radar_state.leadOne.aLeadK = 0.0 - radar_state.leadOne.aLeadTau = 1.0 - mpc = LongitudinalMpc() - mpc.set_cur_state(20.0, 0.0) - mpc.run = lambda: None - - mpc.update(radar_state, 30.0, lead_obstacle_weights=(1.0, 1.0)) - full_authority_params = mpc.params.copy() - lead_before = (radar_state.leadOne.dRel, radar_state.leadOne.vLead, radar_state.leadOne.aLeadK) - mpc.update(radar_state, 30.0, lead_obstacle_weights=(0.2, 1.0)) - softened_params = mpc.params.copy() - - assert softened_params[0, 2] > full_authority_params[0, 2] - np.testing.assert_array_equal(softened_params[:, :2], full_authority_params[:, :2]) - np.testing.assert_array_equal(softened_params[:, 3:], full_authority_params[:, 3:]) - np.testing.assert_array_equal(mpc.lead_obstacle_weights, [0.2, 1.0]) - assert (radar_state.leadOne.dRel, radar_state.leadOne.vLead, radar_state.leadOne.aLeadK) == lead_before - - -@pytest.mark.parametrize("weights", [(1.0,), (np.nan, 1.0), (np.inf, 1.0)]) -def test_mpc_invalid_lead_weights_are_exact_full_authority(weights): - radar_state = messaging.new_message('radarState').radarState - radar_state.leadOne.status = True - radar_state.leadOne.dRel = 60.0 - radar_state.leadOne.vLead = 15.0 + radar_state.leadOne.dRel = 30.0 + radar_state.leadOne.vLead = 5.0 radar_state.leadOne.aLeadK = 0.0 radar_state.leadOne.aLeadTau = 1.0 mpc = LongitudinalMpc() @@ -143,12 +117,14 @@ def test_mpc_invalid_lead_weights_are_exact_full_authority(weights): mpc.update(radar_state, 30.0) stock_params = mpc.params.copy() stock_source = mpc.source + lead_before = (radar_state.leadOne.dRel, radar_state.leadOne.vLead, radar_state.leadOne.aLeadK) - mpc.update(radar_state, 30.0, lead_obstacle_weights=weights) + mpc.update(radar_state, 30.0, accel_max=np.full(N + 1, 0.8), shape_accel_max_in_cruise=True) - np.testing.assert_array_equal(mpc.params, stock_params) + np.testing.assert_array_equal(mpc.params[:, 0], stock_params[:, 0]) + np.testing.assert_array_equal(mpc.params[:, 3:], stock_params[:, 3:]) assert mpc.source == stock_source - np.testing.assert_array_equal(mpc.lead_obstacle_weights, [1.0, 1.0]) + assert (radar_state.leadOne.dRel, radar_state.leadOne.vLead, radar_state.leadOne.aLeadK) == lead_before def test_shadow_target_telemetry_publishes_filtered_cap(): diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 117cff6c86..21a03bc725 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -98,22 +98,11 @@ class LongitudinalPlannerSP: acc_selected: bool, planner_speed: float, previous_mpc_source, previous_should_stop: bool, stock_accel_max: float, planner_accel: float, controller_fault: bool = False) -> float: self.accel_controller_result = self.accel_controller.update( - sm['radarState'], - base_speed=base_speed, - v_ego=sm['carState'].vEgo, - a_ego=sm['carState'].aEgo, - profile=self.accel_personality, - follow_personality=sm['selfdriveState'].personality, - enabled=self.accel_personality_enabled, - acc_selected=acc_selected, - engaged=engaged, - cruise_initialized=cruise_initialized, - previous_mpc_source=previous_mpc_source, - planner_speed=planner_speed, - stock_accel_max=stock_accel_max, - planner_accel=planner_accel, - previous_should_stop=previous_should_stop, - controller_fault=controller_fault, + sm['radarState'], base_speed=base_speed, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo, + profile=self.accel_personality, follow_personality=sm['selfdriveState'].personality, + enabled=self.accel_personality_enabled, acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized, + previous_mpc_source=previous_mpc_source, planner_speed=planner_speed, stock_accel_max=stock_accel_max, + planner_accel=planner_accel, previous_should_stop=previous_should_stop, controller_fault=controller_fault, ) return self.accel_controller_result.target_speed @@ -140,7 +129,7 @@ class LongitudinalPlannerSP: dec.enabled = self.dec.enabled() dec.active = self.dec.active() - # Accel Controller relative-pace governor + # Accel Controller if self.accel_controller_result is not None: result = self.accel_controller_result accel_controller = longitudinalPlanSP.accelController diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 884106017a..c7ef0fa75e 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -4,13 +4,16 @@ from dataclasses import dataclass import numpy as np import pytest -from opendbc.car.interfaces import ACCEL_MIN +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState +from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import MOVING_LEAD_DECEL_ACCEL_MAX + +ROUTINE_GAP_TOLERANCE = 0.02 @dataclass @@ -27,26 +30,20 @@ class ClosedLoopTrace: active: np.ndarray shadow_active: np.ndarray launching: np.ndarray - urgent_bypass: np.ndarray - urgent_recovery: np.ndarray + target_speed: np.ndarray pace: np.ndarray + raw_cap: np.ndarray filtered_cap: np.ndarray selected_lead: np.ndarray profile_accel_max: np.ndarray effective_accel_max: np.ndarray - controller_fault: np.ndarray - actuator_command: np.ndarray - applied_actuator_command: np.ndarray - observed_speed: np.ndarray - observed_acceleration: np.ndarray - lead_obstacle_weight_0: np.ndarray - lead_obstacle_weight_1: np.ndarray state: np.ndarray required_decel: np.ndarray + controller_fault: np.ndarray solver_failures: int -def _set_accel_controller_params(*, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None: +def _set_params(*, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None: params = Params() params.put_bool("AccelPersonalityEnabled", enabled, block=True) params.put("AccelPersonality", profile, block=True) @@ -63,8 +60,9 @@ def _run( dec_enabled: bool = False, **plant_kwargs, ) -> ClosedLoopTrace: - _set_accel_controller_params(enabled=controller_enabled, profile=profile, dec_enabled=dec_enabled) + _set_params(enabled=controller_enabled, profile=profile, dec_enabled=dec_enabled) plant = Plant(**plant_kwargs) + plant.v_lead_prev = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(0.0)) solver_failures = 0 original_mpc_reset = plant.planner.mpc.reset @@ -95,22 +93,16 @@ def _run( controller.active, controller.shadow_active, controller.launching, - plant.planner.accel_controller.live.urgent_bypass_active, - plant.planner.accel_controller.live.urgent_recovery_active, + controller.target_speed, controller.live_pace, + controller.raw_energy_cap, controller.live_filtered_cap, controller.selected_lead, controller.profile_accel_max, controller.effective_accel_max, - controller_fault, - result["actuator_command"], - result["applied_actuator_command"], - result["observed_v_ego"], - result["observed_a_ego"], - controller.lead_obstacle_weights[0], - controller.lead_obstacle_weights[1], controller.state, controller.required_decel, + controller_fault, ) ) sources.append(result["mpc_source"]) @@ -129,22 +121,16 @@ def _run( active=data[:, 8].astype(bool), shadow_active=data[:, 9].astype(bool), launching=data[:, 10].astype(bool), - urgent_bypass=data[:, 11].astype(bool), - urgent_recovery=data[:, 12].astype(bool), - pace=data[:, 13], + target_speed=data[:, 11], + pace=data[:, 12], + raw_cap=data[:, 13], filtered_cap=data[:, 14], selected_lead=data[:, 15].astype(int), profile_accel_max=data[:, 16], effective_accel_max=data[:, 17], - controller_fault=data[:, 18].astype(bool), - actuator_command=data[:, 19], - applied_actuator_command=data[:, 20], - observed_speed=data[:, 21], - observed_acceleration=data[:, 22], - lead_obstacle_weight_0=data[:, 23], - lead_obstacle_weight_1=data[:, 24], - state=data[:, 25].astype(int), - required_decel=data[:, 26], + state=data[:, 18].astype(int), + required_decel=data[:, 19], + controller_fault=data[:, 20].astype(bool), solver_failures=solver_failures, ) @@ -172,32 +158,18 @@ def _command_jerk(trace: ClosedLoopTrace, after: float = 0.0) -> np.ndarray: def _filtered_realized_jerk(trace: ClosedLoopTrace, after: float = 1.0) -> np.ndarray: filtered_acceleration = np.convolve(trace.acceleration, np.ones(3) / 3.0, mode="valid") - jerk = np.diff(filtered_acceleration) / DT_MDL - return jerk[trace.time[2:-1] >= after] + return (np.diff(filtered_acceleration) / DT_MDL)[trace.time[2:-1] >= after] -def _has_propulsion_brake_reversal(trace: ClosedLoopTrace, after: float) -> bool: - indices = np.flatnonzero(trace.time >= after) - commands = trace.a_target[indices] - propulsion_seen = False - for command in commands: - propulsion_seen = propulsion_seen or command > 0.2 - if propulsion_seen and command < -0.2: - return True - return False - - -def _has_stable_brake_gas_brake(values: np.ndarray, threshold: float, frames: int = 5) -> bool: - """Return whether brake, gas, then brake each persist for ``frames`` samples.""" +def _has_brake_gas_brake(values: np.ndarray, threshold: float = 0.2, frames: int = 5) -> bool: phase = 0 brake_seen = False gas_after_brake_seen = False for index in range(len(values) - frames + 1): - window = values[index:index + frames] + window = values[index : index + frames] candidate = -1 if np.all(window <= -threshold) else 1 if np.all(window >= threshold) else 0 if candidate == 0 or candidate == phase: continue - phase = candidate if candidate < 0: if gas_after_brake_seen: @@ -205,79 +177,290 @@ def _has_stable_brake_gas_brake(values: np.ndarray, threshold: float, frames: in brake_seen = True elif brake_seen: gas_after_brake_seen = True - return False @pytest.fixture(autouse=True) -def _restore_controller_defaults(): +def _restore_defaults(): yield - _set_accel_controller_params(enabled=False, profile=1, dec_enabled=False) + _set_params(enabled=False, profile=1, dec_enabled=False) @pytest.mark.parametrize( - ("plant_kwargs", "expect_shadow_active"), + ("plant_kwargs", "expect_shadow"), [ ({"enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0}, False), ({"e2e": True, "lead_relevancy": False, "speed": 20.0}, True), ], ids=("disengaged", "e2e-shadow"), ) -def test_non_actuating_modes_are_bit_exact(plant_kwargs, expect_shadow_active): +def test_non_actuating_modes_match_clean_base(plant_kwargs, expect_shadow): common = dict(duration=2.0, v_lead=14.0, **plant_kwargs) - disabled = _run(controller_enabled=False, **common) - shadow = _run(controller_enabled=True, **common) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) - np.testing.assert_allclose(shadow.a_target, disabled.a_target, atol=1e-6, rtol=0.0) - np.testing.assert_array_equal(shadow.should_stop, disabled.should_stop) - np.testing.assert_array_equal(shadow.fcw, disabled.fcw) - assert shadow.source == disabled.source - assert not shadow.active.any() - if expect_shadow_active: - np.testing.assert_array_equal(shadow.shadow_active, ~shadow.controller_fault) - else: - assert not shadow.shadow_active.any() + np.testing.assert_allclose(trace.a_target, baseline.a_target, atol=1e-6, rtol=0.0) + np.testing.assert_array_equal(trace.should_stop, baseline.should_stop) + np.testing.assert_array_equal(trace.fcw, baseline.fcw) + assert trace.source == baseline.source + assert not trace.active.any() + np.testing.assert_array_equal(trace.shadow_active, ~trace.controller_fault if expect_shadow else np.zeros_like(trace.active)) -def test_disabled_profiles_are_bit_exact_in_engaged_acc(): +def test_disabled_profiles_match_clean_base(): common = dict(duration=2.0, controller_enabled=False, lead_relevancy=True, speed=20.0, distance_lead=70.0, v_lead=14.0) traces = [_run(profile=profile, **common) for profile in range(3)] - for trace in traces[1:]: np.testing.assert_allclose(trace.a_target, traces[0].a_target, atol=1e-6, rtol=0.0) np.testing.assert_array_equal(trace.should_stop, traces[0].should_stop) - np.testing.assert_array_equal(trace.fcw, traces[0].fcw) assert trace.source == traces[0].source - assert all(not trace.active.any() for trace in traces) assert all(np.isinf(trace.effective_accel_max).all() for trace in traces) -def test_dec_radar_lead_selects_acc_and_standstill_uses_shadow_only(): - blended = _run( - duration=2.0, +def test_active_controller_is_pre_mpc_and_preserves_stock_lead_authority(): + _set_params(enabled=True, profile=0) + plant = Plant(lead_relevancy=False, speed=0.0, actuator_delay=0.15, actuator_lag=0.20) + for _ in range(10): + result = plant.step(v_cruise=15.0) + if plant.planner.accel_controller_result.mpc_shape_cruise: + break + controller = plant.planner.accel_controller_result + + assert controller.mpc_shape_cruise + assert controller.mpc_accel_max is not None + assert controller.mpc_apply_accel_constraint + assert min(controller.mpc_accel_max) > 0.0 + np.testing.assert_array_equal(plant.planner.mpc.params[:, 1], controller.mpc_accel_max) + assert ACCEL_MIN <= result["a_target"] <= get_max_accel(plant.speed) + + lead_plant = Plant(lead_relevancy=True, speed=0.0, distance_lead=6.0, actuator_delay=0.15, actuator_lag=0.20) + lead_plant.step(v_lead=0.0, v_cruise=15.0) + controller = lead_plant.planner.accel_controller_result + assert controller.mpc_accel_max[0] < 0.0 + assert controller.mpc_accel_max[-1] == ACCEL_MAX + assert controller.mpc_shape_cruise + assert controller.mpc_apply_accel_constraint + assert np.max(lead_plant.planner.mpc.params[T_IDXS <= 0.40, 1]) <= 0.0 + + +def test_clear_road_launch_is_immediate_and_profiles_separate(): + common = dict( + duration=6.0, controller_enabled=True, - dec_enabled=True, - e2e=True, lead_relevancy=False, speed=0.0, + v_cruise=15.0, + actuator_delay=0.15, + actuator_lag=0.20, ) - radar_acc = _run( - duration=1.0, + traces = [_run(profile=profile, **common) for profile in range(3)] + + for trace in traces: + positive = np.flatnonzero(trace.a_target > 0.05) + moving = np.flatnonzero(trace.speed > 0.01) + assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL + assert len(moving) and trace.time[moving[0]] <= 1.0 + assert np.all(trace.effective_accel_max[trace.active] > 0.0) + assert not np.any(trace.a_target < -0.05) + assert trace.solver_failures == 0 + + onset_times = [float(trace.time[np.flatnonzero(trace.a_target > 0.05)[0]]) for trace in traces] + assert max(onset_times) - min(onset_times) <= DT_MDL + + +def test_profiles_have_distinct_moving_speed_preshape(): + traces = [ + _run( + duration=18.0, + controller_enabled=True, + profile=profile, + lead_relevancy=False, + speed=0.0, + v_cruise=30.0, + actuator_delay=0.15, + actuator_lag=0.20, + ) + for profile in range(3) + ] + samples = [np.flatnonzero(trace.speed >= 10.0)[0] for trace in traces] + configured = [float(trace.profile_accel_max[index]) for trace, index in zip(traces, samples, strict=True)] + effective = [float(trace.effective_accel_max[index]) for trace, index in zip(traces, samples, strict=True)] + assert configured[0] < configured[1] < configured[2] + assert effective[0] < effective[1] < effective[2] + moving_acceleration = [float(np.mean(trace.a_target[(trace.speed >= 14.0) & (trace.speed <= 16.0)])) for trace in traces] + assert moving_acceleration[0] < moving_acceleration[1] < moving_acceleration[2] + assert all(trace.solver_failures == 0 for trace in traces) + + +def test_clear_road_acceleration_crosses_lut_without_solver_failure(): + trace = _run( + duration=12.0, + controller_enabled=True, + profile=1, + lead_relevancy=False, + speed=0.0, + v_cruise=22.352, + actuator_delay=0.15, + actuator_lag=0.20, + ) + assert np.max(trace.speed) > 10.0 + assert trace.solver_failures == 0 + assert np.all(trace.effective_accel_max[trace.active] > 0.0) + + +def test_prius_route_model_launches_without_a_dead_pedal(): + trace = _run( + duration=3.0, + controller_enabled=True, + profile=1, + lead_relevancy=False, + speed=0.0, + v_cruise=22.352, + actuator_model=PRIUS_TSS2_ROUTE_MODEL, + ) + positive = np.flatnonzero(trace.a_target > 0.05) + moving = np.flatnonzero(trace.speed > 0.05) + assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL + assert len(moving) and trace.time[moving[0]] <= 1.0 + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize( + ("actuator_delay", "actuator_lag"), + [(0.10, 0.20), (0.15, 0.25), (0.20, 0.20), (0.25, 0.30), (0.30, 0.35)], + ids=("toyota", "honda", "gm", "hyundai", "ford"), +) +def test_stopped_lead_requires_four_departure_frames_and_launches_promptly(actuator_delay, actuator_lag): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + trace = _run( + duration=2.5, controller_enabled=True, - dec_enabled=True, - e2e=True, lead_relevancy=True, - speed=20.0, - distance_lead=55.0, - v_lead=12.0, + speed=0.0, + distance_lead=6.0, + v_lead=lead_speed, + v_cruise=8.0, + actuator_delay=actuator_delay, + actuator_lag=actuator_lag, ) - assert not blended.active[-10:].any() - np.testing.assert_array_equal(blended.shadow_active, ~blended.controller_fault) - assert radar_acc.active.all() + first_three = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9) + assert np.max(trace.speed[first_three]) < 1e-3 + assert not trace.launching[first_three].any() + positive_departure_pace = np.flatnonzero((trace.time >= departure_time) & (trace.target_speed > 0.0)) + assert len(positive_departure_pace) + before_ego_moves = np.arange(len(trace.time)) >= positive_departure_pace[0] + before_ego_moves &= trace.speed < 0.30 + assert np.all(trace.target_speed[before_ego_moves] > 0.0) + moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) + assert len(moving) and trace.time[moving[0]] <= departure_time + 4 * DT_MDL + 1.0 + prelaunch_pace = trace.target_speed[positive_departure_pace[0] : moving[0] + 1] + assert np.all(np.diff(prelaunch_pace) >= -1e-9) + assert not _has_brake_gas_brake(trace.a_target[trace.time >= departure_time]) + assert trace.solver_failures == 0 -def test_two_frame_dropout_and_false_relief_do_not_release_pace(record_property): +def test_creeping_lead_departure_is_prompt_and_does_not_lurch(): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + if current_time < departure_time: + return 0.0 + if current_time < departure_time + 0.5: + return 1.6 * (current_time - departure_time) + return min(2.5, 0.8 + 0.7 * (current_time - departure_time - 0.5)) + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + common = dict( + duration=6.0, + profile=0, + lead_relevancy=True, + speed=0.0, + distance_lead=3.6, + v_lead=lead_speed, + v_cruise=22.352, + lead_observation_fn=observe, + actuator_delay=0.15, + actuator_lag=0.20, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + after_departure = trace.time >= departure_time + lead_speeds = np.array([lead_speed(max(0.0, current_time - DT_MDL)) for current_time in trace.time]) + baseline_moving = np.flatnonzero((baseline.time >= departure_time) & (baseline.speed > 0.05)) + moving = np.flatnonzero(after_departure & (trace.speed > 0.05)) + assert len(baseline_moving) and len(moving) + assert trace.time[moving[0]] <= baseline.time[baseline_moving[0]] + assert np.all(trace.speed[after_departure] <= lead_speeds[after_departure] + 0.20) + assert not _has_brake_gas_brake(trace.a_target[after_departure]) + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3 + assert trace.solver_failures <= baseline.solver_failures + + +def test_stop_hold_ignores_two_frame_total_lead_dropout(): + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if 1.0 <= current_time < 1.1 else truth + + trace = _run( + duration=2.0, + controller_enabled=True, + lead_relevancy=True, + speed=0.0, + distance_lead=6.0, + v_lead=0.0, + v_cruise=8.0, + lead_observation_fn=observe, + actuator_delay=0.15, + actuator_lag=0.20, + ) + assert np.max(trace.speed) < 1e-3 + assert np.max(trace.pace) == 0.0 + assert trace.solver_failures == 0 + + +def test_low_speed_stopped_lead_never_accelerates_during_stop_hold(): + def lead_speed(current_time: float) -> float: + return max(0.0, 1.9 - 1.16 * current_time) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + moving = lead_speed(current_time) > 0.0 + return truth | {"vLeadK": truth["vLeadK"] if moving else -0.01, "aLeadK": -1.16 if moving else 0.0, "radarTrackId": 7, "radar": True} + + common = dict( + duration=6.0, + profile=0, + lead_relevancy=True, + speed=4.5, + distance_lead=18.0, + v_lead=lead_speed, + v_cruise=23.056, + lead_observation_fn=observe, + actuator_delay=0.15, + actuator_lag=0.20, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + + urgent_demand = (trace.required_decel >= 0.45) & (trace.speed >= 0.30) & ~trace.should_stop + stop_hold = trace.state == int(AccelControllerState.stopHold) + assert urgent_demand.any() and stop_hold.any() + assert np.max(trace.a_target[urgent_demand]) < 0.0 + assert np.max(trace.a_target[stop_hold]) <= 0.0 + assert not _has_brake_gas_brake(trace.a_target[trace.time >= 1.0]) + assert _first_time_below(trace, -1.0) <= _first_time_below(baseline, -1.0) + 1e-9 + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3 + assert trace.solver_failures == 0 + + +def test_moving_lead_dropout_and_false_relief_do_not_release_pace(): def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: if 2.0 <= current_time < 2.1: return None @@ -297,216 +480,57 @@ def test_two_frame_dropout_and_false_relief_do_not_release_pace(record_property) ) baseline = _run(controller_enabled=False, **common) trace = _run(controller_enabled=True, **common) - for start in (2.0, 3.0): before = trace.pace[np.flatnonzero(trace.time < start)[-1]] - guard = (trace.time >= start) & (trace.time < start + 0.2) - during_and_guard = trace.pace[guard & trace.active] - assert np.all(during_and_guard <= before + 1e-9) - assert np.isinf(trace.pace[guard & ~trace.active]).all() - assert not _has_propulsion_brake_reversal(trace, after=1.0) - record_property("clean_base_solver_failures", baseline.solver_failures) - record_property("accel_controller_solver_failures", trace.solver_failures) - assert trace.solver_failures == 0 + guard = (trace.time >= start) & (trace.time < start + 0.2) & trace.active + assert np.all(trace.pace[guard] <= before + 1e-9) + assert not _has_brake_gas_brake(trace.a_target[trace.time >= 1.0]) + assert trace.solver_failures <= baseline.solver_failures -def test_lead_slot_handoff_does_not_resurrect_stale_relief(): - def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if current_time < 2.0: - return truth if lead_name == "leadOne" else None - if current_time < 2.1: - return None - if lead_name == "leadTwo": - return {"dRel": truth["dRel"] + 2.0, "radarTrackId": 38} - return None +def test_low_speed_far_lead_acquisition_does_not_fault_or_lurch(): + acquisition_time = 5.0 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if current_time < acquisition_time else truth trace = _run( - duration=4.0, - controller_enabled=True, - lead_relevancy=True, - speed=20.0, - distance_lead=80.0, - v_lead=14.0, - lead_observation_fn=observe, - actuator_delay=0.20, - actuator_lag=0.25, - ) - - assert np.all(trace.selected_lead[(trace.time >= 0.5) & (trace.time < 2.0)] == 0) - assert np.all(trace.selected_lead[trace.time >= 2.2] == 1) - pace_before_handoff = trace.pace[np.flatnonzero(trace.time < 2.0)[-1]] - handoff_guard = trace.pace[(trace.time >= 2.0) & (trace.time < 2.3)] - assert np.all(handoff_guard <= pace_before_handoff + 1e-9) - assert not _has_propulsion_brake_reversal(trace, after=1.0) - - -def test_benign_far_lead_acquisition_ramps_optimizer_authority_without_jerk(): - acquisition_time = 1.0 - - def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if current_time < acquisition_time or lead_name == "leadTwo": - return None - return truth | {"radarTrackId": 7} - - trace = _run( - duration=3.0, + duration=8.0, controller_enabled=True, profile=0, lead_relevancy=True, - speed=20.0, - distance_lead=126.0, - v_lead=17.0, - v_cruise=20.0, + speed=0.0, + distance_lead=200.0, + v_lead=3.0, + v_cruise=30.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, ) - acquired = np.flatnonzero((trace.time >= acquisition_time) & (trace.selected_lead == 0)) - assert len(acquired) - first = acquired[0] - assert trace.lead_obstacle_weight_0[first] == pytest.approx(0.2) - authority = trace.lead_obstacle_weight_0[first:first + 7] - np.testing.assert_allclose(authority, np.linspace(0.2, 1.0, 7), atol=1e-9, rtol=0.0) - jerk_window = (trace.time[1:] >= acquisition_time) & (trace.time[1:] <= acquisition_time + 0.6) - assert np.max(np.abs(np.diff(trace.a_target)[jerk_window] / DT_MDL)) < 3.0 + acquired = (trace.time >= acquisition_time) & (trace.selected_lead >= 0) + response = trace.time >= acquisition_time + jerk_response = trace.time[1:] >= acquisition_time + assert acquired.any() + assert not trace.controller_fault[response].any() assert trace.solver_failures == 0 + assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0 + assert not _has_brake_gas_brake(trace.a_target[response]) -def test_route_shaped_urgent_lead_acquisition_is_immediate_and_does_not_delay_braking(record_property): - acquisition_time = 1.0 - - def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if current_time < acquisition_time or lead_name == "leadTwo": - return None - return truth | {"radarTrackId": 22} - - common = dict( - duration=10.0, lead_relevancy=True, speed=34.8, - # 11.4 m/s closing before acquisition reproduces the route's observed ~93.6 m gap. - distance_lead=105.0, v_lead=23.4, v_cruise=40.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, - ) - baseline = _run(controller_enabled=False, **common) - trace = _run(controller_enabled=True, **common) - - acquired = np.flatnonzero((trace.time >= acquisition_time) & (trace.selected_lead == 0)) - assert len(acquired) - assert trace.lead_obstacle_weight_0[acquired[0]] == 1.0 - assert trace.solver_failures == 0 - record_property("clean_base_solver_failures", baseline.solver_failures) - if baseline.solver_failures: - pytest.xfail("provisional route gate: clean-base MPC loses the abrupt 34.8-to-23.4 m/s lead-acquisition solve") - for threshold in (-1.0, -2.0): - assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9 - - baseline_gap = baseline.distance_lead - baseline.distance - controlled_gap = trace.distance_lead - trace.distance - assert np.min(controlled_gap) >= np.min(baseline_gap) - 1e-3 - baseline_closing = np.maximum(baseline.speed - common["v_lead"], 0.0) - controlled_closing = np.maximum(trace.speed - common["v_lead"], 0.0) - baseline_ttc = np.divide(baseline_gap, baseline_closing, out=np.full_like(baseline_gap, np.inf), where=baseline_closing > 0.0) - controlled_ttc = np.divide(controlled_gap, controlled_closing, out=np.full_like(controlled_gap, np.inf), where=controlled_closing > 0.0) - assert np.min(controlled_ttc) >= np.min(baseline_ttc) - 1e-3 - - -def test_moderate_urgent_lead_acquisition_does_not_delay_stock_braking(): - acquisition_time = 1.0 - - def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if current_time < acquisition_time or lead_name == "leadTwo": - return None - return truth | {"radarTrackId": 23} - - common = dict( - duration=8.0, lead_relevancy=True, speed=20.0, distance_lead=55.0, v_lead=12.0, v_cruise=20.0, - lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, - ) - baseline = _run(controller_enabled=False, **common) - trace = _run(controller_enabled=True, **common) - - assert trace.solver_failures <= baseline.solver_failures - for threshold in (-1.0, -2.0): - assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9 - if trace.solver_failures: - pytest.xfail("opt-in validation: clean base and controller both lose the moderate abrupt-acquisition solve on this platform") - - -def test_urgent_warm_start_reset_preserves_fcw_history_until_mpc_update(): - lead_visible = False - - def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if not lead_visible or lead_name == "leadTwo": - return None - return truth | {"radarTrackId": 24} - - _set_accel_controller_params(enabled=True) - plant = Plant(lead_relevancy=True, speed=34.8, distance_lead=105.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20) - while plant.current_time < 1.0: - plant.step(v_lead=23.4, v_cruise=40.0) - - lead_visible = True - plant.planner.mpc.crash_cnt = 2.0 - crash_count_at_update = [] - original_update = plant.planner.mpc.update - - def capture_crash_count(*args, **kwargs): - crash_count_at_update.append(plant.planner.mpc.crash_cnt) - return original_update(*args, **kwargs) - - plant.planner.mpc.update = capture_crash_count - plant.step(v_lead=23.4, v_cruise=40.0) - - assert plant.planner.accel_controller_result.reset_mpc - assert crash_count_at_update == [2.0] - - -def test_route_e5_low_speed_urgent_closing_stays_with_stock_braking(): - # E5 became urgent below 5 m/s (ego 4.5, lead 1.9 at 16-18 m), requiring immediate stock-MPC bypass. - def lead_speed(current_time: float) -> float: - return max(0.0, 1.9 - 1.16 * current_time) - - def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if lead_name == "leadTwo": - return None - return truth | {"aLeadK": -1.16 if lead_speed(current_time) > 0.0 else 0.0, "radarTrackId": 7, "radar": True} - - common = dict( - duration=6.0, profile=0, lead_relevancy=True, speed=4.5, distance_lead=18.0, v_lead=lead_speed, v_cruise=23.056, - lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, - ) - baseline = _run(controller_enabled=False, **common) - trace = _run(controller_enabled=True, **common) - - urgent_demand = (trace.required_decel >= 0.45) & (trace.speed >= 0.30) & ~trace.should_stop - urgent_indices = np.flatnonzero(urgent_demand) - assert len(urgent_indices) - first_urgent = urgent_indices[0] - assert trace.speed[first_urgent] < 5.0 - assert trace.urgent_bypass[first_urgent] - assert trace.urgent_bypass[urgent_demand].all() - assert np.max(trace.a_target[urgent_demand]) < 0.0 - - assert _first_time_below(trace, -1.0) <= _first_time_below(baseline, -1.0) + 1e-9 - baseline_gap = baseline.distance_lead - baseline.distance - controlled_gap = trace.distance_lead - trace.distance - assert np.min(controlled_gap) >= np.min(baseline_gap) - 1e-3 - assert trace.solver_failures == 0 - - -def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal(): +def test_alternating_range_glitch_has_bounded_jerk_and_no_reversal(): glitch_start = 5.0 glitch_end = 5.5 def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation: if glitch_start <= current_time < glitch_end: frame = round(current_time / DT_MDL) - observed = dict(truth) - observed["dRel"] = truth["dRel"] + (5.0 if frame % 2 else 0.0) - return observed + return truth | {"dRel": truth["dRel"] + (5.0 if frame % 2 else 0.0)} return truth common = dict( duration=10.0, + controller_enabled=True, lead_relevancy=True, speed=8.0, distance_lead=20.0, @@ -514,489 +538,109 @@ def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal(): actuator_delay=0.20, actuator_lag=0.25, ) - control = _run(controller_enabled=True, **common) - trace = _run(controller_enabled=True, lead_observation_fn=observe, **common) - - np.testing.assert_array_equal(trace.time, control.time) - jerk_window = (trace.time[1:] >= glitch_start) & (trace.time[1:] < glitch_end + 0.5) - assert np.max(np.abs(np.diff(trace.a_target)[jerk_window] / DT_MDL)) < 3.0 - - # Isolate glitch response; the fixture naturally transitions from propulsion to braking later. - response_window = (trace.time >= glitch_start) & (trace.time < glitch_end + 1.0) - disturbance = trace.a_target[response_window] - control.a_target[response_window] - positive = np.flatnonzero(disturbance > 0.2) - if len(positive): - assert not np.any(disturbance[positive[0] + 1:] < -0.2) - - -def test_route_like_tiny_closing_track_noise_does_not_chatter_accel_authority(): - noise_start = 3.0 - phase_time = 2.5 - - def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if lead_name == "leadTwo": - return None - - closing_sample = current_time < noise_start or int((current_time - noise_start) // phase_time) % 2 == 0 - v_ego = truth["vLead"] - truth["vRel"] - v_lead_observed = v_ego - 0.60 if closing_sample else v_ego + 0.10 - a_lead_observed = -0.15 if closing_sample else 0.12 - return truth | { - "vRel": v_lead_observed - v_ego, "aRel": a_lead_observed + truth["aRel"], "vLead": v_lead_observed, - "vLeadK": v_lead_observed, "aLeadK": a_lead_observed, "radarTrackId": 655 if closing_sample else 798, "radar": True, - } - - trace = _run( - duration=16.0, controller_enabled=True, profile=0, lead_relevancy=True, speed=30.0, - # Midpoint of E8's 55-79 m track-switch band isolates relative-speed noise without entering the desired-gap singularity. - distance_lead=70.0, v_lead=30.0, v_cruise=30.0, lead_observation_fn=observe, actuator_delay=0.10, actuator_lag=0.20, - ) - - noise = trace.time >= noise_start - first_noise = np.flatnonzero(noise)[0] - filtered_acceleration = np.convolve(trace.acceleration[noise], np.ones(5) / 5.0, mode="valid") - assert np.max(trace.required_decel[noise]) < 0.03 - assert not trace.urgent_bypass[noise].any() - assert np.all(trace.selected_lead[noise] == 0) - assert np.all(trace.state[noise] == AccelControllerState.free) - assert all(source == LongitudinalPlanSource.cruise for source in trace.source[first_noise:]) - - assert not _has_stable_brake_gas_brake(trace.a_target[noise], threshold=0.08) - assert not _has_stable_brake_gas_brake(trace.effective_accel_max[noise], threshold=0.05) - assert not _has_stable_brake_gas_brake(filtered_acceleration, threshold=0.15) - assert np.max(np.diff(trace.pace[noise])) <= 1e-9 - assert np.min(trace.distance_lead - trace.distance) > 55.0 + control = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + window = (trace.time[1:] >= glitch_start) & (trace.time[1:] < glitch_end + 0.5) + assert np.max(np.abs(np.diff(trace.a_target)[window] / DT_MDL)) < 3.0 + response = (trace.time >= glitch_start) & (trace.time < glitch_end + 1.0) + assert not _has_brake_gas_brake((trace.a_target - control.a_target)[response]) assert trace.solver_failures == 0 -def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal(): - def lead_speed(current_time: float) -> float: - return float(0.1 * (1.0 - np.cos(np.pi * current_time))) - - trace = _run( - duration=9.0, controller_enabled=True, lead_relevancy=True, speed=2.0, distance_lead=10.0, v_lead=lead_speed, v_cruise=8.0, - actuator_delay=0.15, actuator_lag=0.20, - ) - - settled = trace.time >= 4.0 - assert trace.active[settled].all() - assert np.all(trace.pace[settled] == 0.0) - assert np.max(trace.a_target[settled]) <= 0.2 - assert not _has_propulsion_brake_reversal(trace, after=4.0) - - -def test_route_e7_creeping_lead_departure_has_no_stable_brake_gas_brake(): - departure_time = 1.0 - - def lead_speed(current_time: float) -> float: - if current_time < departure_time: - return 0.0 - if current_time < departure_time + 0.5: - return 1.6 * (current_time - departure_time) - if current_time < departure_time + 1.5: - return 0.8 - return min(2.5, 0.8 + 1.13 * (current_time - departure_time - 1.5)) - - def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: - if lead_name == "leadTwo": - return None - return truth | {"radarTrackId": 2133, "radar": True} - - trace = _run( - duration=7.0, controller_enabled=True, profile=0, lead_relevancy=True, speed=0.0, - # E7's bookmarked queue oscillation had a 3.6-3.9 m radar gap. - distance_lead=3.6, v_lead=lead_speed, v_cruise=22.352, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, - ) - - after_departure = trace.time >= departure_time - lead_speeds = np.array([lead_speed(max(0.0, current_time - DT_MDL)) for current_time in trace.time]) - filtered_acceleration = np.convolve(trace.acceleration[after_departure], np.ones(5) / 5.0, mode="valid") - moving = np.flatnonzero(after_departure & (trace.speed > 0.05)) - assert len(moving) - assert trace.time[moving[0]] <= departure_time + 3.5 - assert np.all(trace.speed[after_departure] <= lead_speeds[after_departure] + 0.20) - - assert not _has_stable_brake_gas_brake(trace.a_target[after_departure], threshold=0.20) - assert not _has_stable_brake_gas_brake(filtered_acceleration, threshold=0.20) - assert np.min(trace.distance_lead - trace.distance) >= 3.5 - assert trace.solver_failures == 0 - - -def test_severe_closing_never_delays_braking_or_reduces_clearance(): - common = dict( - duration=12.0, lead_relevancy=True, speed=20.0, distance_lead=160.0, v_lead=3.5, actuator_delay=0.20, actuator_lag=0.20, - ) - baseline = _run(controller_enabled=False, **common) - controlled = _run(controller_enabled=True, **common) - - for threshold in (-1.0, -2.0): - assert _first_time_below(controlled, threshold) <= _first_time_below(baseline, threshold) + 1e-9 - - baseline_gap = baseline.distance_lead - baseline.distance - controlled_gap = controlled.distance_lead - controlled.distance - assert controlled_gap.min() >= baseline_gap.min() - 1e-3 - assert controlled_gap.min() > 0.4 - onset = (controlled.time[1:] > 0.5) & (controlled.time[1:] < 3.0) - assert np.max(np.abs(np.diff(controlled.a_target)[onset] / DT_MDL)) < 4.0 - - -def test_slow_lead_urgent_rejoin_has_no_brake_release_jolt_or_safety_regression(): - common = dict( - duration=25.0, lead_relevancy=True, speed=20.0, distance_lead=100.0, v_lead=10.0, v_cruise=30.0, - actuator_delay=0.20, actuator_lag=0.25, - ) - baseline = _run(controller_enabled=False, **common) - controlled = _run(controller_enabled=True, profile=1, **common) - - assert controlled.urgent_bypass.any() - assert controlled.urgent_recovery.any() - assert controlled.solver_failures == 0 - assert controlled.solver_failures <= baseline.solver_failures - - command_jerk = _command_jerk(controlled, after=1.0) - assert np.max(command_jerk) < 3.0 - assert np.max(np.abs(command_jerk)) < 4.0 - assert not _has_propulsion_brake_reversal(controlled, after=1.0) - - baseline_gap = baseline.distance_lead - baseline.distance - controlled_gap = controlled.distance_lead - controlled.distance - # Compare the valid stock prefix when clean-base acados hits its late macOS solver edge. - baseline_valid = ~baseline.controller_fault - if baseline.controller_fault.any(): - baseline_valid[np.flatnonzero(baseline.controller_fault)[0]:] = False - # Routine matching may trade <1 m of buffer; severe closing retains the exact no-clearance-loss gate. - assert np.min(controlled_gap[baseline_valid]) >= np.min(baseline_gap[baseline_valid]) - 1.0 - baseline_closing = np.maximum(baseline.speed - common["v_lead"], 0.0) - controlled_closing = np.maximum(controlled.speed - common["v_lead"], 0.0) - baseline_ttc = np.divide(baseline_gap, baseline_closing, out=np.full_like(baseline_gap, np.inf), where=baseline_closing > 0.0) - controlled_ttc = np.divide(controlled_gap, controlled_closing, out=np.full_like(controlled_gap, np.inf), where=controlled_closing > 0.0) - assert np.min(controlled_ttc[baseline_valid]) >= np.min(baseline_ttc[baseline_valid]) - 0.50 - assert np.min(controlled_gap) > 20.0 - assert np.min(controlled_ttc) > 6.0 - - # Rejoin cannot command gas while still closing or materially over-slow. - still_closing = controlled.speed > common["v_lead"] + 0.2 - assert np.max(controlled.a_target[still_closing]) <= 0.2 - controlled_undershoot = np.min(controlled.speed - common["v_lead"]) - assert controlled_undershoot >= -1.1 - assert abs(controlled.speed[-1] - common["v_lead"]) < 0.65 - - -@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) @pytest.mark.parametrize( ("actuator_delay", "actuator_lag"), - [ - (0.10, 0.20), - (0.15, 0.25), - (0.20, 0.20), - (0.25, 0.30), - (0.30, 0.35), - ], + [(0.10, 0.20), (0.15, 0.25), (0.20, 0.20), (0.25, 0.30), (0.30, 0.35)], ids=("toyota", "honda", "gm", "hyundai", "ford"), ) -def test_slow_lead_rejoin_is_smooth_across_profiles_and_actuator_dynamics(profile, actuator_delay, actuator_lag): +def test_slow_lead_approach_is_smooth_across_actuator_dynamics(actuator_delay, actuator_lag): lead_speed = 10.0 - trace = _run( - duration=10.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0, distance_lead=100.0, - v_lead=lead_speed, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + common = dict( + duration=15.0, + profile=1, + lead_relevancy=True, + speed=20.0, + distance_lead=100.0, + v_lead=lead_speed, + v_cruise=30.0, + actuator_delay=actuator_delay, + actuator_lag=actuator_lag, ) - - assert trace.urgent_bypass.any() - assert trace.solver_failures == 0 - command_jerk = _command_jerk(trace, after=1.0) - assert np.max(command_jerk) < 3.0 - assert np.max(np.abs(command_jerk)) < 4.0 - assert not _has_propulsion_brake_reversal(trace, after=1.0) - - gap = trace.distance_lead - trace.distance - closing = np.maximum(trace.speed - lead_speed, 0.0) - ttc = np.divide(gap, closing, out=np.full_like(gap, np.inf), where=closing > 0.0) - assert np.min(gap) > 20.0 - assert np.min(ttc) > 3.0 - assert np.min(trace.speed - lead_speed) > -1.25 - assert np.max(trace.a_target[trace.speed > lead_speed + 0.2]) <= 0.2 + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + baseline_peak_jerk = np.max(np.abs(_command_jerk(baseline, after=1.0))) + trace_peak_jerk = np.max(np.abs(_command_jerk(trace, after=1.0))) + assert trace_peak_jerk <= baseline_peak_jerk + 0.1 + assert not _has_brake_gas_brake(trace.a_target[trace.time >= 1.0]) + closing = trace.speed > lead_speed + 0.2 + assert np.max(trace.a_target[closing]) <= 0.2 + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3 + assert trace.solver_failures <= baseline.solver_failures @pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) -def test_decelerating_moving_lead_unwinds_brake_without_false_stop(profile): +def test_decelerating_moving_lead_unwinds_without_brake_gas_brake(profile): def lead_speed(current_time: float) -> float: if current_time < 2.0: return 15.0 if current_time >= 8.0: return 5.0 progress = (current_time - 2.0) / 6.0 - return 15.0 - 10.0 * (3.0 * progress * progress - 2.0 * progress * progress * progress) + return 15.0 - 10.0 * (3.0 * progress**2 - 2.0 * progress**3) - trace = _run( - duration=18.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0, distance_lead=110.0, - v_lead=lead_speed, v_cruise=30.0, actuator_delay=0.20, actuator_lag=0.25, + common = dict( + duration=18.0, + profile=profile, + lead_relevancy=True, + speed=20.0, + distance_lead=110.0, + v_lead=lead_speed, + v_cruise=30.0, + actuator_delay=0.20, + actuator_lag=0.25, ) - + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) after_lead_settles = trace.time >= 8.0 - command_jerk = _command_jerk(trace, after=1.0) - gap = trace.distance_lead - trace.distance - - assert trace.urgent_bypass.any() - assert trace.solver_failures == 0 + moving_lead_decel = trace.effective_accel_max < 0.0 + assert moving_lead_decel.any() + assert np.min(trace.effective_accel_max[moving_lead_decel]) >= MOVING_LEAD_DECEL_ACCEL_MAX - 1e-9 assert not trace.should_stop[after_lead_settles].any() - assert np.max(np.abs(command_jerk)) < 3.5 + assert np.max(np.abs(_command_jerk(trace, after=1.0))) < 3.5 assert np.min(trace.speed[after_lead_settles]) >= 2.0 - assert np.min(gap) > 20.0 - assert not _has_propulsion_brake_reversal(trace, after=1.0) + assert not _has_brake_gas_brake(trace.a_target[trace.time >= 1.0]) + assert np.min(trace.distance_lead - trace.distance) > 20.0 + assert trace.solver_failures <= baseline.solver_failures + + +def test_severe_closing_never_delays_stock_braking_or_reduces_clearance(): + common = dict( + duration=12.0, + lead_relevancy=True, + speed=20.0, + distance_lead=160.0, + v_lead=3.5, + actuator_delay=0.20, + actuator_lag=0.20, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + for threshold in (-1.0, -2.0): + assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9 + baseline_gap = baseline.distance_lead - baseline.distance + controlled_gap = trace.distance_lead - trace.distance + assert np.min(controlled_gap) >= np.min(baseline_gap) - 1e-3 + onset = (trace.time[1:] > 0.5) & (trace.time[1:] < 3.0) + assert np.max(np.abs(np.diff(trace.a_target)[onset] / DT_MDL)) < 4.0 + assert trace.solver_failures == 0 @pytest.mark.parametrize( ("actuator_delay", "actuator_lag"), - [ - (0.10, 0.20), - (0.15, 0.25), - (0.20, 0.20), - (0.25, 0.30), - (0.30, 0.35), - ], + [(0.10, 0.20), (0.15, 0.25), (0.20, 0.20), (0.25, 0.30), (0.30, 0.35)], ids=("toyota", "honda", "gm", "hyundai", "ford"), ) -def test_stopped_lead_noise_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag, record_property): - departure_time = 1.0 - - def lead_speed(current_time: float) -> float: - return 0.0 if current_time < departure_time else 2.0 - - def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation: - frame = round(current_time / DT_MDL) - if current_time < departure_time and frame % 4 == 0: - return {"dRel": truth["dRel"] + 4.0, "vRel": 1.5, "vLead": 1.5, "vLeadK": 1.5, "aLeadK": 0.0} - return truth - - common = dict( - duration=2.5, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, v_cruise=8.0, - lead_observation_fn=observe, actuator_delay=actuator_delay, actuator_lag=actuator_lag, - ) - baseline = _run(controller_enabled=False, **common) - trace = _run(controller_enabled=True, **common) - - baseline_should_stop_clear = np.flatnonzero((baseline.time >= departure_time) & ~baseline.should_stop) - baseline_launched = np.flatnonzero((baseline.time >= departure_time) & (baseline.speed > 0.05)) - assert len(baseline_should_stop_clear) - assert len(baseline_launched) - record_property("clean_base_departure_should_stop_clear_time", float(baseline.time[baseline_should_stop_clear[0]] - departure_time)) - record_property("clean_base_departure_launch_time", float(baseline.time[baseline_launched[0]] - departure_time)) - - before_departure = trace.time < departure_time - assert np.max(trace.speed[before_departure]) < 1e-3 - assert not _has_propulsion_brake_reversal(trace, after=0.3) - first_three_departure_frames = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9) - record_property("predeparture_peak_command", float(np.max(trace.a_target[before_departure]))) - record_property("first_three_departure_frames_peak_command", float(np.max(trace.a_target[first_three_departure_frames]))) - assert np.max(trace.speed[first_three_departure_frames]) < 1e-3 - assert not trace.launching[first_three_departure_frames].any() - - launched = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) - assert len(launched) - launch_time = float(trace.time[launched[0]] - departure_time) - departure_jerk = np.diff(trace.a_target[trace.time >= departure_time]) / DT_MDL - peak_departure_jerk = float(np.max(np.abs(departure_jerk))) - record_property("departure_launch_time", launch_time) - record_property("departure_peak_command_jerk", peak_departure_jerk) - assert launch_time <= 1.0 - assert peak_departure_jerk < 4.0 - assert trace.solver_failures == 0 - assert not _has_propulsion_brake_reversal(trace, after=departure_time) - - @pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) -def test_route_derived_prius_prompt_launch_gate(profile): - departure_time = 1.0 - - def lead_speed(current_time: float) -> float: - return 0.0 if current_time < departure_time else 2.0 - - trace = _run( - duration=3.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, - # Dominant post-SCC/SLA target in the supplied Prius routes (50 mph). - v_cruise=22.352, actuator_model=PRIUS_TSS2_ROUTE_MODEL, - ) - - first_three = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9) - assert not trace.launching[first_three].any() - assert np.max(trace.speed[first_three]) < 1e-3 - moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) - assert len(moving) - assert trace.time[moving[0]] - departure_time <= 1.0 + 1e-9 - assert trace.solver_failures == 0 - - -def test_stop_hold_two_frame_total_lead_dropout_cannot_launch(): - def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: - return None if 1.0 <= current_time < 1.1 else truth - - trace = _run( - duration=2.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=0.0, v_cruise=8.0, - lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, - ) - - assert np.max(trace.speed) < 1e-3 - assert np.max(trace.pace) == 0.0 - assert trace.solver_failures == 0 - assert not _has_propulsion_brake_reversal(trace, after=0.5) - - -@pytest.mark.parametrize("v_ego_stopping", [0.25, 0.10], ids=("toyota-like", "tesla-like")) -def test_stop_hold_above_vehicle_should_stop_threshold_keeps_close_lead_authority(v_ego_stopping): - _set_accel_controller_params(enabled=True, profile=1) - initial_gap = 0.25 - plant = Plant(lead_relevancy=True, speed=0.28, distance_lead=initial_gap, actuator_delay=0.10, actuator_lag=0.20) - plant.planner.CP.vEgoStopping = v_ego_stopping - - gaps = [] - should_stop = [] - solver_statuses = [] - first_controller = None - for _ in range(round(2.0 / DT_MDL)): - result = plant.step(v_lead=0.0, v_cruise=5.0) - controller = plant.planner.accel_controller_result - first_controller = first_controller or controller - gaps.append(result["distance_lead"] - result["distance"]) - should_stop.append(result["should_stop"]) - solver_statuses.append(plant.planner.mpc.last_solution_status) - - assert first_controller is not None - assert first_controller.state == AccelControllerState.stopHold - assert first_controller.target_speed == 0.0 - assert first_controller.lead_obstacle_weights == (1.0, 1.0) - assert np.flatnonzero(should_stop)[0] * DT_MDL <= 1.0 - assert min(gaps) > 0.05 - assert plant.speed == 0.0 - assert not any(solver_statuses) - - -def test_clear_road_launch_is_immediate_bounded_and_profiles_feel_distinct(): - common = dict( - duration=6.0, controller_enabled=True, lead_relevancy=False, speed=0.0, v_cruise=15.0, actuator_delay=0.15, actuator_lag=0.20, - ) - traces = [_run(profile=profile, **common) for profile in range(3)] - - onset_times = [] - movement_times = [] - for trace in traces: - positive = np.flatnonzero(trace.a_target > 0.05) - moving = np.flatnonzero(trace.speed > 0.01) - assert len(positive) - assert len(moving) - onset_times.append(float(trace.time[positive[0]])) - movement_times.append(float(trace.time[moving[0]])) - assert trace.solver_failures == 0 - - assert max(onset_times) - min(onset_times) <= DT_MDL - assert max(onset_times) <= 4 * DT_MDL - assert max(movement_times) <= 1.0 - - sample_time = 2.0 - realized = [float(trace.acceleration[np.searchsorted(trace.time, sample_time)]) for trace in traces] - assert realized[0] < realized[1] < realized[2], (sample_time, realized) - final_speeds = [trace.speed[-1] for trace in traces] - assert final_speeds[0] < final_speeds[1] < final_speeds[2] - assert final_speeds[1] - final_speeds[0] > 0.5 - assert final_speeds[2] - final_speeds[1] > 0.4 - - -def test_accelerating_lead_departure_is_prompt_smooth_and_profiles_feel_distinct(): - departure_time = 1.0 - - def lead_speed(current_time: float) -> float: - return 0.0 if current_time < departure_time else min(15.0, 2.0 * (current_time - departure_time)) - - common = dict( - duration=10.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, - v_cruise=22.352, actuator_model=PRIUS_TSS2_ROUTE_MODEL, - ) - traces = [_run(profile=profile, **common) for profile in range(3)] - first_credible_lead_time = departure_time + 0.20 - movement_times = [] - for trace in traces: - before_confirmation = trace.time < first_credible_lead_time + 3 * DT_MDL - assert np.max(trace.speed[before_confirmation]) < 1e-3 - - moving = np.flatnonzero((trace.time >= first_credible_lead_time) & (trace.speed > 0.05)) - assert len(moving) - movement_times.append(float(trace.time[moving[0]])) - assert movement_times[-1] - first_credible_lead_time <= 1.0 - - lead_speeds = np.array([lead_speed(max(0.0, t - DT_MDL)) for t in trace.time]) - assert np.all(trace.speed <= lead_speeds + 0.20) - assert np.min(trace.distance_lead - trace.distance) >= 5.99 - assert not trace.fcw.any() - assert trace.solver_failures == 0 - assert not _has_propulsion_brake_reversal(trace, after=departure_time) - departure_jerk = np.diff(trace.a_target)[trace.time[1:] >= departure_time] / DT_MDL - assert np.max(np.abs(departure_jerk)) < 4.0 - - assert max(movement_times) - min(movement_times) <= DT_MDL - steady = (traces[0].time >= 8.0) & (traces[0].time <= 10.0) - mean_speeds = [float(np.mean(trace.speed[steady])) for trace in traces] - assert mean_speeds[0] < mean_speeds[1] < mean_speeds[2] - assert mean_speeds[1] - mean_speeds[0] >= 0.60 - assert mean_speeds[2] - mean_speeds[1] >= 0.20 - - terminal_distances = [float(trace.distance[-1]) for trace in traces] - assert terminal_distances[1] - terminal_distances[0] >= 2.0 - assert terminal_distances[2] - terminal_distances[1] >= 0.50 - - -def test_profile_trajectory_is_pre_mpc_and_not_a_custom_output_clamp(): - _set_accel_controller_params(enabled=True, profile=0) - plant = Plant(speed=10.0, actuator_delay=0.15, actuator_lag=0.20) - # Seed above Eco's table value to prove pre-MPC slew rather than output clipping. - plant.acceleration = 1.30 - plant.planner.a_desired = 1.30 - - result = plant.step(v_cruise=30.0) - controller = plant.planner.accel_controller_result - - assert controller.mpc_accel_max is not None - assert controller.mpc_shape_cruise - np.testing.assert_array_equal(plant.planner.mpc.params[:, 1], controller.mpc_accel_max) - assert result["a_target"] > controller.profile_accel_max - assert ACCEL_MIN <= result["a_target"] <= get_max_accel(plant.speed) - - -def test_solver_fault_discards_live_state_before_fresh_preshape_seed(): - _set_accel_controller_params(enabled=True, profile=1) - plant = Plant(speed=10.0, actuator_delay=0.15, actuator_lag=0.20) - plant.step(v_cruise=30.0) - assert plant.planner.accel_controller_result.active - - plant.planner.mpc.last_solution_status = 3 - plant.planner.mpc.reset() - plant.step(v_cruise=30.0) - faulted = plant.planner.accel_controller_result - assert not faulted.active - assert np.isinf(faulted.live_pace) - assert faulted.mpc_accel_max is None - assert not faulted.mpc_shape_cruise - - # A successful solve must seed from current state, not discarded pre-fault history. - plant.planner.mpc.last_solution_status = 0 - plant.step(v_cruise=30.0) - recovered = plant.planner.accel_controller_result - assert recovered.active - assert np.isfinite(recovered.live_pace) - assert recovered.mpc_accel_max is not None - assert recovered.mpc_shape_cruise - - -@pytest.mark.parametrize( - ("actuator_delay", "actuator_lag", "current_tn_jerk_p95"), - [ - (0.10, 0.20, 0.0988673), - (0.15, 0.25, 0.1010401), - (0.20, 0.20, 0.1004875), - (0.25, 0.30, 0.0973712), - (0.30, 0.35, 0.1050558), - ], - ids=("toyota", "honda", "gm", "hyundai", "ford"), -) -def test_far_lead_deceleration_is_early_across_actuator_dynamics(actuator_delay, actuator_lag, current_tn_jerk_p95, record_property): +def test_far_lead_deceleration_starts_early_and_stays_smooth(profile, actuator_delay, actuator_lag): common = dict( duration=11.0, lead_relevancy=True, @@ -1007,50 +651,69 @@ def test_far_lead_deceleration_is_early_across_actuator_dynamics(actuator_delay, actuator_lag=actuator_lag, ) baseline = _run(controller_enabled=False, **common) - controlled = _run(controller_enabled=True, profile=1, **common) - + trace = _run(controller_enabled=True, profile=profile, **common) baseline_onset = _sustained_time_below(baseline, -0.10) - controlled_onset = _sustained_time_below(controlled, -0.10) - assert controlled_onset <= baseline_onset - 0.5 - - # Earlier onset cannot worsen routine peak deceleration or realized jerk. - assert controlled.acceleration.min() >= baseline.acceleration.min() - 0.1 - baseline_jerk = _filtered_realized_jerk(baseline) - controlled_jerk = _filtered_realized_jerk(controlled) - clean_base_jerk_p95 = float(np.percentile(np.abs(baseline_jerk), 95)) - controller_jerk_p95 = float(np.percentile(np.abs(controlled_jerk), 95)) - record_property("clean_base_filtered_realized_jerk_p95", clean_base_jerk_p95) - record_property("current_tn_filtered_realized_jerk_p95", current_tn_jerk_p95) - record_property("accel_controller_filtered_realized_jerk_p95", controller_jerk_p95) - assert np.isfinite(clean_base_jerk_p95) - assert np.isfinite(controller_jerk_p95) - if controller_jerk_p95 > current_tn_jerk_p95: - pytest.xfail("opt-in validation: filtered realized-jerk p95 still exceeds the saved current-tn comparator") - assert controller_jerk_p95 <= current_tn_jerk_p95 + trace_onset = _sustained_time_below(trace, -0.10) + early_restrictive_pace = (trace.pace < trace.speed - 0.01) & (trace.time <= baseline_onset) + assert early_restrictive_pace.any() + assert np.max(trace.a_target[early_restrictive_pace] - baseline.a_target[early_restrictive_pace]) <= 1e-6 + assert trace_onset <= baseline_onset - 0.5 + assert trace.acceleration.min() >= baseline.acceleration.min() - 0.1 + baseline_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)) + trace_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) + assert trace_p95 <= max(0.15, baseline_p95 + 0.02) + assert not _has_brake_gas_brake(trace.a_target[trace.time >= 1.0]) + assert trace.solver_failures == 0 -def test_profiles_order_anticipation_and_pace_rates(): - common = dict( - duration=10.0, - controller_enabled=True, - lead_relevancy=True, - speed=25.0, - distance_lead=200.0, - v_lead=15.0, - actuator_delay=0.20, - actuator_lag=0.25, - ) - traces = [_run(profile=profile, **common) for profile in range(3)] - onsets = [] - for trace in traces: - restricting = np.flatnonzero(np.diff(trace.pace) < -1e-6) - assert len(restricting) - onsets.append(float(trace.time[restricting[0] + 1])) - assert onsets[0] < onsets[1] < onsets[2] +def test_solver_fault_discards_live_state_and_recovers(): + _set_params(enabled=True, profile=1) + plant = Plant(speed=0.0, actuator_delay=0.15, actuator_lag=0.20) + plant.step(v_cruise=15.0) + assert plant.planner.accel_controller_result.active + assert plant.planner.mpc.last_solution_status == 0 - expected_down_rates = [0.25, 0.335, 0.50] - measured_down_rates = [] - for trace in traces: - restricting = np.flatnonzero(np.diff(trace.pace) < -1e-6) - measured_down_rates.append(float(np.median(-np.diff(trace.pace)[restricting] / DT_MDL))) - np.testing.assert_allclose(measured_down_rates, expected_down_rates, atol=1e-6, rtol=0.0) + plant.planner.mpc.last_solution_status = 3 + plant.planner.mpc.reset() + for _ in range(3): + plant.step(v_cruise=15.0) + faulted = plant.planner.accel_controller_result + assert not faulted.active + assert np.isinf(faulted.live_pace) + assert faulted.target_speed == 15.0 + assert faulted.mpc_accel_max is not None + if plant.planner.mpc.last_solution_status == 0: + break + assert plant.planner.mpc.last_solution_status == 0 + + plant.step(v_cruise=15.0) + recovered = plant.planner.accel_controller_result + assert recovered.active + assert np.isfinite(recovered.live_pace) + + +def test_solver_fault_keeps_restrictive_lead_target_until_recovery(): + _set_params(enabled=True, profile=1) + plant = Plant(lead_relevancy=True, speed=25.0, distance_lead=200.0, actuator_delay=0.15, actuator_lag=0.20) + plant.v_lead_prev = 15.0 + for _ in range(30): + plant.step(v_lead=15.0, v_cruise=30.0) + + previous_target = plant.planner.accel_controller_result.target_speed + assert previous_target < 30.0 + assert plant.planner.mpc.last_solution_status == 0 + + plant.planner.mpc.last_solution_status = 3 + plant.planner.mpc.reset() + for _ in range(3): + result = plant.step(v_lead=15.0, v_cruise=30.0) + faulted = plant.planner.accel_controller_result + assert not faulted.active + assert faulted.target_speed <= previous_target + 1e-9 + assert result["a_target"] <= 0.2 + if plant.planner.mpc.last_solution_status == 0: + break + assert plant.planner.mpc.last_solution_status == 0 + + plant.step(v_lead=15.0, v_cruise=30.0) + assert plant.planner.accel_controller_result.active