long plan

selfdrive/controls/lib/longitudinal_planner.py

@@ -216,6 +216,10 @@ POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB = 0.95
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA = 0.35
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_ACCEL = 0.25
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN = 0.10
+COMFORTABLE_PULLAWAY_FOLLOW_MIN_MODEL_PROB = 0.95
+COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_DELTA = -0.05
+COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_ACCEL = 0.20
+COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN = 0.20
 LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED = 12.0
 LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB = 0.85
 LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE = 0.20
@@ -1271,7 +1275,29 @@ class LongitudinalPlanner:
     headway_margin = actual_headway - float(t_follow)
     return headway_margin >= POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN
 
-  def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow):
+  def is_comfortable_accelerating_away_follow(self, lead, v_ego, t_follow):
+    if lead is None or not lead.status or float(v_ego) < POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED:
+      return False
+
+    lead_radar = bool(getattr(lead, "radar", False))
+    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
+    if not lead_radar and lead_prob < COMFORTABLE_PULLAWAY_FOLLOW_MIN_MODEL_PROB:
+      return False
+
+    if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET:
+      return False
+
+    lead_delta = float(lead.vLead) - float(v_ego)
+    lead_accel = float(getattr(lead, "aLeadK", 0.0))
+    if (lead_delta < COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_DELTA or
+        lead_accel < COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_ACCEL):
+      return False
+
+    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
+    headway_margin = actual_headway - float(t_follow)
+    return headway_margin >= COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN
+
+  def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow, current_source=None, tracking_lead_active=False):
     if lead is None or not lead.status:
       return None
 
@@ -1303,6 +1329,9 @@ class LongitudinalPlanner:
     if gap_error > gap_buffer:
       return None
 
+    if current_source == "cruise" and tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow):
+      return None
+
     if not low_speed_follow_window and self.is_stable_post_departure_pullaway(lead, v_ego, t_follow):
       return None
 
@@ -1374,6 +1403,9 @@ class LongitudinalPlanner:
     if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED:
       return None
 
+    if tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow):
+      return None
+
     closing_speed = max(float(v_ego) - float(lead.vLead), 0.0)
     raw_close_lead = self.raw_close_lead_needs_control(lead, v_ego)
     unresolved_slow_lead = (
@@ -2535,7 +2567,13 @@ class LongitudinalPlanner:
         close_final_guard_cap = min(close_final_guard_caps)
 
     if allow_complex_follow_logic and lead_one_active:
-      lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow)
+      lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(
+        self.lead_one,
+        scene_v_ego,
+        effective_t_follow,
+        current_source=self.mpc.source,
+        tracking_lead_active=tracking_lead,
+      )
       if lead_catchup_accel_cap is not None:
         self.a_desired = min(self.a_desired, lead_catchup_accel_cap)
         output_a_target = min(output_a_target, lead_catchup_accel_cap)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -1912,6 +1912,26 @@ def test_route_8bc6_post_departure_catchup_cap_skips_accelerating_away_radar_lea
   assert cap is None
 
 
+def test_route_8bc6_cruise_tracking_cap_skips_comfortable_accelerating_radar_follow():
+  v_ego = 18.744474411010742
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(
+    status=True, d_rel=33.650001525878906, v_lead=19.049156188964844,
+    a_lead=0.4783128798007965, radar=True, model_prob=0.9989967942237854, y_rel=-0.2,
+  )
+
+  cap = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert cap is None
+
+
 def test_low_speed_follow_catchup_uses_raw_vehicle_speed_when_cluster_runs_high():
   v_ego = 7.8
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -2346,6 +2366,46 @@ def test_route_8bc6_post_departure_cruise_cap_skips_accelerating_away_radar_lead
   assert cap is None
 
 
+def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_radar_follow():
+  v_ego = 24.108949661254883
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(
+    status=True, d_rel=36.75, v_lead=24.234233856201172,
+    a_lead=0.3050585687160492, radar=True, model_prob=0.9995405077934265, y_rel=-0.2,
+  )
+
+  cap = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.160530924797058,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert cap is None
+
+
+def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelerates_away():
+  v_ego = 22.293441772460938
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(
+    status=True, d_rel=36.5, v_lead=22.264942169189453,
+    a_lead=0.2780209183692932, radar=True, model_prob=0.999357283115387, y_rel=0.35,
+  )
+
+  cap = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.2014657258987427,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert cap is None
+
+
 def test_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again():
   v_ego = 19.03
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)