From a71be7c47b0cb486675ec587c0ddf90acd358e3f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 17 Jun 2026 11:24:45 -0500 Subject: [PATCH] long plan --- .../controls/lib/longitudinal_planner.py | 42 ++++++++++++- .../tests/test_longitudinal_planner.py | 60 +++++++++++++++++++ 2 files changed, 100 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 06e92ccc8..6ded61183 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index a1c636e16..1b033bb9f 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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)