From 43a5c09412e5fef570afc9357141f6e1516e44d1 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 27 Apr 2026 11:49:39 -0500 Subject: [PATCH] Dom's Plan V3 --- .../controls/lib/longitudinal_planner.py | 53 ++++++++++++- .../test_conditional_experimental_mode.py | 16 ++++ .../tests/test_longitudinal_planner.py | 74 +++++++++++++++++++ .../lib/conditional_experimental_mode.py | 19 ++++- 4 files changed, 156 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 444d45a4dc..1cf2f9909c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -39,6 +39,17 @@ VISION_LEAD_APPROACH_MAX_DECEL = 0.45 VISION_LEAD_APPROACH_MIN_DECEL = 0.12 VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85 VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 +LEAD_APPROACH_TFOLLOW_TRIGGER_TIME = 4.5 +LEAD_APPROACH_TFOLLOW_FULL_TIME = 1.5 +LEAD_APPROACH_TFOLLOW_MAX_DELTA = 0.18 +LEAD_APPROACH_TFOLLOW_MAX_CLOSING_SPEED = 6.0 +LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE = 2.5 +LEAD_APPROACH_TFOLLOW_MIN_CLOSING_SPEED = 0.75 +LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE = 0.2 +LEAD_APPROACH_TFOLLOW_WINDOW_MIN = 6.0 +LEAD_APPROACH_TFOLLOW_WINDOW_GAIN = 0.35 +LEAD_APPROACH_TFOLLOW_RATE_UP = 1.0 +LEAD_APPROACH_TFOLLOW_RATE_DOWN = 0.18 # Uncertainty-based filter disable thresholds UNCERT_SLOPE_TRIG = 0.12 # per second @@ -188,6 +199,7 @@ class LongitudinalPlanner: # Uncertainty slope tracking self._uncert_last = 0.0 self._uncert_last_t = None + self.effective_t_follow = None @property def mlsim(self): @@ -292,6 +304,40 @@ class LongitudinalPlanner: return max(accel_min, -approach_decel) + def get_dynamic_t_follow(self, base_t_follow, lead, v_ego): + base_t_follow = float(base_t_follow) + target_t_follow = base_t_follow + + if lead is not None and lead.status: + lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) + if bool(getattr(lead, "radar", False)) or lead_prob >= VISION_LEAD_APPROACH_MIN_MODEL_PROB: + lead_brake = max(0.0, -float(lead.aLeadK)) + closing_speed = max(0.0, v_ego - lead.vLead) + if closing_speed >= LEAD_APPROACH_TFOLLOW_MIN_CLOSING_SPEED or lead_brake >= LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE: + desired_gap = float(desired_follow_distance(v_ego, lead.vLead, base_t_follow)) + approach_window = max(LEAD_APPROACH_TFOLLOW_WINDOW_MIN, LEAD_APPROACH_TFOLLOW_WINDOW_GAIN * float(v_ego)) + if float(lead.dRel) <= desired_gap + approach_window: + reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) + projected_closing_speed = closing_speed + 0.5 * lead_brake * reaction_t + gap_to_follow = max(float(lead.dRel) - desired_gap, 0.0) + time_to_follow = gap_to_follow / max(projected_closing_speed, 0.1) + time_factor = float(np.clip((LEAD_APPROACH_TFOLLOW_TRIGGER_TIME - time_to_follow) / + (LEAD_APPROACH_TFOLLOW_TRIGGER_TIME - LEAD_APPROACH_TFOLLOW_FULL_TIME), 0.0, 1.0)) + closing_factor = float(np.clip(closing_speed / LEAD_APPROACH_TFOLLOW_MAX_CLOSING_SPEED, 0.0, 1.0)) + brake_factor = float(np.clip(lead_brake / LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE, 0.0, 1.0)) + target_delta = LEAD_APPROACH_TFOLLOW_MAX_DELTA * np.clip( + 0.55 * time_factor + 0.25 * closing_factor + 0.20 * brake_factor, 0.0, 1.0) + target_t_follow = base_t_follow + float(target_delta) + + if self.effective_t_follow is None: + self.effective_t_follow = base_t_follow + + rate = LEAD_APPROACH_TFOLLOW_RATE_UP if target_t_follow > self.effective_t_follow else LEAD_APPROACH_TFOLLOW_RATE_DOWN + step = rate * self.dt + self.effective_t_follow = float(np.clip(target_t_follow, self.effective_t_follow - step, self.effective_t_follow + step)) + self.effective_t_follow = max(base_t_follow, self.effective_t_follow) + return self.effective_t_follow + @staticmethod def raw_close_lead_needs_control(lead, v_ego): if lead is None or not lead.status: @@ -387,6 +433,7 @@ class LongitudinalPlanner: # safety path so ACC/chill does not ignore a visible lead during that debounce. lead_control_active = tracking_lead or raw_close_lead_control lead_one_active = bool(self.lead_one.status and lead_control_active) + effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego) lead_dist = self.lead_one.dRel if lead_one_active else 50.0 @@ -504,7 +551,7 @@ class LongitudinalPlanner: if not self.mlsim: self.mpc.mode = dec_mpc_mode self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, - sm['starpilotPlan'].dangerFactor, sm['starpilotPlan'].tFollow, + sm['starpilotPlan'].dangerFactor, effective_t_follow, personality=personality, tracking_lead=lead_control_active) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -534,7 +581,7 @@ class LongitudinalPlanner: if lead_one_active: rel_v = max(0.0, v_ego - self.lead_one.vLead) # dynamic time headway adds a small buffer when uncertainty is elevated - base_th = 1.6 + base_th = max(1.6, effective_t_follow) th = base_th + 0.6 * max(0.0, uncertainty - 0.42) desired_gap = th * v_ego if (self.lead_dist_f is not None and self.lead_dist_f < desired_gap and rel_v > 0.5): @@ -581,7 +628,7 @@ class LongitudinalPlanner: cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min) if cap is not None: close_lead_caps.append(cap) - approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, sm['starpilotPlan'].tFollow) + approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, effective_t_follow) if approach_cap is not None: close_lead_caps.append(approach_cap) if close_lead_caps: diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 35e415a72a..cb08748904 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -86,6 +86,22 @@ def test_far_visible_lead_does_not_block_stop_light(): assert cem.stop_light_detected +def test_stop_light_stays_latched_until_untracked_stopped_lead_handoff(): + v_ego = 45 * CV.MPH_TO_MS + model_length = v_ego * 4.0 + + cem = make_cem(model_length=model_length) + run_stop_light_detector(cem, v_ego, steps=30) + assert cem.stop_light_detected + + cem.starpilot_planner.lead_one.status = True + cem.starpilot_planner.lead_one.dRel = model_length - 5.0 + cem.starpilot_planner.lead_one.vLead = 0.5 + run_stop_light_detector(cem, v_ego, steps=10, tracking_lead=False) + + assert cem.stop_light_detected + + class DummyThemeManager: def update_wheel_image(self, *args, **kwargs): pass diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 62f111c3ae..70354e067b 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -225,6 +225,80 @@ def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap(): assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None +@pytest.mark.parametrize("model_version", ["v11", "v12"]) +def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version): + v_ego = 21.535 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-3.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984), + ) + sm["starpilotPlan"].vCruise = v_ego + 8.0 + + for _ in range(8): + planner.update(sm, make_toggles(model_version)) + + assert planner.effective_t_follow is not None + assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.05 + assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.2 + + +@pytest.mark.parametrize("model_version", ["v11", "v12"]) +def test_dynamic_t_follow_stays_near_base_for_far_highway_lead(model_version): + v_ego = 29.26 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9), + ) + sm["starpilotPlan"].vCruise = v_ego + 3.0 + + for _ in range(12): + planner.update(sm, make_toggles(model_version)) + + assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) + + +@pytest.mark.parametrize("model_version", ["v11", "v12"]) +def test_dynamic_t_follow_releases_toward_base_after_lead_opens(model_version): + v_ego = 21.535 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-3.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984), + ) + + for _ in range(8): + planner.update(sm, make_toggles(model_version)) + + boosted_t_follow = planner.effective_t_follow + sm["radarState"].leadOne = make_lead(status=True, d_rel=66.168, v_lead=20.751, a_lead=0.261, radar=False, model_prob=0.975) + for _ in range(12): + planner.update(sm, make_toggles(model_version)) + + assert boosted_t_follow is not None + assert planner.effective_t_follow < boosted_t_follow + assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) + + @pytest.mark.parametrize("model_version", ["v11", "v12"]) def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_version): approach_v_ego = 21.535 diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 9a57143192..e6a265ee45 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -44,6 +44,7 @@ class ConditionalExperimentalMode: STOP_LIGHT_ON_MARGIN = 2.5 STOP_LIGHT_OFF_MARGIN = 4.0 STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0 + STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0 # ===== END TUNING PARAMETERS ===== @@ -226,11 +227,23 @@ class ConditionalExperimentalMode: # present; far/stale leads should not suppress true stop-light detection. lead = getattr(self.starpilot_planner, "lead_one", None) lead_distance = float(getattr(lead, "dRel", float("inf"))) + lead_speed = float(getattr(lead, "vLead", float("inf"))) lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN - self.lead_clear_filter.update(not lead_relevant) - lead_cleared = self.lead_clear_filter.x >= THRESHOLD + handoff_to_stopped_lead = ( + self.stop_light_detected and + lead_relevant and + not self.starpilot_planner.tracking_lead and + lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED + ) + if handoff_to_stopped_lead: + lead_cleared = True + else: + self.lead_clear_filter.update(not lead_relevant) + lead_cleared = self.lead_clear_filter.x >= THRESHOLD self.stop_light_filter.update(model_stopping and lead_cleared) - self.stop_light_detected = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) + self.stop_light_detected = bool( + (self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) or handoff_to_stopped_lead + ) else: self.stop_light_filter.x = 0 self.stop_light_detected = False