From 6d0559c6d918ea04de4f47d4b0efeb6dcb7ff7ab Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 25 Apr 2026 20:05:18 -0500 Subject: [PATCH] Dom's Plan --- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- .../controls/lib/longitudinal_planner.py | 15 ++++--- .../controls/tests/test_lead_behavior.py | 17 +++++++ .../tests/test_longitudinal_planner.py | 44 ++++++++++++++++++- .../test/longitudinal_maneuvers/maneuver.py | 2 + .../test/longitudinal_maneuvers/plant.py | 27 +++++++++++- .../test_longitudinal.py | 39 ++++++++++++++++ starpilot/controls/starpilot_planner.py | 21 +++++---- 8 files changed, 146 insertions(+), 21 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 39f08b886..c665ce7e3 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -523,7 +523,7 @@ class LongitudinalMpc: v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo - self.status = (lead_one.status and tracking_lead) or lead_two.status + self.status = tracking_lead and (lead_one.status or lead_two.status) lead_xv_0 = self.process_lead(lead_one, tracking_lead) lead_xv_1 = self.process_lead(lead_two, tracking_lead) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 68777dbfe..928cc5bbd 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -338,8 +338,9 @@ class LongitudinalPlanner: # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead # safety path so ACC/chill does not ignore a visible lead during that debounce. lead_control_active = tracking_lead or raw_close_lead_control + lead_one_active = bool(self.lead_one.status and lead_control_active) - lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0 + lead_dist = self.lead_one.dRel if lead_one_active else 50.0 # Smooth lead distance (EMA) to avoid chatter in thresholds alpha = max(0.02, min(0.15, 0.05 + 0.002 * v_ego)) @@ -351,7 +352,7 @@ class LongitudinalPlanner: # Lead stability estimation and recent-brake timer now_t = time.monotonic() # relative speed (ego - lead) positive when closing - v_rel = (v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0 + v_rel = (v_ego - self.lead_one.vLead) if lead_one_active else 0.0 if self.prev_lead_dist is None: d_rel_dot = 0.0 else: @@ -365,7 +366,7 @@ class LongitudinalPlanner: # Stable lead heuristic (short window, cheap to compute) recently_braked = (now_t - self.last_big_brake_t) < 0.7 self.stable_lead = ( - self.lead_one.status and + lead_one_active and abs(v_rel) < 0.5 and abs(d_rel_dot) < 0.5 and not recently_braked @@ -427,13 +428,13 @@ class LongitudinalPlanner: self._uncert_last = uncertainty self._uncert_last_t = now_t - closing_fast = (self.lead_one.status and (v_ego - self.lead_one.vLead) > 0.5) + closing_fast = lead_one_active and (v_ego - self.lead_one.vLead) > 0.5 # Trigger if either slope is high or magnitude is high; require a valid lead and closing panic_bypass = closing_fast and (uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG) if panic_bypass: try: - cloudlog.error(f"LON_SLOPE; slope={uncert_slope:.3f}/s; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f}; v_rel={(v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0:.2f}; lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}; trigger=True") + cloudlog.error(f"LON_SLOPE; slope={uncert_slope:.3f}/s; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f}; v_rel={(v_ego - self.lead_one.vLead) if lead_one_active else 0.0:.2f}; lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}; trigger=True") except Exception: pass @@ -445,7 +446,7 @@ class LongitudinalPlanner: prev_accel_constraint, personality=personality, v_ego=v_ego, - lead_dist=self.lead_dist_f if self.lead_dist_f is not None else lead_dist, + lead_dist=self.lead_dist_f if lead_one_active and self.lead_dist_f is not None else 50.0, uncertainty=uncertainty, panic_bypass=panic_bypass) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) @@ -482,7 +483,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 # Anticipatory pre-brake to avoid "coming in hot" when closing on a lead - if self.lead_one.status: + if lead_one_active: rel_v = max(0.0, v_ego - self.lead_one.vLead) # dynamic time headway adds a small buffer when uncertainty is elevated base_th = 1.6 diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index 0b71f8751..30f143029 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -1,5 +1,6 @@ from openpilot.selfdrive.controls.lib.lead_behavior import ( get_tracked_lead_catchup_bias, + should_track_lead, should_disable_far_lead_throttle, ) @@ -42,3 +43,19 @@ def test_disable_far_lead_throttle_keeps_mild_coast_near_target_gap(): def test_disable_far_lead_throttle_rejects_fast_closing(): should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 3.5, False) assert not should_disable + + +def test_should_track_lead_keeps_radar_leads_on_model_horizon(): + assert should_track_lead(True, 95.0, 100.0, 6.0, 30.0, v_lead=25.0, radar=True) + + +def test_should_track_lead_rejects_far_vision_only_highway_lead(): + assert not should_track_lead(True, 82.0, 140.0, 6.0, 29.0, v_lead=25.0, radar=False) + + +def test_should_track_lead_accepts_closer_vision_only_highway_lead(): + assert should_track_lead(True, 56.0, 140.0, 6.0, 29.0, v_lead=25.0, radar=False) + + +def test_should_track_lead_accepts_fast_closing_vision_lead_early(): + assert should_track_lead(True, 90.0, 140.0, 6.0, 20.0, v_lead=0.0, radar=False) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 262a568ab..122dcfe3f 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -13,7 +13,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl from openpilot.selfdrive.modeld.constants import ModelConstants, Plan -def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0): +def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0, + radar: bool = False, model_prob: float = 0.0): lead = log.RadarState.LeadData.new_message() lead.status = status lead.dRel = d_rel @@ -22,7 +23,8 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead lead.aLeadK = a_lead lead.vRel = 0.0 lead.aRel = 0.0 - lead.modelProb = 0.0 + lead.modelProb = model_prob + lead.radar = radar return lead @@ -148,6 +150,44 @@ def test_acc_mode_uses_close_raw_lead_when_tracking_lead_is_debounced(model_vers ) +@pytest.mark.parametrize("model_version", ["v11", "v12"]) +def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_tracking(model_version): + v_ego = 29.0 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) + planner_far_vision = LongitudinalPlanner(CP, init_v=v_ego) + sm_no_lead = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + ) + sm_far_vision = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=82.0, v_lead=25.0, radar=False, model_prob=0.9), + ) + sm_no_lead["starpilotPlan"].vCruise = v_ego + 2.0 + sm_far_vision["starpilotPlan"].vCruise = v_ego + 2.0 + + no_lead_outputs = [] + far_vision_outputs = [] + for _ in range(8): + planner_no_lead.update(sm_no_lead, make_toggles(model_version)) + planner_far_vision.update(sm_far_vision, make_toggles(model_version)) + no_lead_outputs.append(planner_no_lead.output_a_target) + far_vision_outputs.append(planner_far_vision.output_a_target) + + assert planner_far_vision.mode == "acc" + assert not planner_far_vision.raw_close_lead_needs_control(sm_far_vision["radarState"].leadOne, v_ego) + np.testing.assert_allclose(far_vision_outputs, no_lead_outputs, atol=1e-6) + + def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch): monkeypatch.setenv("DEBUG", "0") fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index dfd5b3e10..2d1e8e5b2 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -18,6 +18,7 @@ class Maneuver: self.only_lead2 = kwargs.get("only_lead2", False) self.only_radar = kwargs.get("only_radar", False) + self.track_lead_with_gate = kwargs.get("track_lead_with_gate", False) self.ensure_start = kwargs.get("ensure_start", False) self.ensure_slowdown = kwargs.get("ensure_slowdown", False) self.enabled = kwargs.get("enabled", True) @@ -36,6 +37,7 @@ class Maneuver: enabled=self.enabled, only_lead2=self.only_lead2, only_radar=self.only_radar, + track_lead_with_gate=self.track_lead_with_gate, e2e=self.e2e, personality=self.personality, force_decel=self.force_decel, diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 1b688a1e9..57ebd9539 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -10,11 +10,15 @@ from types import SimpleNamespace from cereal import log import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import Ratekeeper, DT_MDL from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.lead_behavior import should_track_lead from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from openpilot.starpilot.common.starpilot_variables import THRESHOLD class Plant: @@ -51,7 +55,8 @@ class Plant: gc.collect() def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, - enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False): + enabled=True, only_lead2=False, only_radar=False, track_lead_with_gate=False, + e2e=False, personality=0, force_decel=False): self.rate = 1. / DT_MDL current_prefix = os.environ.get("OPENPILOT_PREFIX") @@ -82,9 +87,11 @@ class Plant: self.enabled = enabled self.only_lead2 = only_lead2 self.only_radar = only_radar + self.track_lead_with_gate = track_lead_with_gate self.e2e = e2e self.personality = personality self.force_decel = force_decel + self.tracking_lead_filter = FirstOrderFilter(0.0, 0.5, DT_MDL) self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate @@ -179,6 +186,22 @@ class Plant: car_state.carState.vCruise = float(v_cruise * 3.6) car_control.carControl.orientationNED = [0., float(pitch), 0.] + tracking_lead = bool(status) + if self.track_lead_with_gate: + tracking_candidate = should_track_lead( + status, + float(d_rel), + float(position.x[-1]) if len(position.x) else 0.0, + STOP_DISTANCE, + float(self.speed), + v_lead=float(v_lead), + radar=bool(self.only_radar), + ) + self.tracking_lead_filter.update(tracking_candidate) + tracking_lead = self.tracking_lead_filter.x >= THRESHOLD + else: + self.tracking_lead_filter.update(float(status)) + # ******** get controlsState messages for plotting *** starpilot_plan = SimpleNamespace( vCruise=float(v_cruise), @@ -189,7 +212,7 @@ class Plant: accelerationJerk=5.0, dangerJerk=5.0, speedJerk=5.0, - trackingLead=bool(status), + trackingLead=bool(tracking_lead), desiredFollowDistance=float(d_rel), dangerFactor=1.0, tFollow=1.45, diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index ab1800b4f..388d7264c 100644 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -1,5 +1,6 @@ import itertools from parameterized import parameterized_class +import pytest from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -189,3 +190,41 @@ class TestLongitudinalControl: print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode') valid, _ = maneuver.evaluate() assert valid + + +@pytest.mark.parametrize("maneuver", [ + Maneuver( + 'vision gate: approach stopped car at 25m/s, initial distance 120m', + duration=20., + initial_speed=25., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[30., 0.], + breakpoints=[0., 1.], + track_lead_with_gate=True, + ), + Maneuver( + 'vision gate: approach stopped car at 20m/s, initial distance 90m', + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=90., + speed_lead_values=[20., 0.], + breakpoints=[0., 1.], + track_lead_with_gate=True, + ), + Maneuver( + 'vision gate: approach slower cut-in car at 20m/s', + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=50., + speed_lead_values=[15., 15.], + breakpoints=[1., 11.], + only_lead2=True, + track_lead_with_gate=True, + ), +], ids=lambda maneuver: maneuver.title) +def test_tracking_lead_gate_maneuvers(maneuver): + valid, _ = maneuver.evaluate() + assert valid diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index a6cb09bee..ffb432116 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -10,6 +10,7 @@ from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET +from openpilot.selfdrive.controls.lib.lead_behavior import should_track_lead from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature @@ -124,7 +125,7 @@ class StarPilotPlanner: self.road_curvature_detected = (1 / abs(self.road_curvature))**0.5 < v_ego > CRUISING_SPEED and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) if not sm["carState"].standstill: - self.tracking_lead = self.update_lead_status(starpilot_toggles.stop_distance) + self.tracking_lead = self.update_lead_status(v_ego, starpilot_toggles.stop_distance) self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles) @@ -145,14 +146,16 @@ class StarPilotPlanner: else: self.starpilot_weather.weather_id = 0 - def update_lead_status(self, stop_distance=STOP_DISTANCE): - # Keep a minimum lead-tracking cushion independent of user stop-distance tuning. - # Regressions here can cause ACC/chill to de-prioritize lead control at low speeds. - tracking_buffer = max(float(stop_distance), 4.0) - - following_lead = self.lead_one.status - following_lead &= self.lead_one.dRel < self.model_length + tracking_buffer - + def update_lead_status(self, v_ego, stop_distance=STOP_DISTANCE): + following_lead = should_track_lead( + self.lead_one.status, + self.lead_one.dRel, + self.model_length, + stop_distance, + v_ego, + v_lead=self.lead_one.vLead, + radar=bool(getattr(self.lead_one, "radar", False)), + ) self.tracking_lead_filter.update(following_lead) return self.tracking_lead_filter.x >= THRESHOLD