diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b3a99eb6c..249bd1959 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -210,6 +210,11 @@ LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15 +POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED = 12.0 +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 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 @@ -1240,6 +1245,28 @@ class LongitudinalPlanner: brake_floor = -hold_brake return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor) + def is_stable_post_departure_pullaway(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 < POST_DEPARTURE_FOLLOW_BYPASS_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 < POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA or + lead_accel < POST_DEPARTURE_FOLLOW_BYPASS_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 >= POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN + def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow): if lead is None or not lead.status: return None @@ -1272,6 +1299,9 @@ class LongitudinalPlanner: if gap_error > gap_buffer: return None + if not low_speed_follow_window and self.is_stable_post_departure_pullaway(lead, v_ego, t_follow): + return None + # If the lead is already pace-matched or pulling away, keep any catch-up # accel very small while we're near the follow target so we don't surge into # the lead and immediately ask for brake again. @@ -1361,9 +1391,11 @@ class LongitudinalPlanner: # momentarily falls near the pull-away threshold. That produces the repeated # 0.18 m/s^2 "surge / give up / surge" behavior seen in real logs. lead_accel = float(getattr(lead, "aLeadK", 0.0)) - if (lead_delta >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA and - lead_accel >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN and - gap_error >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN): + if self.is_stable_post_departure_pullaway(lead, v_ego, t_follow) or ( + lead_delta >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA and + lead_accel >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN and + gap_error >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN + ): return None base_cap = float(np.interp( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 022972cbf..7b2af9516 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1827,6 +1827,19 @@ def test_low_speed_follow_catchup_accel_cap_limits_close_vision_catchup(): assert 0.15 <= cap <= 0.45 +def test_route_8bc6_post_departure_catchup_cap_skips_accelerating_away_radar_lead(): + v_ego = 19.03 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, + ) + + cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) + + 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) @@ -2225,6 +2238,46 @@ def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead(): assert cap is None +def test_route_8bc6_post_departure_cruise_cap_skips_accelerating_away_radar_lead(): + v_ego = 19.03 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, + ) + + cap = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.45, + 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) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=33.85, v_lead=18.90, a_lead=-0.35, radar=True, model_prob=1.0, y_rel=-0.30, + ) + + catchup_cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) + cruise_cap = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=True, + ) + + assert catchup_cap is not None + assert cruise_cap is not None + + def test_cruise_tracking_lead_accel_transition_target_damps_mid_speed_reacquisition_burst(): v_ego = 16.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)