diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e77c99291..6b4f78dc1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -373,6 +373,8 @@ NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED = 3.5 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC = 8.0 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET = 0.45 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 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35 NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22 @@ -2020,13 +2022,6 @@ class LongitudinalPlanner: relative_speed = float(v_ego) - float(lead.vLead) closing_speed = max(0.0, relative_speed) - if closing_speed > NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED: - return None - - ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - if ttc < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC: - return None - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) if actual_headway < max(0.0, float(base_t_follow) - NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET): return None @@ -2036,6 +2031,20 @@ class LongitudinalPlanner: if actual_headway > float(base_t_follow) + max_headway_above_target: return None + max_closing_speed = NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED + if ( + not identical_radar_duplicates and + not lead_radar and + actual_headway >= float(base_t_follow) + NEAR_DUPLICATE_VISION_TRANSITION_MIN_HEADWAY_MARGIN + ): + max_closing_speed += NEAR_DUPLICATE_VISION_TRANSITION_EXTRA_CLOSING_SPEED + if closing_speed > max_closing_speed: + return None + + ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") + if ttc < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC: + return None + target_delta = float(output_a_target) - float(prev_output_a_target) if abs(target_delta) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A: return None diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index f884b3a85..9c766f677 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -3231,6 +3231,55 @@ 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_generous_headway_duplicate_vision_sign_flip(): + v_ego = 25.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=54.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99) + lead_two = make_lead(status=True, d_rel=54.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99) + 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_two, + v_ego, + 1.45, + prev_output_a_target=0.70, + output_a_target=-0.61, + current_source="lead1", + tracking_lead_active=True, + ) + + assert smoothed is not None + assert smoothed == pytest.approx(0.52, abs=1e-6) + + +def test_near_duplicate_lead_transition_target_skips_fast_duplicate_vision_sign_flip_when_headway_is_not_generous(): + v_ego = 25.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=43.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99) + lead_two = make_lead(status=True, d_rel=43.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99) + 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_two, + v_ego, + 1.45, + prev_output_a_target=0.70, + output_a_target=-0.61, + current_source="lead1", + tracking_lead_active=True, + ) + + assert smoothed is None + + def test_duplicate_slow_lead_brake_hold_prevents_zero_cross_from_duplicate_voacc_leads(): v_ego = 24.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)