From 361996f4bccd6b6d05b0c70f06db84c95b64816d Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 18 Aug 2026 11:38:42 -0700 Subject: [PATCH] long: bit faster --- .../lib/accel_controller/accel_controller.py | 19 ++- .../tests/test_accel_controller.py | 35 +++- .../controls/lib/longitudinal_planner.py | 2 +- .../tests/test_plant_sp.py | 150 +++++++++++++++++- 4 files changed, 195 insertions(+), 11 deletions(-) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index ee070e0cc9..5450f26d82 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -15,7 +15,7 @@ from openpilot.sunnypilot import get_sanitize_int_param AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile MAX_ACCEL_PROFILES = { - AccelProfile.eco: [1.45, 1.40, 1.20, 0.65, 0.48, 0.36, 0.22, 0.085, 0.055, 0.045], + AccelProfile.eco: [1.45, 1.40, 1.20, 0.85, 0.62, 0.36, 0.22, 0.085, 0.055, 0.045], AccelProfile.normal: [2.00, 1.95, 1.80, 1.06, 0.81, 0.69, 0.42, 0.160, 0.10, 0.08], AccelProfile.sport: [2.00, 1.99, 1.95, 1.45, 1.10, 0.82, 0.53, 0.240, 0.13, 0.09], } @@ -36,8 +36,12 @@ LEAD_GAP_WIDEN_PROFILES = { AccelProfile.normal: 0.20, AccelProfile.sport: 0.10, } -LEAD_DECEL_FOR_MAX_WIDEN = 3.0 # m/s^2, lead decel that saturates the widen amount -GAP_WIDEN_ONSET_ALPHA = 0.15 +LEAD_DECEL_FOR_MAX_WIDEN = 3.0 # m/s^2, lead decel (aLeadK) that saturates the widen amount +# aLeadK is a differentiated, filtered estimate -- it can lag several tenths of a second behind +# the lead actually closing. vRel is measured directly every frame, so closing speed alone (even +# before aLeadK has caught up) can independently trigger the same widen. +LEAD_CLOSING_FOR_MAX_WIDEN = 5.0 # m/s, closing speed (-vRel) that alone saturates the widen amount +GAP_WIDEN_ONSET_ALPHA = 0.12 GAP_WIDEN_RELEASE_ALPHA = 0.08 # Taper out below city speed so the lever only shapes higher-speed anticipation. GAP_WIDEN_TAPER_LOW_SPEED = 3.0 # m/s, widen fully tapered out at/below this speed @@ -88,12 +92,11 @@ class AccelController: self.last_min_accel = min(self.last_min_accel, self.last_max_accel - 0.1) return float(self.last_min_accel) - def get_t_follow_multiplier(self, lead_present: bool, lead_accel: float, v_ego: float) -> float: + def get_t_follow_multiplier(self, lead_present: bool, lead_accel: float, v_ego: float, lead_v_rel: float = 0.0) -> float: max_widen = LEAD_GAP_WIDEN_PROFILES[self._profile] - if lead_present and lead_accel < 0.0: - target_widen = min(-lead_accel / LEAD_DECEL_FOR_MAX_WIDEN, 1.0) * max_widen - else: - target_widen = 0.0 + decel_widen = min(-lead_accel / LEAD_DECEL_FOR_MAX_WIDEN, 1.0) if lead_accel < 0.0 else 0.0 + closing_widen = min(-lead_v_rel / LEAD_CLOSING_FOR_MAX_WIDEN, 1.0) if lead_v_rel < 0.0 else 0.0 + target_widen = max(decel_widen, closing_widen) * max_widen if lead_present else 0.0 alpha = GAP_WIDEN_ONSET_ALPHA if target_widen > self.last_t_follow_widen else GAP_WIDEN_RELEASE_ALPHA self.last_t_follow_widen += alpha * (target_widen - self.last_t_follow_widen) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 428e2f3ac3..abce01e9dd 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -28,7 +28,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import ( AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES, - LEAD_GAP_WIDEN_PROFILES, LEAD_DECEL_FOR_MAX_WIDEN, + LEAD_GAP_WIDEN_PROFILES, LEAD_DECEL_FOR_MAX_WIDEN, LEAD_CLOSING_FOR_MAX_WIDEN, ) @@ -233,6 +233,39 @@ class TestLeadGapWiden(OpenpilotTestCase): multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, v_ego=20.0) self.assertAlmostEqual(multiplier, 1.0 + LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal], places=2) + def test_closing_lead_widens_even_with_zero_lead_accel(self): + # aLeadK is a laggy, differentiated estimate -- vRel (measured directly) must be able to + # trigger the same widen on its own, before aLeadK has caught up to a real closing event. + for _ in range(200): + multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0, + lead_v_rel=-LEAD_CLOSING_FOR_MAX_WIDEN * 2) + self.assertAlmostEqual(multiplier, 1.0 + LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal], places=2) + + def test_opening_lead_v_rel_never_widens(self): + for _ in range(50): + multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0, lead_v_rel=3.0) + self.assertEqual(multiplier, 1.0) + + def test_widen_takes_whichever_signal_is_more_urgent(self): + # a laggy aLeadK=0 (not yet updated) shouldn't suppress an already-strong closing-rate signal + for _ in range(200): + multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0, + lead_v_rel=-LEAD_CLOSING_FOR_MAX_WIDEN * 2) + from_v_rel_only = multiplier + for _ in range(200): + multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, + v_ego=20.0, lead_v_rel=0.0) + from_a_lead_only = multiplier + self.assertAlmostEqual(from_v_rel_only, from_a_lead_only, places=2) + + def test_lead_v_rel_default_matches_pre_existing_callers(self): + # regression guard: existing call sites that don't pass lead_v_rel must be unaffected + controller_a, controller_b = AccelController(), AccelController() + for _ in range(50): + a = controller_a.get_t_follow_multiplier(lead_present=True, lead_accel=-1.0, v_ego=20.0) + b = controller_b.get_t_follow_multiplier(lead_present=True, lead_accel=-1.0, v_ego=20.0, lead_v_rel=0.0) + self.assertEqual(a, b) + def test_widen_never_shrinks_below_stock(self): for lead_accel in [-0.5, -1.5, -3.0, -6.0, 0.5, 0.0]: multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=lead_accel, v_ego=20.0) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 85a9fabd02..67c6019cae 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -58,7 +58,7 @@ class LongitudinalPlannerSP: if not self.accel_controller.is_enabled(): return None lead = sm['radarState'].leadOne - return self.accel_controller.get_t_follow_multiplier(lead.present, lead.aLeadK, v_ego) + return self.accel_controller.get_t_follow_multiplier(lead.present, lead.aLeadK, v_ego, lead.vRel) def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: CS = sm['carState'] diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py index e99e578101..55b7b06de1 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py @@ -1,12 +1,17 @@ +from collections import deque from collections.abc import Callable import math from typing import cast +from unittest import mock from openpilot.common.parameterized import parameterized +from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant -from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import accel_controller as accel_controller_module +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelProfile +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw") @@ -162,3 +167,146 @@ class TestPlantSP(OpenpilotTestCase): def test_invalid_actuator_dynamics(self, delay, lag): with self.assertRaises(ValueError): PlantSP(actuator_delay=delay, actuator_lag=lag) + + def test_v_rel_widen_anticipates_lagged_a_lead_k_without_safety_regression(self): + # Real radar's aLeadK is a filtered/differentiated estimate that lags the lead actually + # closing (confirmed on a recorded route: vRel escalated ~1s before aLeadK caught up). + # get_t_follow_multiplier's vRel trigger exists to widen t_follow before aLeadK updates -- + # this drives that scenario through the real closed-loop MPC while keeping dRel/vRel live. + params = Params() + params.put_bool("AccelPersonalityEnabled", True, block=True) + params.put("AccelPersonality", AccelProfile.normal, block=True) + + lead_accel_lag = 1.0 + brake_start = 3.0 # let the lead/MPC state settle for well over two seconds first + brake_end = 4.4 + lever_window_end = 4.45 # isolate pre-aLeadK widening while both synthetic arms remain solver-valid + full_event_end = 10.0 # include the complete brake and recovery response + lag_steps = round(lead_accel_lag / DT_MDL) + assert brake_start >= 2.0 + real_get_t_follow_multiplier = AccelController.get_t_follow_multiplier + + def run(strip_v_rel_widen: bool, onset_alpha: float, end_time: float): + # PlantSP publishes leadOne and leadTwo every frame. A shared history would advance + # twice per frame and silently turn this intended 1.0 s lag into 0.5 s. + histories = {lead_name: deque([0.0] * lag_steps, maxlen=lag_steps + 1) + for lead_name in ("leadOne", "leadTwo")} + lag_transitions = {lead_name: [] for lead_name in histories} + + def lagged_a_lead(current_time, lead_name, truth): + history = histories[lead_name] + history.append(truth["aLeadK"]) + out = dict(truth) + out["aLeadK"] = history[0] + lag_transitions[lead_name].append((current_time, truth["aLeadK"], out["aLeadK"])) + return out + + plant = PlantSP(lead_relevancy=True, speed=14.0, distance_lead=30.0, e2e=False, + lead_observation_fn=lagged_a_lead, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True) + v_lead = 14.0 + plant.v_lead_prev = v_lead # do not manufacture a 280 m/s^2 acceleration on the first frame + + solver_failures = 0 + original_reset = plant.planner.mpc.reset + + def counting_reset(*args, **kwargs): + nonlocal solver_failures + if plant.planner.mpc.solution_status != 0: + solver_failures += 1 + return original_reset(*args, **kwargs) + + plant.planner.mpc.reset = counting_reset + + def patched_t_follow(self, lead_present, lead_accel, v_ego, lead_v_rel=0.0): + return real_get_t_follow_multiplier(self, lead_present, lead_accel, v_ego, + 0.0 if strip_v_rel_widen else lead_v_rel) + + trace = [] + with mock.patch.object(accel_controller_module, "GAP_WIDEN_ONSET_ALPHA", onset_alpha), \ + mock.patch.object(AccelController, "get_t_follow_multiplier", patched_t_follow): + for _ in range(round(end_time / DT_MDL)): + current_time = plant.current_time + a_lead_cmd = -2.5 if brake_start <= current_time < brake_end else 0.0 + v_lead = max(0.0, v_lead + a_lead_cmd * plant.ts) + result = plant.step(v_lead=v_lead, v_cruise=22.0) + truth_lead = result["truth_lead"] + ttc = truth_lead["dRel"] / max(-truth_lead["vRel"], 1e-3) if truth_lead["vRel"] < 0.0 else None + trace.append({ + "time": current_time, + "realized_acceleration": result["realized_acceleration"], + "target_acceleration": result["a_target"], + "gap": result["distance_lead"] - result["distance"], + "ttc": ttc, + "fcw": result["fcw"], + "t_follow": result["t_follow_multiplier"], + }) + + event_index = round(brake_start / DT_MDL) + pre_event_accel = trace[event_index - 1]["realized_acceleration"] + onset_time = next(row["time"] for row in trace[event_index:] + if row["realized_acceleration"] <= pre_event_accel - 0.1) + measurement = trace[event_index - 1:] + realized_jerks = [(after["realized_acceleration"] - before["realized_acceleration"]) / DT_MDL + for before, after in zip(measurement, measurement[1:], strict=False)] + target_jerks = [(after["target_acceleration"] - before["target_acceleration"]) / DT_MDL + for before, after in zip(measurement, measurement[1:], strict=False)] + + measured_lags = {} + for lead_name, transitions in lag_transitions.items(): + true_onset = next(t for t, true_a, _ in transitions if true_a < -0.1) + observed_onset = next(t for t, _, observed_a in transitions if observed_a < -0.1) + measured_lags[lead_name] = observed_onset - true_onset + + return { + "solver_failures": solver_failures, + "lag_s": measured_lags, + "onset_s": onset_time, + "worst_negative_realized_jerk_mps3": min(realized_jerks), + "worst_negative_target_jerk_mps3": min(target_jerks), + "worst_positive_realized_jerk_mps3": max(realized_jerks), + "peak_realized_decel_mps2": min(row["realized_acceleration"] for row in measurement), + "peak_target_decel_mps2": min(row["target_acceleration"] for row in measurement), + "min_gap_m": min(row["gap"] for row in trace), + "end_gap_m": trace[-1]["gap"], + "min_ttc_s": min(row["ttc"] for row in trace if row["ttc"] is not None), + "max_t_follow_pre_lag": max(row["t_follow"] for row in trace + if brake_start <= row["time"] < brake_start + lead_accel_lag), + "fcw": any(row["fcw"] for row in trace), + } + + production_alpha = accel_controller_module.GAP_WIDEN_ONSET_ALPHA + reference_alpha = 0.15 + # Only isolate the controller's vRel-based t-follow widening in the short comparison. + # The real MPC still receives the same live radar vRel in every arm. + without_v_rel_widen = run(strip_v_rel_widen=True, onset_alpha=production_alpha, end_time=lever_window_end) + reference_onset = run(strip_v_rel_widen=False, onset_alpha=reference_alpha, end_time=full_event_end) + production = run(strip_v_rel_widen=False, onset_alpha=production_alpha, end_time=full_event_end) + diagnostics = f"without_v_rel_widen={without_v_rel_widen}, reference_onset={reference_onset}, production={production}" + + for metrics in (without_v_rel_widen, reference_onset, production): + assert metrics["solver_failures"] == 0, diagnostics + assert not metrics["fcw"], diagnostics + assert metrics["min_gap_m"] > 29.0, diagnostics + assert metrics["min_ttc_s"] > 15.0, diagnostics + for measured_lag in metrics["lag_s"].values(): + self.assertAlmostEqual(measured_lag, lead_accel_lag, delta=DT_MDL / 2.0, msg=diagnostics) + + self.assertAlmostEqual(without_v_rel_widen["max_t_follow_pre_lag"], 1.0, places=6, msg=diagnostics) + self.assertGreater(production["max_t_follow_pre_lag"], 1.015, diagnostics) + self.assertLessEqual(production["onset_s"], without_v_rel_widen["onset_s"] + DT_MDL, diagnostics) + + # The gentler onset filter must improve braking jerk over the previous 0.15 value + # without delaying the response or materially reducing separation. + self.assertLessEqual(production["onset_s"], reference_onset["onset_s"] + DT_MDL, diagnostics) + self.assertGreater(production["worst_negative_realized_jerk_mps3"], + reference_onset["worst_negative_realized_jerk_mps3"], diagnostics) + self.assertGreater(production["worst_negative_target_jerk_mps3"], + reference_onset["worst_negative_target_jerk_mps3"], diagnostics) + self.assertLessEqual(production["worst_positive_realized_jerk_mps3"], + reference_onset["worst_positive_realized_jerk_mps3"] + 0.1, diagnostics) + self.assertGreaterEqual(production["min_ttc_s"], reference_onset["min_ttc_s"] - 0.1, diagnostics) + self.assertGreaterEqual(production["end_gap_m"], reference_onset["end_gap_m"] - 0.5, diagnostics) + self.assertGreaterEqual(production["peak_realized_decel_mps2"], + reference_onset["peak_realized_decel_mps2"], diagnostics) + self.assertGreaterEqual(production["peak_target_decel_mps2"], + reference_onset["peak_target_decel_mps2"] - 0.1, diagnostics)