Dom's Plan

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)

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

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)

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")

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,

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,

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

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