From 6046a84e9521546ddbe967922fc22043db671a88 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 11 Jul 2026 21:38:32 -0500 Subject: [PATCH] rare as trees --- .../controls/lib/longitudinal_planner.py | 68 +++++++++++++++++++ .../tests/test_longitudinal_planner.py | 46 +++++++++++++ 2 files changed, 114 insertions(+) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cc009f1f8..2865d1317 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -108,6 +108,17 @@ LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED = 1.2 LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA = 0.8 LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL = 0.5 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 +INSIDE_GAP_CLOSING_MIN_EGO_SPEED = 8.0 +INSIDE_GAP_CLOSING_MIN_LEAD_SPEED = 5.0 +INSIDE_GAP_CLOSING_MIN_SPEED = 0.5 +INSIDE_GAP_CLOSING_FULL_SPEED = 2.5 +INSIDE_GAP_CLOSING_MIN_DEFICIT = 3.0 +INSIDE_GAP_CLOSING_DEFICIT_RATIO = 0.15 +INSIDE_GAP_CLOSING_BRAKE_DEFICIT_RATIO = 0.25 +INSIDE_GAP_CLOSING_BRAKE_MIN_SPEED = 1.0 +INSIDE_GAP_CLOSING_MAX_DECEL = 0.65 +INSIDE_GAP_CLOSING_MAX_LATERAL_OFFSET = 1.75 +INSIDE_GAP_CLOSING_VISION_MIN_MODEL_PROB = 0.95 VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0 VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5 VISION_LEAD_APPROACH_FULL_TIME = 1.0 @@ -692,6 +703,50 @@ class LongitudinalPlanner: return max(accel_min, -required_decel) + @staticmethod + def get_inside_gap_closing_lead_accel_cap(lead, v_ego, accel_min, t_follow): + if lead is None or not lead.status: + return None + + ego_speed = float(v_ego) + lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) + if ego_speed < INSIDE_GAP_CLOSING_MIN_EGO_SPEED or lead_speed < INSIDE_GAP_CLOSING_MIN_LEAD_SPEED: + return None + if abs(float(getattr(lead, "yRel", 0.0))) > INSIDE_GAP_CLOSING_MAX_LATERAL_OFFSET: + return None + + 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 < INSIDE_GAP_CLOSING_VISION_MIN_MODEL_PROB: + return None + + closing_speed = ego_speed - lead_speed + if closing_speed < INSIDE_GAP_CLOSING_MIN_SPEED: + return None + + desired_gap = float(desired_follow_distance(ego_speed, lead_speed, float(t_follow))) + gap_deficit = desired_gap - float(lead.dRel) + trigger_deficit = max(INSIDE_GAP_CLOSING_MIN_DEFICIT, + INSIDE_GAP_CLOSING_DEFICIT_RATIO * desired_gap) + if gap_deficit <= trigger_deficit: + return None + + brake_deficit = INSIDE_GAP_CLOSING_BRAKE_DEFICIT_RATIO * desired_gap + deficit_factor = float(np.clip( + (gap_deficit - brake_deficit) / max(brake_deficit, 1.0), + 0.0, + 1.0, + )) + closing_factor = float(np.clip( + (closing_speed - INSIDE_GAP_CLOSING_BRAKE_MIN_SPEED) / + (INSIDE_GAP_CLOSING_FULL_SPEED - INSIDE_GAP_CLOSING_BRAKE_MIN_SPEED), + 0.0, + 1.0, + )) + required_decel = 0.45 * deficit_factor + 0.20 * closing_factor + required_decel = min(required_decel, INSIDE_GAP_CLOSING_MAX_DECEL) + return max(float(accel_min), -required_decel) + def get_vision_lead_approach_cap(self, lead, v_ego, accel_min, t_follow): if lead is None or not lead.status or bool(getattr(lead, "radar", False)): return None @@ -3184,6 +3239,19 @@ class LongitudinalPlanner: output_a_target = max(output_a_target, MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL) output_should_stop = False + inside_gap_closing_cap = None + if lead_control_active: + inside_gap_closing_lead = self.lead_two if self.mpc.source == 'lead1' else self.lead_one + inside_gap_closing_cap = self.get_inside_gap_closing_lead_accel_cap( + inside_gap_closing_lead, + scene_v_ego, + output_accel_min, + sm['starpilotPlan'].tFollow, + ) + if inside_gap_closing_cap is not None: + self.a_desired = min(self.a_desired, inside_gap_closing_cap) + output_a_target = min(output_a_target, inside_gap_closing_cap) + self.output_a_target = output_a_target self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 2f13032e0..1b9ce75cf 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2524,6 +2524,52 @@ def test_cruise_tracking_lead_accel_cap_skips_spacious_tracking_only_follow(): assert cap is None +def test_inside_gap_closing_lead_cap_blocks_route_accel_burst(): + v_ego = 17.1 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=26.3, v_lead=16.5, a_lead=0.0, radar=True, model_prob=1.0) + + cap = planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25) + + assert cap is not None + assert cap == pytest.approx(0.0) + + +def test_inside_gap_closing_lead_cap_strengthens_with_route_closure(): + v_ego = 19.8 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=23.3, v_lead=17.3, a_lead=0.0, radar=True, model_prob=1.0) + + cap = planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25) + + assert cap is not None + assert -0.7 <= cap <= -0.5 + + +@pytest.mark.parametrize("lead", [ + make_lead(status=True, d_rel=36.0, v_lead=16.5, radar=True, model_prob=1.0), + make_lead(status=True, d_rel=26.3, v_lead=17.8, radar=True, model_prob=1.0), + make_lead(status=True, d_rel=26.3, v_lead=16.5, radar=False, model_prob=0.8), + make_lead(status=True, d_rel=26.3, v_lead=16.5, radar=True, model_prob=1.0, y_rel=2.0), +]) +def test_inside_gap_closing_lead_cap_ignores_normal_or_ambiguous_follow(lead): + v_ego = 17.1 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + + assert planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25) is None + + +def test_inside_gap_closing_lead_cap_does_not_touch_standstill_departure(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + lead = make_lead(status=True, d_rel=5.0, v_lead=1.0, a_lead=0.5, radar=True, model_prob=1.0) + + assert planner.get_inside_gap_closing_lead_accel_cap(lead, 0.0, -1.0, 1.25) 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)