diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index b97b2a310f..21146ede47 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -3,20 +3,22 @@ from openpilot.common.constants import CV HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED = 45. * CV.MPH_TO_MS +TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED = 52. * CV.MPH_TO_MS +TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL = 1.5 VISION_LEAD_TRACK_MIN_DISTANCE = 25.0 VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75 VISION_LEAD_TRACK_CLOSING_GAIN = 0.20 VISION_LEAD_TRACK_CLOSING_CAP = 2.50 -TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.35 -TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.65 +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 TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN = 1.05 TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START = 2.75 TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END = 3.10 TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET = 0.90 TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET = 1.60 -TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.65 -TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.75 +TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.45 +TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.55 RADARLESS_MATCHED_FOLLOW_MIN_SPEED = 22.0 RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED = 2.0 RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY = 0.95 @@ -26,6 +28,11 @@ RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE = 0.35 RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB = 0.70 +def _smoothstep(value: float, start: float, end: float) -> float: + factor = min(1.0, max(0.0, (float(value) - float(start)) / max(float(end) - float(start), 1e-3))) + return factor * factor * (3.0 - 2.0 * factor) + + def should_track_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float, v_ego: float, *, v_lead: float | None = None, radar: bool = False) -> bool: if not lead_status: @@ -72,44 +79,42 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga desired_hw = desired_gap / max(v_ego, 1e-3) headway_margin = actual_hw - desired_hw - if v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED: - return 0.0 - if v_cruise is not None and v_ego >= v_cruise: - return 0.0 if gap_error <= 0.0: return 0.0 + speed_factor = _smoothstep(v_ego, HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED, TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED) + cruise_factor = 1.0 + if v_cruise is not None: + cruise_factor = _smoothstep(v_cruise - v_ego, 0.0, TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL) + if speed_factor == 0.0 or cruise_factor == 0.0: + return 0.0 + # Encourage ACC to treat a tracked lead as the active constraint when we're # hanging far above the requested time gap, but don't override cruise for a # truly distant lead or one we're already closing on decisively. - if headway_margin <= TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN: - return 0.0 fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN, TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw) fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN, TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw) - if headway_margin >= fade_end_margin: - return 0.0 - if closing_speed > max(2.5, 0.12 * v_ego): - return 0.0 + entry_factor = _smoothstep(headway_margin, + TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, + TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN) + exit_factor = 1.0 - _smoothstep(headway_margin, fade_start_margin, fade_end_margin) - entry_factor = min(1.0, max(0.0, (headway_margin - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN) / - max(TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, 1e-3))) - exit_factor = 1.0 - if headway_margin > fade_start_margin: - exit_factor = min(1.0, max(0.0, (fade_end_margin - headway_margin) / max(fade_end_margin - fade_start_margin, 1e-3))) + closing_fade_end = max(2.5, 0.12 * v_ego) + closing_fade_start = max(1.75, 0.08 * v_ego) + closing_factor = 1.0 - _smoothstep(closing_speed, closing_fade_start, closing_fade_end) lateral_factor = 1.0 if y_rel is not None: lateral_offset = abs(float(y_rel)) - if lateral_offset >= TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET: - return 0.0 - if lateral_offset > TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET: - lateral_factor = min(1.0, max(0.0, (TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - lateral_offset) / - max(TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET, 1e-3))) + lateral_factor = 1.0 - _smoothstep(lateral_offset, + TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET, + TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET) - bias_cap = max(14.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego) - return min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * entry_factor * exit_factor * lateral_factor + bias_cap = max(10.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego) + return (min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * speed_factor * cruise_factor * + entry_factor * exit_factor * closing_factor * lateral_factor) def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float, diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index defcd6fe02..2135613fde 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -23,7 +23,7 @@ def test_tracked_lead_catchup_bias_ignores_very_far_gap(): def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap(): bias = get_tracked_lead_catchup_bias(30.4, 63.0, 40.0, 0.4) - assert bias > 14.0 + assert bias > 9.0 def test_tracked_lead_catchup_bias_reduces_for_laterally_offset_lead(): @@ -45,6 +45,24 @@ def test_tracked_lead_catchup_bias_stays_off_once_at_set_speed(): assert bias == 0.0 +def test_tracked_lead_catchup_bias_fades_smoothly_near_set_speed(): + below_set = get_tracked_lead_catchup_bias(27.70, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8) + at_set = get_tracked_lead_catchup_bias(27.78, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8) + assert 0.0 < below_set < 0.25 + assert at_set == 0.0 + + +def test_tracked_lead_catchup_bias_dials_back_camry_bookmark_case(): + bias = get_tracked_lead_catchup_bias(27.40, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8) + assert 0.0 < bias < 2.0 + + +def test_tracked_lead_catchup_bias_fades_smoothly_at_closing_limit(): + below_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.75) + above_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.77) + assert abs(below_limit - above_limit) < 0.1 + + def test_disable_far_lead_throttle_rejects_two_second_plus_gap(): should_disable = should_disable_far_lead_throttle(31.4, 78.7, 38.0, 0.1, False) assert not should_disable