Mizuzu

selfdrive/controls/lib/lead_behavior.py

@@ -12,6 +12,11 @@ VISION_LEAD_TRACK_CLOSING_CAP = 2.50
 VISION_LEAD_TRACK_EXIT_TIME_GAP = 2.30
 VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET = 1.6
 VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB = 0.70
+VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB = 0.95
+VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET = 1.1
+VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN = 0.55
+VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED = 20.0
+VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED = 25.0
 TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.40
 TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.70
 TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75
@@ -65,7 +70,23 @@ def should_hold_tracked_vision_lead(lead_status: bool, lead_distance: float, mod
   model_limit = float(model_length) + tracking_buffer
   vision_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE,
                           float(v_ego) * VISION_LEAD_TRACK_EXIT_TIME_GAP + tracking_buffer)
-  return float(lead_distance) < min(model_limit, vision_exit_limit)
+  if float(lead_distance) < min(model_limit, vision_exit_limit):
+    return True
+
+  path_relative_offset = abs(float(y_rel) + float(path_y))
+  if (float(model_prob) < VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB or
+      path_relative_offset > VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET):
+    return False
+
+  speed_factor = 1.0 - _smoothstep(
+    v_ego,
+    VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED,
+    VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED,
+  )
+  continuity_time_gap = VISION_LEAD_TRACK_EXIT_TIME_GAP + VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN * speed_factor
+  continuity_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE,
+                              float(v_ego) * continuity_time_gap + tracking_buffer)
+  return float(lead_distance) < continuity_exit_limit
 
 
 def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lead: float, t_follow: float, *,

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -656,7 +656,7 @@ class LongitudinalMpc:
     return track_one >= 0 and track_one == track_two
 
   @staticmethod
-  def leads_are_near_duplicates(lead_one, lead_two, v_ego):
+  def leads_are_near_duplicates(lead_one, lead_two, v_ego, *, vision_min_speed=None):
     if lead_one is None or lead_two is None or not lead_one.status or not lead_two.status:
       return False
     if LongitudinalMpc.leads_share_identical_radar_track(lead_one, lead_two):
@@ -666,7 +666,8 @@ class LongitudinalMpc:
         abs(float(lead_one.dRel) - float(lead_two.dRel)) <= NEAR_DUPLICATE_LEAD_SOURCE_MAX_DREL_DIFF and
         abs(float(lead_one.vRel) - float(lead_two.vRel)) <= max(1.0, NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF)
       )
-    if float(v_ego) < NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED:
+    min_vision_speed = NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED if vision_min_speed is None else float(vision_min_speed)
+    if float(v_ego) < min_vision_speed:
       return False
     lead_one_radar = bool(getattr(lead_one, "radar", False))
     lead_two_radar = bool(getattr(lead_two, "radar", False))

selfdrive/controls/lib/longitudinal_planner.py

@@ -402,6 +402,7 @@ NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET = 0.85
 NEAR_DUPLICATE_VISION_TRANSITION_MIN_HEADWAY_MARGIN = 0.55
 NEAR_DUPLICATE_VISION_TRANSITION_EXTRA_CLOSING_SPEED = 1.25
 LOW_SPEED_IDENTICAL_RADAR_DUPLICATE_TRANSITION_EXTRA_HEADWAY = 0.15
+LOW_SPEED_DUPLICATE_VISION_TRANSITION_EXTRA_HEADWAY = 0.75
 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35
 NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22
 NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP = 0.32
@@ -1888,7 +1889,12 @@ class LongitudinalPlanner:
       self.duplicate_vision_comfort_lead_source = None
       return None
 
-    if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego):
+    if not self.mpc.leads_are_near_duplicates(
+      self.lead_one,
+      self.lead_two,
+      v_ego,
+      vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED,
+    ):
       self.duplicate_vision_comfort_lead_source = None
       return None
 
@@ -2106,7 +2112,12 @@ class LongitudinalPlanner:
     if not (self.lead_one.status and self.lead_two.status):
       return None
     identical_radar_duplicates = self.mpc.leads_share_identical_radar_track(self.lead_one, self.lead_two)
