From 9ccd61e8cfc05e8aa1da62ad73def4eee79fbc99 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 17 Jul 2026 00:17:41 -0500 Subject: [PATCH] Mizuzu --- selfdrive/controls/lib/lead_behavior.py | 23 ++++++++++- .../lib/longitudinal_mpc_lib/long_mpc.py | 5 ++- .../controls/lib/longitudinal_planner.py | 17 +++++++- .../controls/tests/test_lead_behavior.py | 14 +++++++ .../tests/test_longitudinal_planner.py | 41 +++++++++++++++++++ 5 files changed, 95 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index a665240f2..7068a10e0 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -12,6 +12,11 @@ VISION_LEAD_TRACK_CLOSING_CAP = 2.50 VISION_LEAD_TRACK_EXIT_TIME_GAP = 2.30 VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET = 1.6 VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB = 0.70 +VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB = 0.95 +VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET = 1.1 +VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN = 0.55 +VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED = 20.0 +VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED = 25.0 TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.40 TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.70 TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75 @@ -65,7 +70,23 @@ def should_hold_tracked_vision_lead(lead_status: bool, lead_distance: float, mod model_limit = float(model_length) + tracking_buffer vision_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE, float(v_ego) * VISION_LEAD_TRACK_EXIT_TIME_GAP + tracking_buffer) - return float(lead_distance) < min(model_limit, vision_exit_limit) + if float(lead_distance) < min(model_limit, vision_exit_limit): + return True + + path_relative_offset = abs(float(y_rel) + float(path_y)) + if (float(model_prob) < VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB or + path_relative_offset > VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET): + return False + + speed_factor = 1.0 - _smoothstep( + v_ego, + VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED, + VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED, + ) + continuity_time_gap = VISION_LEAD_TRACK_EXIT_TIME_GAP + VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN * speed_factor + continuity_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE, + float(v_ego) * continuity_time_gap + tracking_buffer) + return float(lead_distance) < continuity_exit_limit def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lead: float, t_follow: float, *, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 001e85ccd..faf0f064b 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -656,7 +656,7 @@ class LongitudinalMpc: return track_one >= 0 and track_one == track_two @staticmethod - def leads_are_near_duplicates(lead_one, lead_two, v_ego): + def leads_are_near_duplicates(lead_one, lead_two, v_ego, *, vision_min_speed=None): if lead_one is None or lead_two is None or not lead_one.status or not lead_two.status: return False if LongitudinalMpc.leads_share_identical_radar_track(lead_one, lead_two): @@ -666,7 +666,8 @@ class LongitudinalMpc: abs(float(lead_one.dRel) - float(lead_two.dRel)) <= NEAR_DUPLICATE_LEAD_SOURCE_MAX_DREL_DIFF and abs(float(lead_one.vRel) - float(lead_two.vRel)) <= max(1.0, NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF) ) - if float(v_ego) < NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED: + min_vision_speed = NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED if vision_min_speed is None else float(vision_min_speed) + if float(v_ego) < min_vision_speed: return False lead_one_radar = bool(getattr(lead_one, "radar", False)) lead_two_radar = bool(getattr(lead_two, "radar", False)) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index da174189a..e7aff6005 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -402,6 +402,7 @@ NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET = 0.85 NEAR_DUPLICATE_VISION_TRANSITION_MIN_HEADWAY_MARGIN = 0.55 NEAR_DUPLICATE_VISION_TRANSITION_EXTRA_CLOSING_SPEED = 1.25 LOW_SPEED_IDENTICAL_RADAR_DUPLICATE_TRANSITION_EXTRA_HEADWAY = 0.15 +LOW_SPEED_DUPLICATE_VISION_TRANSITION_EXTRA_HEADWAY = 0.75 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35 NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22 NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP = 0.32 @@ -1888,7 +1889,12 @@ class LongitudinalPlanner: self.duplicate_vision_comfort_lead_source = None return None - if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego): + if not self.mpc.leads_are_near_duplicates( + self.lead_one, + self.lead_two, + v_ego, + vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED, + ): self.duplicate_vision_comfort_lead_source = None return None @@ -2106,7 +2112,12 @@ class LongitudinalPlanner: if not (self.lead_one.status and self.lead_two.status): return None identical_radar_duplicates = self.mpc.leads_share_identical_radar_track(self.lead_one, self.lead_two) - if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego): + if not self.mpc.leads_are_near_duplicates( + self.lead_one, + self.lead_two, + v_ego, + vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED, + ): return None low_speed_extension_active = bool( tracking_lead_active and @@ -2135,6 +2146,8 @@ class LongitudinalPlanner: max_headway_above_target = NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET if low_speed_extension_active and identical_radar_duplicates: max_headway_above_target += LOW_SPEED_IDENTICAL_RADAR_DUPLICATE_TRANSITION_EXTRA_HEADWAY + elif low_speed_extension_active and not lead_radar: + max_headway_above_target += LOW_SPEED_DUPLICATE_VISION_TRANSITION_EXTRA_HEADWAY if actual_headway > float(base_t_follow) + max_headway_above_target: return None diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index 7ef737060..2df853e93 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -158,6 +158,20 @@ def test_should_hold_tracked_vision_lead_releases_beyond_exit_gap(): ) +def test_should_hold_tracked_vision_lead_ignores_shortened_model_horizon_in_bolt_stutter_case(): + assert should_hold_tracked_vision_lead( + True, 54.6, 40.0, 6.0, 19.4, + model_prob=1.0, y_rel=0.05, radar=False, + ) + + +def test_should_hold_tracked_vision_lead_does_not_extend_low_confidence_short_horizon_case(): + assert not should_hold_tracked_vision_lead( + True, 54.6, 40.0, 6.0, 19.4, + model_prob=0.90, y_rel=0.05, radar=False, + ) + + def test_radarless_matched_follow_window_accepts_pace_matched_highway_follow(): assert is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 2f3c1dca7..82ab76ad5 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -3658,6 +3658,22 @@ def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_ assert planner.duplicate_vision_comfort_lead_source == "lead0" +def test_duplicate_vision_comfort_lead_supports_mid_speed_follow_churn(): + v_ego = 16.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=30.0, v_lead=15.8, a_lead=0.02, radar=False, model_prob=1.0, y_rel=0.05) + lead_two = make_lead(status=True, d_rel=30.1, v_lead=15.82, a_lead=0.01, radar=False, model_prob=1.0, y_rel=0.08) + lead_one.vRel = lead_one.vLead - v_ego + lead_two.vRel = lead_two.vLead - v_ego + planner.lead_one = lead_one + planner.lead_two = lead_two + + selected = planner.get_duplicate_vision_comfort_lead(v_ego) + + assert selected is lead_one + + def test_duplicate_vision_comfort_lead_latches_source_until_duplicate_cluster_resolves(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -3777,6 +3793,31 @@ def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_h assert smoothed == pytest.approx(0.57, abs=1e-6) +def test_near_duplicate_lead_transition_target_damps_mid_speed_duplicate_vision_handoff(): + v_ego = 17.6 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=44.8, v_lead=16.8, a_lead=0.0, radar=False, model_prob=1.0) + lead_two = make_lead(status=True, d_rel=44.9, v_lead=16.82, a_lead=0.0, radar=False, model_prob=1.0) + lead_one.vRel = lead_one.vLead - v_ego + lead_two.vRel = lead_two.vLead - v_ego + planner.lead_one = lead_one + planner.lead_two = lead_two + + smoothed = planner.get_near_duplicate_lead_transition_target( + lead_one, + v_ego, + 1.25, + prev_output_a_target=0.49, + output_a_target=0.03, + current_source="cruise", + tracking_lead_active=True, + ) + + assert smoothed is not None + assert smoothed == pytest.approx(0.17, abs=1e-6) + + def test_near_duplicate_lead_transition_target_damps_generous_headway_duplicate_vision_sign_flip(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)