From d7ec15bddbfa35ec33571d952180e206b71db34c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 4 May 2026 09:55:38 -0500 Subject: [PATCH] planner --- .../controls/lib/longitudinal_planner.py | 39 ++++++++++--- .../tests/test_longitudinal_planner.py | 57 +++++++++++++++++++ 2 files changed, 89 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 02103e3fd..9bc5135c0 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -54,8 +54,14 @@ VISION_UNTRACKED_SLOW_LEAD_FULL_TTC = 8.0 VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.4 VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE = 80.0 VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE = 120.0 +VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_DISTANCE_TIME = 5.7 VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL = 0.85 VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL = 0.1 +VISION_UNTRACKED_SLOW_LEAD_RELAXED_MODEL_PROB = 0.68 +VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED = 8.0 +VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC = 10.0 +VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED = 10.0 +VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED = 16.0 VISION_SLOW_LEAD_MAX_SPEED = 5.0 VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5 VISION_SLOW_LEAD_TRIGGER_TTC = 4.5 @@ -363,8 +369,6 @@ class LongitudinalPlanner: return None lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB: - return None lead_brake = max(0.0, -float(lead.aLeadK)) reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) @@ -377,21 +381,42 @@ class LongitudinalPlanner: if closing_ratio < VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO: return None - max_distance = float(np.clip(VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME * v_ego, + projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) + if projected_ttc > VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC: + return None + + min_model_prob = VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB + max_distance_time = VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME + if float(lead.vLead) <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED and \ + projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC: + closing_relax = float(np.clip((projected_closing_speed - VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED) / + (VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED - + VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED), 0.0, 1.0)) + ttc_relax = float(np.clip((VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC - projected_ttc) / + (VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC - + VISION_UNTRACKED_SLOW_LEAD_FULL_TTC), 0.0, 1.0)) + relax_factor = closing_relax * ttc_relax + min_model_prob = float(np.interp(relax_factor, [0.0, 1.0], + [VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB, + VISION_UNTRACKED_SLOW_LEAD_RELAXED_MODEL_PROB])) + max_distance_time = float(np.interp(relax_factor, [0.0, 1.0], + [VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME, + VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_DISTANCE_TIME])) + + max_distance = float(np.clip(max_distance_time * v_ego, VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE, VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE)) if float(lead.dRel) > max_distance: return None - projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) - if projected_ttc > VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC: + if lead_prob < min_model_prob: return None time_factor = float(np.clip((VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - projected_ttc) / (VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - VISION_UNTRACKED_SLOW_LEAD_FULL_TTC), 0.0, 1.0)) - prob_factor = float(np.clip((lead_prob - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB) / - (VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB), + prob_factor = float(np.clip((lead_prob - min_model_prob) / + (VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - min_model_prob), 0.0, 1.0)) closing_factor = float(np.clip((closing_ratio - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO) / (VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO), diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 1f196580e..5166a81cd 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -268,6 +268,29 @@ def test_vision_untracked_slow_lead_cap_reaches_high_confidence_far_slower_lead_ assert not planner.raw_close_lead_needs_control(route_like_lead, v_ego) +def test_vision_untracked_slow_lead_cap_relaxes_confidence_for_near_stopped_high_closure_lead(): + v_ego = 20.35 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + route_like_lead = make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70) + + route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) + + assert route_cap is not None + assert route_cap < -0.55 + + +def test_vision_untracked_slow_lead_cap_keeps_low_confidence_floor_for_less_threatening_lead(): + v_ego = 20.35 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + less_threatening_lead = make_lead(status=True, d_rel=115.4, v_lead=9.5, a_lead=0.0, radar=False, model_prob=0.75) + + assert planner.get_vision_untracked_slow_lead_cap(less_threatening_lead, v_ego, -1.0) is None + + def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop(): v_ego = 13.207 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -477,6 +500,40 @@ def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_track assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confidence_is_midrange(model_version): + v_ego = 20.35 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) + planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) + sm_no_lead = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + ) + sm_with_lead = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70), + ) + sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 + sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 + + for _ in range(8): + planner_no_lead.update(sm_no_lead, make_toggles(model_version)) + planner_with_lead.update(sm_with_lead, make_toggles(model_version)) + + assert planner_with_lead.mode == "acc" + assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.12 + assert planner_with_lead.output_a_target < -0.45 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version): v_ego = 28.7