mica

selfdrive/controls/lib/longitudinal_planner.py

@@ -378,6 +378,7 @@ 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
 NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP = 0.18
+DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN = 0.35
 DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED = 12.0
 DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB = 0.95
 DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL = 0.35
@@ -602,6 +603,7 @@ class LongitudinalPlanner:
     self.lead_depart_accel_hold_floor = None
     self.spacious_follow_cap_bypass_until = 0.0
     self.post_departure_follow_settle_until = 0.0
+    self.duplicate_vision_comfort_lead_source = None
 
     if self.is_preap:
       try:
@@ -1763,6 +1765,44 @@ class LongitudinalPlanner:
       return self.lead_two
     return None
 
+  def get_duplicate_vision_comfort_lead(self, v_ego):
+    if not (self.lead_one.status and self.lead_two.status):
+      self.duplicate_vision_comfort_lead_source = None
+      return None
+
+    if bool(getattr(self.lead_one, "radar", False)) or bool(getattr(self.lead_two, "radar", False)):
+      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):
+      self.duplicate_vision_comfort_lead_source = None
+      return None
+
+    lead_sources = {
+      "lead0": self.lead_one,
+      "lead1": self.lead_two,
+    }
+    latched_lead = lead_sources.get(self.duplicate_vision_comfort_lead_source)
+    if latched_lead is not None and latched_lead.status:
+      return latched_lead
+
+    min_center_offset = min(abs(float(getattr(lead, "yRel", 0.0))) for lead in lead_sources.values())
+    centered_sources = [
+      (source, lead) for source, lead in lead_sources.items()
+      if abs(float(getattr(lead, "yRel", 0.0))) <= min_center_offset + DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN
+    ]
+    selected_source, selected_lead = min(
+      centered_sources,
+      key=lambda item: (
+        float(item[1].dRel),
+        float(item[1].vLead),
+        -max(0.0, -float(getattr(item[1], "aLeadK", 0.0))),
+        abs(float(getattr(item[1], "yRel", 0.0))),
+      ),
+    )
+    self.duplicate_vision_comfort_lead_source = selected_source
+    return selected_lead
+
   def lead_is_spacious_brake_cap_window(self, lead, v_ego, base_t_follow):
     if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED:
       return False
@@ -2915,6 +2955,8 @@ class LongitudinalPlanner:
         effective_t_follow,
         allow_optional_far_lead_logic=True,
       )
+    duplicate_vision_comfort_lead = self.get_duplicate_vision_comfort_lead(scene_v_ego) if allow_complex_follow_logic else None
+    comfort_follow_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None and follow_control_lead is not None else follow_control_lead
     if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass:
       if not output_should_stop and not vision_low_speed_stop_active:
         tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor(
@@ -2946,7 +2988,9 @@ class LongitudinalPlanner:
           self.a_desired = max(self.a_desired, low_speed_transition_brake_cap)
           output_a_target = max(output_a_target, low_speed_transition_brake_cap)
 
-    comfort_lead = self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one
+    comfort_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None else (
+      self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one
+    )
     if allow_complex_follow_logic and comfort_lead is not None and not panic_bypass:
       far_lead_brake_cap = self.get_far_lead_brake_cap(comfort_lead, scene_v_ego, effective_t_follow)
       if far_lead_brake_cap is not None:
@@ -2964,9 +3008,9 @@ class LongitudinalPlanner:
         self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap)
         output_a_target = max(output_a_target, tracked_vision_model_brake_cap)
 
-    if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
+    if allow_complex_follow_logic and comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
       matched_follow_transition_target = self.get_matched_follow_transition_target(
-        follow_control_lead,
+        comfort_follow_lead,
         scene_v_ego,
         effective_t_follow,
         prev_output_a_target,
@@ -3014,9 +3058,9 @@ class LongitudinalPlanner:
           self.a_desired = max(self.a_desired, duplicate_slow_lead_brake_hold_target)
         output_a_target = duplicate_slow_lead_brake_hold_target
 
-    if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
+    if allow_complex_follow_logic and comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
       cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap(
-        follow_control_lead,
+        comfort_follow_lead,
         scene_v_ego,
         effective_t_follow,
         self.mpc.source,
@@ -3027,7 +3071,7 @@ class LongitudinalPlanner:
         output_a_target = min(output_a_target, cruise_tracking_lead_accel_cap)
 
       cruise_tracking_lead_transition_target = self.get_cruise_tracking_lead_accel_transition_target(
-        follow_control_lead,
+        comfort_follow_lead,
         scene_v_ego,
         effective_t_follow,
         prev_output_a_target,
@@ -3039,7 +3083,7 @@ class LongitudinalPlanner:
         output_a_target = min(output_a_target, cruise_tracking_lead_transition_target)
 
       mild_follow_zero_cross_guard_target = self.get_mild_follow_zero_cross_guard_target(
-        follow_control_lead,
+        comfort_follow_lead,
         scene_v_ego,
         effective_t_follow,
         prev_output_a_target,

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -3052,6 +3052,65 @@ def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads():
   assert lead_1_bias == 0.0
 
 
+def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_offset_candidate():
+  v_ego = 25.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=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10)
+  lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05)
+  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
+  assert planner.duplicate_vision_comfort_lead_source == "lead0"
+
+
+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)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10)
+  lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05)
+  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
+
+  initial = planner.get_duplicate_vision_comfort_lead(v_ego)
+
+  lead_one.yRel = 1.25
+  lead_two.yRel = 0.05
+  lead_one.dRel = 44.4
+  lead_two.dRel = 44.5
+  latched = planner.get_duplicate_vision_comfort_lead(v_ego)
+
+  assert initial is lead_one
+  assert latched is lead_one
+  assert planner.duplicate_vision_comfort_lead_source == "lead0"
+
+
+def test_duplicate_vision_comfort_lead_resets_when_duplicate_cluster_clears():
+  v_ego = 25.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=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10)
+  lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05)
+  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
+
+  assert planner.get_duplicate_vision_comfort_lead(v_ego) is lead_one
+
+  lead_two.dRel = 49.5
+  assert planner.get_duplicate_vision_comfort_lead(v_ego) is None
+  assert planner.duplicate_vision_comfort_lead_source is None
+
+
 def test_near_duplicate_lead_transition_target_damps_same_source_sign_flip():
   v_ego = 25.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)