-    if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego):
+    if not self.mpc.leads_are_near_duplicates(
+      self.lead_one,
+      self.lead_two,
+      v_ego,
+      vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED,
+    ):
       return None
     low_speed_extension_active = bool(
       tracking_lead_active and
@@ -2135,6 +2146,8 @@ class LongitudinalPlanner:
     max_headway_above_target = NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET
     if low_speed_extension_active and identical_radar_duplicates:
       max_headway_above_target += LOW_SPEED_IDENTICAL_RADAR_DUPLICATE_TRANSITION_EXTRA_HEADWAY
+    elif low_speed_extension_active and not lead_radar:
+      max_headway_above_target += LOW_SPEED_DUPLICATE_VISION_TRANSITION_EXTRA_HEADWAY
     if actual_headway > float(base_t_follow) + max_headway_above_target:
       return None
 

selfdrive/controls/tests/test_lead_behavior.py

@@ -158,6 +158,20 @@ def test_should_hold_tracked_vision_lead_releases_beyond_exit_gap():
   )
 
 
+def test_should_hold_tracked_vision_lead_ignores_shortened_model_horizon_in_bolt_stutter_case():
+  assert should_hold_tracked_vision_lead(
+    True, 54.6, 40.0, 6.0, 19.4,
+    model_prob=1.0, y_rel=0.05, radar=False,
+  )
+
+
+def test_should_hold_tracked_vision_lead_does_not_extend_low_confidence_short_horizon_case():
+  assert not should_hold_tracked_vision_lead(
+    True, 54.6, 40.0, 6.0, 19.4,
+    model_prob=0.90, y_rel=0.05, radar=False,
+  )
+
+
 def test_radarless_matched_follow_window_accepts_pace_matched_highway_follow():
   assert is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95)
 

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -3658,6 +3658,22 @@ def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_
   assert planner.duplicate_vision_comfort_lead_source == "lead0"
 
 
+def test_duplicate_vision_comfort_lead_supports_mid_speed_follow_churn():
+  v_ego = 16.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=30.0, v_lead=15.8, a_lead=0.02, radar=False, model_prob=1.0, y_rel=0.05)
+  lead_two = make_lead(status=True, d_rel=30.1, v_lead=15.82, a_lead=0.01, radar=False, model_prob=1.0, y_rel=0.08)
+  lead_one.vRel = lead_one.vLead - v_ego
+  lead_two.vRel = lead_two.vLead - v_ego
+  planner.lead_one = lead_one
+  planner.lead_two = lead_two
+
+  selected = planner.get_duplicate_vision_comfort_lead(v_ego)
+
+  assert selected is lead_one
+
+
 def test_duplicate_vision_comfort_lead_latches_source_until_duplicate_cluster_resolves():
   v_ego = 25.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -3777,6 +3793,31 @@ def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_h
   assert smoothed == pytest.approx(0.57, abs=1e-6)
 
 
+def test_near_duplicate_lead_transition_target_damps_mid_speed_duplicate_vision_handoff():
+  v_ego = 17.6
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=44.8, v_lead=16.8, a_lead=0.0, radar=False, model_prob=1.0)
+  lead_two = make_lead(status=True, d_rel=44.9, v_lead=16.82, a_lead=0.0, radar=False, model_prob=1.0)
+  lead_one.vRel = lead_one.vLead - v_ego
+  lead_two.vRel = lead_two.vLead - v_ego
+  planner.lead_one = lead_one
+  planner.lead_two = lead_two
+
+  smoothed = planner.get_near_duplicate_lead_transition_target(
+    lead_one,
+    v_ego,
+    1.25,
+    prev_output_a_target=0.49,
+    output_a_target=0.03,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert smoothed is not None
+  assert smoothed == pytest.approx(0.17, abs=1e-6)
+
+
 def test_near_duplicate_lead_transition_target_damps_generous_headway_duplicate_vision_sign_flip():
   v_ego = 25.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)