From b47c18426db44ad81045a246ba0d72b30a8803af Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 19 Jun 2026 14:14:07 -0500 Subject: [PATCH] Steam Your Water --- .../controls/lib/longitudinal_planner.py | 51 ++++++++ .../tests/test_longitudinal_planner.py | 112 ++++++++++++++++++ 2 files changed, 163 insertions(+) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 08ca94d58e..b8d6ef488a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -219,6 +219,14 @@ 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 +POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME = 75.0 +POST_DEPARTURE_FOLLOW_SETTLE_MIN_SPEED = 8.0 +POST_DEPARTURE_FOLLOW_SETTLE_MIN_MODEL_PROB = 0.9 +POST_DEPARTURE_FOLLOW_SETTLE_MAX_LATERAL_OFFSET = 1.15 +POST_DEPARTURE_FOLLOW_SETTLE_MAX_CLOSING_SPEED = 0.8 +POST_DEPARTURE_FOLLOW_SETTLE_MAX_LEAD_BRAKE = 0.10 +POST_DEPARTURE_FOLLOW_SETTLE_MIN_HEADWAY_MARGIN = 0.10 +POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN = 0.05 COMFORTABLE_PULLAWAY_FOLLOW_MIN_MODEL_PROB = 0.95 COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_DELTA = -0.05 COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_ACCEL = 0.20 @@ -582,6 +590,7 @@ class LongitudinalPlanner: self.manual_stop_resume_override_until = 0.0 self.lead_depart_accel_hold_until = 0.0 self.spacious_follow_cap_bypass_until = 0.0 + self.post_departure_follow_settle_until = 0.0 if self.is_preap: try: @@ -1320,6 +1329,41 @@ class LongitudinalPlanner: headway_margin = actual_headway - float(t_follow) return headway_margin >= COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN + def post_departure_follow_settle_active(self, lead, v_ego, t_follow): + if lead is None or not lead.status: + return False + if time.monotonic() > self.post_departure_follow_settle_until: + self.post_departure_follow_settle_until = 0.0 + return False + if float(v_ego) < POST_DEPARTURE_FOLLOW_SETTLE_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_SETTLE_MIN_MODEL_PROB: + return False + + if abs(float(getattr(lead, "yRel", 0.0))) > POST_DEPARTURE_FOLLOW_SETTLE_MAX_LATERAL_OFFSET: + return False + + actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) + headway_margin = actual_headway - float(t_follow) + if headway_margin <= POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN: + self.post_departure_follow_settle_until = 0.0 + return False + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > POST_DEPARTURE_FOLLOW_SETTLE_MAX_LEAD_BRAKE: + self.post_departure_follow_settle_until = 0.0 + return False + + closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) + if closing_speed > POST_DEPARTURE_FOLLOW_SETTLE_MAX_CLOSING_SPEED: + self.post_departure_follow_settle_until = 0.0 + return False + + return headway_margin >= POST_DEPARTURE_FOLLOW_SETTLE_MIN_HEADWAY_MARGIN + def is_spacious_low_closure_follow(self, lead, v_ego, t_follow): if lead is None or not lead.status or float(v_ego) < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED: return False @@ -1356,6 +1400,9 @@ class LongitudinalPlanner: if lead is None or not lead.status: return None + if self.post_departure_follow_settle_active(lead, v_ego, t_follow): + return None + lead_radar = bool(getattr(lead, "radar", False)) lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) @@ -1457,6 +1504,8 @@ class LongitudinalPlanner: def get_cruise_tracking_lead_accel_cap(self, lead, v_ego, t_follow, current_source, tracking_lead_active): if lead is None or not lead.status or current_source != "cruise": return None + if self.post_departure_follow_settle_active(lead, v_ego, t_follow): + return None if not (CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED): return None @@ -2641,9 +2690,11 @@ class LongitudinalPlanner: vision_low_speed_stop_active = False output_should_stop = False output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) + self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME if lead_control_active and lead_depart_ready and not depart_safety_veto and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED: output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) + self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME if depart_safety_veto or output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)): self.lead_depart_accel_hold_until = 0.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index ca77f066e1..057644c67e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2487,6 +2487,118 @@ def test_route_8bc6_radar_matched_follow_catchup_cap_holds_small_cap_for_slower_ assert cap == pytest.approx(0.04, abs=1e-6) +def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_catchup_cap(): + v_ego = 16.4023914337 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=25.75, v_lead=15.9339208603, + a_lead=0.1164954603, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap_without_latch = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.25, + current_source="lead0", + tracking_lead_active=True, + ) + + planner.post_departure_follow_settle_until = time.monotonic() + 5.0 + cap_with_latch = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.25, + current_source="lead0", + tracking_lead_active=True, + ) + + assert cap_without_latch == pytest.approx(0.03, abs=1e-6) + assert cap_with_latch is None + + +def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_cruise_cap(): + v_ego = 19.2975330353 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=28.9, v_lead=19.6019687653, + a_lead=0.1241656989, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap_without_latch = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.25, + current_source="cruise", + tracking_lead_active=True, + ) + + planner.post_departure_follow_settle_until = time.monotonic() + 5.0 + cap_with_latch = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.25, + current_source="cruise", + tracking_lead_active=True, + ) + + assert cap_without_latch == pytest.approx(0.10831585854957321, abs=1e-6) + assert cap_with_latch is None + + +def test_post_departure_settle_latch_does_not_bypass_when_lead_brakes_again(): + v_ego = 19.192998192 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.post_departure_follow_settle_until = time.monotonic() + 5.0 + lead = make_lead( + status=True, d_rel=30.5, v_lead=19.147550216, + a_lead=-0.16, radar=True, model_prob=1.0, y_rel=0.2, + ) + + catchup_cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.25, + current_source="cruise", + tracking_lead_active=True, + ) + cruise_cap = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.25, + current_source="cruise", + tracking_lead_active=True, + ) + + assert catchup_cap is not None + assert cruise_cap is not None + assert planner.post_departure_follow_settle_until == 0.0 + + +def test_post_departure_settle_latch_clears_once_follow_has_settled_near_target(): + v_ego = 16.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.post_departure_follow_settle_until = time.monotonic() + 5.0 + lead = make_lead( + status=True, d_rel=20.48, v_lead=16.25, + a_lead=0.18, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.25, + current_source="lead0", + tracking_lead_active=True, + ) + + assert cap is not None + assert planner.post_departure_follow_settle_until == 0.0 + + 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)