From 98a913693d9d0d897bf32190eb7b71e8e2cedc42 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 7 Aug 2026 11:27:59 -0500 Subject: [PATCH] The Velvet Night --- selfdrive/controls/lib/lead_follow_policy.py | 208 +++ .../controls/lib/longitudinal_planner.py | 1391 ++------------- .../controls/tests/test_lead_follow_policy.py | 71 + .../tests/test_longitudinal_planner.py | 1513 ----------------- 4 files changed, 411 insertions(+), 2772 deletions(-) create mode 100644 selfdrive/controls/lib/lead_follow_policy.py create mode 100644 selfdrive/controls/tests/test_lead_follow_policy.py diff --git a/selfdrive/controls/lib/lead_follow_policy.py b/selfdrive/controls/lib/lead_follow_policy.py new file mode 100644 index 000000000..516c659c0 --- /dev/null +++ b/selfdrive/controls/lib/lead_follow_policy.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python3 +"""Small, deterministic policy for ordinary moving-lead following. + +Stop, departure, and model-target decisions remain in longitudinal_planner. +This policy only shapes the non-urgent follow output after MPC has selected a +lead. Keeping that boundary explicit prevents comfort smoothing from weakening +the raw lead safety path. +""" + +from dataclasses import dataclass + +import numpy as np + +from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance + + +FOLLOW_MIN_SPEED = 8.0 +FOLLOW_MATCHED_MIN_SPEED = 22.0 +FOLLOW_MAX_CLOSING = 3.5 +FOLLOW_MAX_LEAD_BRAKE = 0.35 +FOLLOW_GAP_BUFFER_MIN = 4.0 +FOLLOW_GAP_BUFFER_GAIN = 0.15 +FOLLOW_ACCEL_MAX = 0.55 +FOLLOW_TRANSITION_MIN_STEP = 0.06 +FOLLOW_TRANSITION_MAX_STEP = 0.18 +FOLLOW_TRANSITION_MIN_TTC = 6.0 +FOLLOW_HEADWAY_MARGIN = 0.90 +FOLLOW_SIGN_CROSS_STEP = 0.10 +FOLLOW_TRANSITION_MAX_BRAKE = 0.25 + + +@dataclass(frozen=True) +class FollowResult: + lead: object | None + accel_cap: float | None + brake_floor: float | None + target: float + + +def _lead_brake(lead) -> float: + return max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + + +def _lead_prob(lead) -> float: + return float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) + + +def _headway(lead, v_ego: float) -> float: + return float(lead.dRel) / max(float(v_ego), 1e-3) + + +def _matched(lead, v_ego: float, t_follow: float) -> bool: + if lead is None or not lead.status or float(v_ego) < FOLLOW_MATCHED_MIN_SPEED: + return False + + relative_speed = float(v_ego) - float(lead.vLead) + if not (-1.2 <= relative_speed <= 2.2) or _lead_brake(lead) > FOLLOW_MAX_LEAD_BRAKE: + return False + + radar = bool(getattr(lead, "radar", False)) + if not radar and not is_radarless_matched_follow_window( + v_ego, lead.dRel, lead.vLead, t_follow, + radar=False, lead_brake=_lead_brake(lead), lead_prob=_lead_prob(lead), + ): + return False + + headway = _headway(lead, v_ego) + return max(1.05, float(t_follow) - 0.35) <= headway <= float(t_follow) + 0.90 + + +def select_lead(lead_one, lead_two, source: str, active: bool, v_ego: float, t_follow: float): + """Return the lead already selected by MPC; never fuse or re-select tracks.""" + if not active: + return None + if source == "lead1": + return lead_two if lead_two is not None and lead_two.status else None + if source == "lead0": + return lead_one if lead_one is not None and lead_one.status else None + if lead_one is not None and lead_one.status: + return lead_one + if lead_two is not None and lead_two.status: + return lead_two + return None + + +def is_nonurgent_duplicate_vision_follow(lead_one, lead_two, v_ego: float, t_follow: float, mpc) -> bool: + """Keep MPC's duplicate-track filter only for clearly non-urgent vision pairs.""" + if (lead_one is None or lead_two is None or not lead_one.status or not lead_two.status or + bool(getattr(lead_one, "radar", False)) or bool(getattr(lead_two, "radar", False))): + return False + if not mpc.leads_are_near_duplicates(lead_one, lead_two, v_ego, vision_min_speed=10.0): + return False + + closing_speed = max(0.0, float(v_ego) - float(lead_one.vLead)) + ttc = float(lead_one.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") + headway = float(lead_one.dRel) / max(float(v_ego), 1e-3) + return ttc > 6.0 and headway > max(0.85, float(t_follow) - 0.45) + + +def _catchup_cap(lead, v_ego: float, t_follow: float, *, source: str, tracking: bool, + post_departure: bool) -> float | None: + if lead is None or not lead.status or post_departure: + return None + + radar = bool(getattr(lead, "radar", False)) + prob = _lead_prob(lead) + brake = _lead_brake(lead) + low_speed = not radar and v_ego <= 12.0 and prob >= 0.85 and brake <= 0.20 + if v_ego < FOLLOW_MIN_SPEED and not low_speed: + return None + + lead_delta = float(lead.vLead) - float(v_ego) + minimum_delta = -1.2 if low_speed else -0.5 + if lead_delta < minimum_delta: + return None + + gap_error = float(lead.dRel) - float(desired_follow_distance(v_ego, lead.vLead, t_follow)) + buffer = max(6.0 if low_speed else FOLLOW_GAP_BUFFER_MIN, + (0.35 if low_speed else FOLLOW_GAP_BUFFER_GAIN) * float(v_ego)) + if gap_error > buffer: + return None + + if radar and tracking and _matched(lead, v_ego, t_follow): + if gap_error > buffer - 0.75: + return None + if source == "cruise" and gap_error <= 0.75 and lead_delta < minimum_delta: + return 0.04 + + edge = np.interp( + lead_delta, + [-1.2, -0.5, 0.0, 1.0, 2.0] if low_speed else [-0.5, 0.0, 1.0], + [0.20, 0.20, 0.24, 0.38, 0.55] if low_speed else [0.16, 0.08, 0.02], + ) + near = min(float(edge), 0.16 if low_speed else 0.03) + gap_factor = float(np.clip(max(gap_error, 0.0) / max(buffer, 0.1), 0.0, 1.0)) + cap = float(np.interp(gap_factor, [0.0, 1.0], [near, float(edge)])) + allowance = float(np.clip(gap_error / 4.0, 0.0, 1.0)) + if v_ego >= 12.0: + allowance *= 0.55 * float(np.clip((0.35 - brake) / 0.35, 0.0, 1.0)) + cap += allowance * 0.55 + if not low_speed: + entry = float(np.clip((v_ego - 8.0) / 4.0, 0.0, 1.0)) + cap = float(np.interp(entry, [0.0, 1.0], [1.5, cap])) + return min(FOLLOW_ACCEL_MAX if low_speed else 1.5, cap) + + +def _matched_brake_floor(lead, v_ego: float, t_follow: float) -> float | None: + if lead is None or not _matched(lead, v_ego, t_follow): + return None + + relative_speed = float(v_ego) - float(lead.vLead) + decel = float(np.interp(relative_speed, [-1.2, 0.0, 2.2], [0.08, 0.12, 0.32])) + deficit = float(np.clip((float(t_follow) - _headway(lead, v_ego)) / 0.35, 0.0, 1.0)) + return -min(0.32, decel + 0.05 * deficit) + + +def _transition_target(lead, v_ego: float, t_follow: float, previous: float, target: float) -> float | None: + if lead is None or not lead.status: + return None + if abs(float(target) - float(previous)) < 0.06: + return None + if _lead_brake(lead) > 0.8: + return None + # The follow smoother may shape small MPC reversals, but it must never turn + # an already meaningful braking request into a coast request. + if float(target) < -FOLLOW_TRANSITION_MAX_BRAKE: + return None + + closing = max(0.0, float(v_ego) - float(lead.vLead)) + ttc = float(lead.dRel) / max(closing, 0.1) if closing > 0.1 else float("inf") + margin = _headway(lead, v_ego) - float(t_follow) + if closing > FOLLOW_MAX_CLOSING or ttc < FOLLOW_TRANSITION_MIN_TTC or margin > FOLLOW_HEADWAY_MARGIN: + return None + + opening = max(0.0, float(lead.vLead) - float(v_ego)) + up_step = float(np.interp(opening, [0.0, 1.0, 2.25], + [FOLLOW_TRANSITION_MIN_STEP, 0.12, FOLLOW_TRANSITION_MAX_STEP])) + down_step = float(np.interp(closing, [0.0, FOLLOW_MAX_CLOSING], [0.06, 0.18])) + if float(previous) * float(target) < 0.0: + up_step = down_step = FOLLOW_SIGN_CROSS_STEP + + limited = float(np.clip(target, float(previous) - down_step, float(previous) + up_step)) + return limited if abs(limited - float(target)) > 1e-6 else None + + +def apply(lead_one, lead_two, *, source: str, active: bool, v_ego: float, t_follow: float, + previous_target: float, raw_target: float, tracking: bool, post_departure: bool, + blocked: bool, panic_bypass: bool) -> FollowResult: + if blocked or panic_bypass or post_departure: + return FollowResult(None, None, None, float(raw_target)) + + lead = select_lead(lead_one, lead_two, source, active, v_ego, t_follow) + if lead is None: + return FollowResult(None, None, None, float(raw_target)) + + cap = _catchup_cap(lead, v_ego, t_follow, source=source, tracking=tracking, + post_departure=post_departure) + floor = _matched_brake_floor(lead, v_ego, t_follow) + target = float(raw_target) + if floor is not None: + target = max(target, floor) + if cap is not None: + target = min(target, cap) + transition = _transition_target(lead, v_ego, t_follow, previous_target, target) + if transition is not None: + target = transition + return FollowResult(lead, cap, floor, target) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 7d3e89f8a..70ee17e6a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,6 +16,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import shoul from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window +from openpilot.selfdrive.controls.lib.lead_follow_policy import apply as apply_follow_policy +from openpilot.selfdrive.controls.lib.lead_follow_policy import is_nonurgent_duplicate_vision_follow from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_far_follow_output_slew_rates, get_toyota_sienna_post_departure_restop_cap, @@ -44,8 +46,6 @@ LC_MERGE_ACCEL_MIN_DIST = 30.0 LC_MERGE_HEADROOM_MIN = 2.0 LC_MERGE_ACCEL_BIAS = 0.55 -LON_MPC_STEP = 0.2 # first step is 0.2s -A_CRUISE_MIN = -1.0 A_CRUISE_MAX_BP = [0.0, 5., 10., 15., 20., 25., 40.] A_CRUISE_MAX_VALS = [1.125, 1.125, 1.125, 1.125, 1.25, 1.25, 1.5] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -76,40 +76,11 @@ STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6 STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 0.8 STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08 STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL = 0.18 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1 STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME = 0.30 RADAR_STANDSTILL_GAP_SETTLE_ACCEL = 0.18 -RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME = 0.50 -RADAR_STANDSTILL_GAP_SETTLE_ENTRY_MARGIN = 0.60 -RADAR_STANDSTILL_GAP_SETTLE_EXIT_MARGIN = 0.15 -RADAR_STANDSTILL_GAP_SETTLE_MAX_EXTRA_GAP = 1.5 -RADAR_STANDSTILL_GAP_SETTLE_MAX_EGO_SPEED = 0.45 -RADAR_STANDSTILL_GAP_SETTLE_MAX_LEAD_SPEED = 0.15 -RADAR_STANDSTILL_GAP_SETTLE_MAX_LATERAL_OFFSET = 1.0 -LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75 -LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 -LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 -LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 -LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 LEAD_DEPART_CONFIDENT_CONFIRM_TIME = 0.35 LEAD_DEPART_RELEASE_HOLD_TIME = 1.5 LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME = 0.15 -LEAD_DEPART_RELEASE_HOLD_MIN_DISTANCE = 3.0 -LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_SPEED = 0.55 -LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_DELTA = 0.25 -LEAD_DEPART_RELEASE_HOLD_MAX_LEAD_BRAKE = 0.15 -LEAD_DEPART_RELEASE_HOLD_MIN_MODEL_PROB = 0.95 -LEAD_DEPART_RELEASE_HOLD_MAX_LATERAL_OFFSET = 1.0 -LEAD_DEPART_RELEASE_HOLD_CONFLICT_SPEED = 0.25 -LEAD_DEPART_RELEASE_HOLD_CONFLICT_DISTANCE_MARGIN = 3.0 -VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED = 10.0 -VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE = 25.0 -VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME = 1.35 -VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY = 1.35 -VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC = 8.0 -VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET = 1.5 STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED = 0.5 STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED = 0.45 STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA = 0.35 @@ -119,48 +90,8 @@ STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE = 3.0 STANDSTILL_STOPPED_LEAD_GUARD_DISTANCE_MARGIN = 3.0 STANDSTILL_STOPPED_LEAD_GUARD_MIN_BRAKE = 0.16 STANDSTILL_STOPPED_LEAD_GUARD_MAX_BRAKE = 0.26 -RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6 -RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5 -RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0 -RADAR_DEPART_CONFLICT_MIN_MODEL_PROB = 0.95 -RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE = 18.0 -RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL = 0.9 -RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED = 2.0 -RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH = 4.0 LEAD_DEPART_ACCEL_HOLD_TIME = 1.2 LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 2.0 -LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6 -LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_DELTA = 0.5 -LEAD_DEPART_ACCEL_HOLD_MIN_GAP = 3.5 -LEAD_DEPART_ACCEL_HOLD_FULL_GAP = 6.0 -LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED = 2.2 -LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB = 0.85 -LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL = 0.12 -LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE = 0.2 -LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL = 0.25 -LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.55 -LEAD_DEPART_ACCEL_ASSIST = 0.10 -LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_GAP = 3.75 -LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_CLOSING_SPEED = 0.45 -LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_LEAD_BRAKE = 0.2 -LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_HEADWAY_MARGIN = 0.10 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED = 4.5 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE = 4.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE = 18.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED = 4.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.75 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.9 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA = -0.5 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA = 0.75 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL = -0.4 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL = 0.25 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL = 0.08 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL = 0.22 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED = 1.25 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP = 4.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED = 1.2 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA = 0.8 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL = 0.5 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 INSIDE_GAP_CLOSING_MIN_EGO_SPEED = 8.0 INSIDE_GAP_CLOSING_MIN_LEAD_SPEED = 5.0 @@ -277,40 +208,11 @@ VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED = 0.75 VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED = 0.8 VISION_CLOSE_STOP_HOLD_MAX_DISTANCE = 3.5 VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB = 0.95 -VISION_CLOSE_STOP_HOLD_MIN_BRAKE = 0.20 -VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.36 VISION_CLOSE_SETTLE_MAX_EGO_SPEED = 0.75 VISION_CLOSE_SETTLE_MAX_LEAD_SPEED = 2.75 VISION_CLOSE_SETTLE_MAX_DISTANCE = 4.2 -VISION_CLOSE_SETTLE_MAX_LEAD_DELTA = 2.6 -VISION_CLOSE_SETTLE_MIN_BRAKE = 0.16 -VISION_CLOSE_SETTLE_MAX_BRAKE = 0.30 -VISION_CLOSE_FINAL_GUARD_MAX_EGO_SPEED = 0.5 -VISION_CLOSE_FINAL_GUARD_MAX_LEAD_SPEED = 2.75 -VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE = 4.5 -VISION_CLOSE_FINAL_GUARD_MIN_BRAKE = 0.18 -VISION_CLOSE_FINAL_GUARD_MAX_BRAKE = 0.28 -VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED = 2.5 -VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED = 3.5 -VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE = 4.2 -VISION_CLOSE_RELEASE_HOLD_MIN_MODEL_PROB = 0.98 -VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA = -0.1 -VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA = 1.5 -VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE = 0.18 -VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE = 0.40 -MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0 -MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0 MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL = 0.2 FORCE_STOP_HANDOFF_MAX_VCRUISE = 0.5 -LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 -LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 -LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0 -LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15 -RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN = 0.75 -RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP = 0.04 -RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR = 0.75 -RADAR_CATCHUP_ACCEL_CAP_ENTRY_FULL_SPEED = 12.0 -RADAR_CATCHUP_ACCEL_CAP_ENTRY_MAX = 1.5 POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME = 75.0 POST_DEPARTURE_FOLLOW_SETTLE_MIN_SPEED = 8.0 POST_DEPARTURE_FOLLOW_SETTLE_MAX_ARM_SPEED = 16.0 @@ -322,51 +224,6 @@ POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_LEAD_DELTA = -0.10 POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_LEAD_ACCEL = 0.20 POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_HEADWAY_MARGIN = 0.08 POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN = 0.05 -FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED = 12.0 -FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN = 0.10 -FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_HEADWAY_MARGIN = 0.90 -FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN = 4.0 -FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_CLOSING_SPEED = 0.20 -FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED = 1.50 -FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE = 0.35 -FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_ACCEL = 0.55 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED = 12.0 -LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB = 0.85 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE = 0.20 -LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_LEAD_DELTA = -1.2 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_MIN = 6.0 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.35 -LOW_SPEED_FOLLOW_TRANSITION_MIN_SPEED = 3.0 -LOW_SPEED_FOLLOW_TRANSITION_MAX_SPEED = 12.0 -LOW_SPEED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.85 -LOW_SPEED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.20 -LOW_SPEED_FOLLOW_TRANSITION_MIN_GAP_MARGIN = 1.0 -LOW_SPEED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 0.6 -LOW_SPEED_FOLLOW_TRANSITION_PREV_ACCEL_MIN = 0.18 -LOW_SPEED_FOLLOW_TRANSITION_TARGET_BRAKE_MIN = -0.18 -LOW_SPEED_FOLLOW_TRANSITION_MAX_BRAKE = 0.14 -LOW_SPEED_FOLLOW_TRANSITION_MIN_BRAKE = 0.08 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED = 10.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED = 20.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.85 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE = 0.25 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED = 1.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN = 12.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.9 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.15 -CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED = 1.5 -CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA = 0.25 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL = 0.18 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_SPEED = 12.0 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_SPEED = 22.0 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_MODEL_PROB = 0.9 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LEAD_BRAKE = 0.35 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LATERAL_OFFSET = 1.15 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED = 2.25 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_HEADWAY_ABOVE_TARGET = 0.95 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_DELTA_A = 0.18 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_STEP = 0.06 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_STEP = 0.18 # Uncertainty-based filter disable thresholds UNCERT_SLOPE_TRIG = 0.12 # per second @@ -375,9 +232,6 @@ UNCERT_PANIC_MIN_CLOSING_SPEED = 2.0 UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN = 0.08 UNCERT_PANIC_MAX_GAP_BUFFER_MIN = 8.0 UNCERT_PANIC_MAX_GAP_BUFFER_GAIN = 0.35 -UNCERT_DUPLICATE_VISION_MIN_TTC = 6.0 -UNCERT_DUPLICATE_VISION_MIN_HEADWAY = 0.85 -UNCERT_DUPLICATE_VISION_HEADWAY_BELOW_TARGET = 0.45 STEADY_FOLLOW_SMOOTHING_MIN_SPEED = 22.0 STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED = 0.15 STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED = 1.8 @@ -385,40 +239,7 @@ STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY = 0.95 STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET = 0.35 STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET = 0.90 STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE = 0.35 -STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB = 0.7 STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR = 0.24 -STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY = 1.05 -STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET = 0.90 -STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED = -1.2 -STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED = 2.2 -STEADY_FOLLOW_BRAKE_CAP_ZERO_REL_SPEED_DECEL = 0.12 -STEADY_FOLLOW_BRAKE_CAP_OPENING_DECEL = 0.08 -STEADY_FOLLOW_BRAKE_CAP_MIN_DECEL = 0.18 -STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL = 0.32 -STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_HEADWAY_MARGIN = 0.45 -STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MIN_HEADWAY = 1.80 -STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MAX_LEAD_BRAKE = 0.15 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DISTANCE = 80.0 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED = 0.1 -FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED = 2.0 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_MODEL_PROB = 0.95 -FAR_LEAD_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE = 0.12 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_TTC = 7.5 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN = 0.55 -FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 1.00 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL = 0.05 -FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL = 0.18 -FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DISTANCE = 40.0 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED = 0.5 -FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED = 1.8 -FAR_RADAR_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE = 0.12 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_TTC = 20.0 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN = 0.55 -FAR_RADAR_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 0.95 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DECEL = 0.04 -FAR_RADAR_COMFORT_BRAKE_CAP_MAX_DECEL = 0.14 -FAR_RADAR_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME = 0.75 # At low speed, preserve the existing handoff slew long enough to bridge a # brief slow-lead CEM dropout without extending high-speed release behavior. @@ -435,85 +256,6 @@ EXPERIMENTAL_RELEASE_ACCEL_MIN_HEADWAY_MARGIN = 0.0 EXPERIMENTAL_RELEASE_ACCEL_MIN_DELTA_A = 0.12 EXPERIMENTAL_RELEASE_ACCEL_STEP = 0.06 MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 20.0 -MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.25 -MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 0.75 -MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.9 -MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.18 -MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 1.75 -MATCHED_FOLLOW_TRANSITION_MIN_TTC = 12.0 -MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP = 0.08 -MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.18 -MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.08 -MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.16 -MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.10 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 10.0 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED = MATCHED_FOLLOW_TRANSITION_MIN_SPEED -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.45 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN = 0.20 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_LEAD_DELTA = 0.25 -MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_CLOSING_SPEED = 0.25 -MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP = 0.05 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 1.00 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.98 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.08 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 1.25 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC = 18.0 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP = 0.06 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.10 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.05 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.08 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.06 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET = -0.12 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A = 0.12 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_SPEED = 3.0 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_SPEED = 35.0 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_MODEL_PROB = 0.95 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_LEAD_BRAKE = 0.80 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_CLOSING_SPEED = 3.25 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_TTC = 6.0 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_HEADWAY_MARGIN = 0.25 -MILD_FOLLOW_ZERO_CROSS_GUARD_FULL_HEADWAY_MARGIN = 0.85 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DELTA_A = 0.08 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DEADBAND = 0.04 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_DEADBAND = 0.08 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_SPEED = 20.0 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_DISTANCE = 80.0 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_PROB = 0.85 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_LEAD_DELTA = 1.0 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_HEADWAY_MARGIN = 0.75 -FAR_OPENING_RADAR_BRAKE_GUARD_MAX_LEAD_BRAKE = 0.25 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_PREV_TARGET = -0.08 -FAR_OPENING_RADAR_BRAKE_GUARD_MAX_NEW_TARGET = -0.15 -FAR_OPENING_RADAR_BRAKE_GUARD_MAX_CRUISE_DEFICIT = 0.25 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_ACCEL = -0.50 -NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED = 20.0 -NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB = 0.95 -NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE = 0.35 -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 -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 -NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP = 0.18 -DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED = 12.0 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB = 0.95 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED = 0.5 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_LEAD_BRAKE = 0.8 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_HEADWAY_ABOVE_TARGET = 0.85 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_DELTA_A = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_DREL_DIFF = 1.5 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_VREL_DIFF = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_POSITIVE_STEP = 0.12 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_POSITIVE_STEP = 0.28 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_SIGN_CROSS_STEP = 0.22 TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0 TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95 TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL = 0.80 @@ -617,6 +359,98 @@ def get_vehicle_min_accel(CP, v_ego): return float(ACCEL_MIN) +# Restored planner constants retained by CEM, stop, and departure paths. +A_CRUISE_MIN = -1.0 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1 +RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME = 0.50 +RADAR_STANDSTILL_GAP_SETTLE_ENTRY_MARGIN = 0.60 +RADAR_STANDSTILL_GAP_SETTLE_EXIT_MARGIN = 0.15 +RADAR_STANDSTILL_GAP_SETTLE_MAX_EXTRA_GAP = 1.5 +RADAR_STANDSTILL_GAP_SETTLE_MAX_EGO_SPEED = 0.45 +RADAR_STANDSTILL_GAP_SETTLE_MAX_LEAD_SPEED = 0.15 +RADAR_STANDSTILL_GAP_SETTLE_MAX_LATERAL_OFFSET = 1.0 +LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75 +LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 +LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 +LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 +LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 +LEAD_DEPART_RELEASE_HOLD_MIN_DISTANCE = 3.0 +LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_SPEED = 0.55 +LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_DELTA = 0.25 +LEAD_DEPART_RELEASE_HOLD_MAX_LEAD_BRAKE = 0.15 +LEAD_DEPART_RELEASE_HOLD_MIN_MODEL_PROB = 0.95 +LEAD_DEPART_RELEASE_HOLD_MAX_LATERAL_OFFSET = 1.0 +LEAD_DEPART_RELEASE_HOLD_CONFLICT_SPEED = 0.25 +LEAD_DEPART_RELEASE_HOLD_CONFLICT_DISTANCE_MARGIN = 3.0 +VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED = 10.0 +VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE = 25.0 +VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME = 1.35 +VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY = 1.35 +VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC = 8.0 +VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET = 1.5 +RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6 +RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5 +RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0 +RADAR_DEPART_CONFLICT_MIN_MODEL_PROB = 0.95 +RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE = 18.0 +RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL = 0.9 +RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED = 2.0 +RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH = 4.0 +LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6 +LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_DELTA = 0.5 +LEAD_DEPART_ACCEL_HOLD_MIN_GAP = 3.5 +LEAD_DEPART_ACCEL_HOLD_FULL_GAP = 6.0 +LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED = 2.2 +LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB = 0.85 +LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL = 0.12 +LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE = 0.2 +LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL = 0.25 +LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.55 +LEAD_DEPART_ACCEL_ASSIST = 0.10 +LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_GAP = 3.75 +LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_CLOSING_SPEED = 0.45 +LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_LEAD_BRAKE = 0.2 +LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_HEADWAY_MARGIN = 0.10 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED = 4.5 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE = 4.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE = 18.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED = 4.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.75 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.9 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA = -0.5 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA = 0.75 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL = -0.4 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL = 0.25 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL = 0.08 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL = 0.22 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED = 1.25 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP = 4.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED = 1.2 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA = 0.8 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL = 0.5 +VISION_CLOSE_STOP_HOLD_MIN_BRAKE = 0.20 +VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.36 +VISION_CLOSE_SETTLE_MAX_LEAD_DELTA = 2.6 +VISION_CLOSE_SETTLE_MIN_BRAKE = 0.16 +VISION_CLOSE_SETTLE_MAX_BRAKE = 0.30 +VISION_CLOSE_FINAL_GUARD_MAX_EGO_SPEED = 0.5 +VISION_CLOSE_FINAL_GUARD_MAX_LEAD_SPEED = 2.75 +VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE = 4.5 +VISION_CLOSE_FINAL_GUARD_MIN_BRAKE = 0.18 +VISION_CLOSE_FINAL_GUARD_MAX_BRAKE = 0.28 +VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED = 2.5 +VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED = 3.5 +VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE = 4.2 +VISION_CLOSE_RELEASE_HOLD_MIN_MODEL_PROB = 0.98 +VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA = -0.1 +VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA = 1.5 +VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE = 0.18 +VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE = 0.40 +MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0 +MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0 + def get_planner_v_ego(CP, car_state): v_ego = max(car_state.vEgo, car_state.vEgoCluster) @@ -1831,494 +1665,6 @@ class LongitudinalPlanner: return True - @staticmethod - def get_follow_accel_cap_allowance(lead, v_ego, t_follow): - if lead is None or not lead.status or float(v_ego) < FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED: - return 0.0 - - closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(t_follow) - if (headway_margin <= FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN or - closing_speed >= FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED or - lead_brake >= FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE): - return 0.0 - - headway_factor = float(np.clip( - (headway_margin - FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN) / - (FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_HEADWAY_MARGIN - FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN), - 0.0, - 1.0, - )) - closing_factor = float(np.clip( - (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED - closing_speed) / - (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED - FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_CLOSING_SPEED), - 0.0, - 1.0, - )) - brake_factor = float(np.clip( - (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE - lead_brake) / - FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE, - 0.0, - 1.0, - )) - return FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_ACCEL * headway_factor * closing_factor * brake_factor - - def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow, current_source=None, tracking_lead_active=False): - if lead is None or not lead.status: - return None - - if self.post_departure_follow_settle_active(lead, v_ego, t_follow): - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - low_speed_follow_window = ( - not lead_radar and - v_ego <= LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED and - lead_prob >= LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB and - lead_brake <= LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE - ) - if v_ego < LEAD_CATCHUP_ACCEL_MIN_EGO and not low_speed_follow_window: - return None - - lead_delta = float(lead.vLead) - float(v_ego) - min_lead_delta = LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_LEAD_DELTA if low_speed_follow_window else LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - if low_speed_follow_window: - gap_buffer = max(LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_MIN, - LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego)) - else: - gap_buffer = max(LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN, - LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN * float(v_ego)) - gap_error = float(lead.dRel) - desired_gap - - radar_matched_follow_active = ( - lead_radar and - tracking_lead_active and - self.lead_is_matched_follow_window(lead, v_ego, t_follow) - ) - if radar_matched_follow_active and gap_error > (gap_buffer - RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN): - return None - - if (radar_matched_follow_active and current_source == "cruise" and - gap_error <= RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR and - lead_delta < min_lead_delta): - return RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP - - if lead_delta < min_lead_delta: - return None - - if gap_error > gap_buffer: - return None - - # Keep the near-target cap conservative, then continuously relax it when - # there is real headway to use. Avoid binary bypasses around zero vRel. - if low_speed_follow_window: - edge_cap = float(np.interp(lead_delta, [min_lead_delta, 0.0, 1.0, 2.0], [0.20, 0.24, 0.38, 0.55])) - near_cap = min(edge_cap, 0.16) - else: - edge_cap = float(np.interp(lead_delta, [-0.5, 0.0, 1.0], [0.16, 0.08, 0.02])) - near_cap = min(edge_cap, 0.03) - gap_factor = float(np.clip(max(gap_error, 0.0) / max(gap_buffer, 0.1), 0.0, 1.0)) - cap = float(np.interp(gap_factor, [0.0, 1.0], [near_cap, edge_cap])) - allowance_factor = float(np.clip(gap_error / FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN, 0.0, 1.0)) - cap += allowance_factor * self.get_follow_accel_cap_allowance(lead, v_ego, t_follow) - if not low_speed_follow_window: - entry_factor = float(np.clip( - (float(v_ego) - LEAD_CATCHUP_ACCEL_MIN_EGO) / - (RADAR_CATCHUP_ACCEL_CAP_ENTRY_FULL_SPEED - LEAD_CATCHUP_ACCEL_MIN_EGO), - 0.0, - 1.0, - )) - cap = float(np.interp(entry_factor, [0.0, 1.0], [RADAR_CATCHUP_ACCEL_CAP_ENTRY_MAX, cap])) - return cap - - def get_low_speed_follow_transition_brake_cap(self, lead, v_ego, t_follow, prev_output_a_target, output_a_target): - if lead is None or not lead.status: - return None - if bool(getattr(lead, "radar", False)): - return None - if not (LOW_SPEED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) <= LOW_SPEED_FOLLOW_TRANSITION_MAX_SPEED): - return None - if prev_output_a_target <= LOW_SPEED_FOLLOW_TRANSITION_PREV_ACCEL_MIN: - return None - if output_a_target >= LOW_SPEED_FOLLOW_TRANSITION_TARGET_BRAKE_MIN: - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < LOW_SPEED_FOLLOW_TRANSITION_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > LOW_SPEED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE: - return None - - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - if closing_speed > LOW_SPEED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED: - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - if float(lead.dRel) < desired_gap + LOW_SPEED_FOLLOW_TRANSITION_MIN_GAP_MARGIN: - return None - - cap_decel = float(np.interp( - closing_speed, - [0.0, LOW_SPEED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED], - [LOW_SPEED_FOLLOW_TRANSITION_MIN_BRAKE, LOW_SPEED_FOLLOW_TRANSITION_MAX_BRAKE], - )) - return -cap_decel - - def get_cruise_tracking_lead_accel_cap(self, lead, v_ego, t_follow, current_source, tracking_lead_active): - if lead is None or not lead.status or current_source != "cruise": - return None - if self.post_departure_follow_settle_active(lead, v_ego, t_follow): - return None - if not (CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED): - return None - - lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) - if not bool(getattr(lead, "radar", False)) and lead_prob < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE and not tracking_lead_active: - return None - - if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET: - return None - - lead_delta = float(lead.vLead) - float(v_ego) - if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED: - return None - - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - if ( - float(v_ego) < FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED and - lead_delta >= 0.35 and - lead_accel >= 0.25 - ): - return None - - closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) - raw_close_lead = self.raw_close_lead_needs_control(lead, v_ego) - unresolved_slow_lead = ( - closing_speed >= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED and - lead_delta <= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA - ) - if not tracking_lead_active and not raw_close_lead and not unresolved_slow_lead: - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - gap_error = float(lead.dRel) - desired_gap - gap_buffer = max(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN, - CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego)) - if gap_error > gap_buffer: - return None - - base_cap = float(np.interp( - lead_delta, - [-1.5, -0.5, 0.0, 0.5, CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED], - [0.0, 0.04, 0.08, 0.12, 0.16], - )) - - if raw_close_lead: - base_cap = min(base_cap, float(np.interp(closing_speed, [0.5, 1.5, 3.5], [0.10, 0.05, 0.0]))) - else: - base_cap = min(base_cap, float(np.interp(closing_speed, [0.0, 1.0, 2.0], [0.18, 0.12, 0.06]))) - - gap_factor = float(np.clip(gap_error / max(gap_buffer, 0.1), 0.0, 1.0)) - cap = min(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL, base_cap + 0.06 * gap_factor) - allowance_factor = float(np.clip(gap_error / FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN, 0.0, 1.0)) - cap += allowance_factor * self.get_follow_accel_cap_allowance(lead, v_ego, t_follow) - return max(0.0, cap) - - def get_cruise_tracking_lead_accel_transition_target(self, lead, v_ego, t_follow, - prev_output_a_target, output_a_target, - current_source): - if lead is None or not lead.status or current_source != "cruise": - return None - if not (CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_SPEED): - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if target_delta < CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_DELTA_A: - return None - - lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) - if not bool(getattr(lead, "radar", False)) and lead_prob < CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LEAD_BRAKE: - return None - - if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LATERAL_OFFSET: - return None - - lead_delta = float(lead.vLead) - float(v_ego) - if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(t_follow) - if headway_margin > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_HEADWAY_ABOVE_TARGET: - return None - - positive_step = float(np.interp( - lead_delta, - [-1.0, 0.0, 1.0, CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED], - [CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_STEP, - 0.08, - 0.12, - CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_STEP], - )) - if headway_margin > 0.0: - headway_factor = float(np.clip( - headway_margin / max(CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_HEADWAY_ABOVE_TARGET, 1e-3), - 0.0, - 1.0, - )) - positive_step = float(np.interp( - headway_factor, - [0.0, 1.0], - [positive_step, CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_STEP], - )) - - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(min(output_a_target, upper)) - return smoothed_target if smoothed_target < float(output_a_target) - 1e-6 else None - - def lead_is_matched_follow_window(self, lead, v_ego, base_t_follow): - if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: - return False - - relative_speed = float(v_ego) - float(lead.vLead) - if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED): - return False - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE: - return False - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and not is_radarless_matched_follow_window( - v_ego, - lead.dRel, - lead.vLead, - base_t_follow, - radar=lead_radar, - lead_brake=lead_brake, - lead_prob=lead_prob, - ): - return False - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway < max(STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY, float(base_t_follow) - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET): - return False - if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: - return False - return True - - def get_matched_follow_control_lead(self, v_ego, t_follow): - if self.mpc.source == 'lead1' and self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow): - return self.lead_two - if self.lead_is_matched_follow_window(self.lead_one, v_ego, t_follow): - return self.lead_one - if self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow): - return self.lead_two - return None - - def get_follow_control_lead(self, lead_control_active, v_ego, t_follow, *, allow_optional_far_lead_logic=True): - if allow_optional_far_lead_logic: - matched_follow_lead = self.get_matched_follow_control_lead(v_ego, t_follow) - if matched_follow_lead is not None: - return matched_follow_lead - - if not lead_control_active: - return None - - if self.lead_one.status: - return self.lead_one - if self.lead_two.status: - return self.lead_two - return None - - def get_duplicate_vision_comfort_lead(self, v_ego): - if not (self.lead_one.status and self.lead_two.status): - self.duplicate_vision_comfort_lead_source = None - return None - - if bool(getattr(self.lead_one, "radar", False)) or bool(getattr(self.lead_two, "radar", False)): - 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, - vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED, - ): - self.duplicate_vision_comfort_lead_source = None - return None - - lead_sources = { - "lead0": self.lead_one, - "lead1": self.lead_two, - } - latched_lead = lead_sources.get(self.duplicate_vision_comfort_lead_source) - if latched_lead is not None and latched_lead.status: - return latched_lead - - min_center_offset = min(abs(float(getattr(lead, "yRel", 0.0))) for lead in lead_sources.values()) - centered_sources = [ - (source, lead) for source, lead in lead_sources.items() - if abs(float(getattr(lead, "yRel", 0.0))) <= min_center_offset + DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN - ] - selected_source, selected_lead = min( - centered_sources, - key=lambda item: ( - float(item[1].dRel), - float(item[1].vLead), - -max(0.0, -float(getattr(item[1], "aLeadK", 0.0))), - abs(float(getattr(item[1], "yRel", 0.0))), - ), - ) - self.duplicate_vision_comfort_lead_source = selected_source - return selected_lead - - def is_nonurgent_duplicate_vision_follow(self, v_ego, t_follow): - if bool(getattr(self.lead_one, "radar", False)) or bool(getattr(self.lead_two, "radar", False)): - return False - 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 False - - lead = self.lead_one - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - minimum_headway = max( - UNCERT_DUPLICATE_VISION_MIN_HEADWAY, - float(t_follow) - UNCERT_DUPLICATE_VISION_HEADWAY_BELOW_TARGET, - ) - return ttc > UNCERT_DUPLICATE_VISION_MIN_TTC and actual_headway > minimum_headway - - def lead_is_spacious_brake_cap_window(self, lead, v_ego, base_t_follow): - if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: - return False - - relative_speed = float(v_ego) - float(lead.vLead) - if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED): - return False - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MAX_LEAD_BRAKE: - return False - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB: - return False - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway < max(float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_HEADWAY_MARGIN, - STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MIN_HEADWAY): - return False - if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: - return False - return True - - def get_matched_follow_brake_cap(self, lead, v_ego, base_t_follow): - if not ( - self.lead_is_matched_follow_window(lead, v_ego, base_t_follow) or - self.lead_is_spacious_brake_cap_window(lead, v_ego, base_t_follow) - ): - return None - - relative_speed = float(v_ego) - float(lead.vLead) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - cap_decel = float(np.interp( - relative_speed, - [STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED, 0.0, STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED], - [STEADY_FOLLOW_BRAKE_CAP_OPENING_DECEL, - STEADY_FOLLOW_BRAKE_CAP_ZERO_REL_SPEED_DECEL, - STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL], - )) - headway_deficit = float(np.clip((float(base_t_follow) - actual_headway) / STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET, 0.0, 1.0)) - cap_decel = min(STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL, cap_decel + 0.05 * headway_deficit) - return -cap_decel - - def get_far_lead_brake_cap(self, lead, v_ego, base_t_follow): - if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: - return None - - relative_speed = float(v_ego) - float(lead.vLead) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - - if bool(getattr(lead, "radar", False)): - if not (FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED <= relative_speed <= FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED): - return None - if lead_brake > FAR_RADAR_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE: - return None - if float(lead.dRel) < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DISTANCE or headway_margin < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN: - return None - - ttc = float(lead.dRel) / max(relative_speed, 1e-3) - if ttc < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_TTC: - return None - - cap_decel = float(np.interp( - relative_speed, - [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED, FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED], - [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DECEL, FAR_RADAR_COMFORT_BRAKE_CAP_MAX_DECEL], - )) - relax_decel = float(np.interp( - headway_margin, - [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN, FAR_RADAR_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN], - [0.0, FAR_RADAR_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL], - )) - return -max(0.0, cap_decel - relax_decel) - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_MODEL_PROB: - return None - - if not (FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED <= relative_speed <= FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED): - return None - - if lead_brake > FAR_LEAD_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE: - return None - - if float(lead.dRel) < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DISTANCE or headway_margin < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN: - return None - - ttc = float(lead.dRel) / max(relative_speed, 1e-3) - if ttc < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_TTC: - return None - - cap_decel = float(np.interp( - relative_speed, - [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED, FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED], - [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL, FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL], - )) - relax_decel = float(np.interp( - headway_margin, - [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN, FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN], - [0.0, FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL], - )) - return -max(0.0, cap_decel - relax_decel) - def update_experimental_release_accel_state(self, experimental_mode, now_t, v_ego=None): if self.prev_experimental_mode is True and not experimental_mode: low_speed_release = v_ego is not None and float(v_ego) < EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_THRESHOLD @@ -2357,354 +1703,6 @@ class LongitudinalPlanner: return min(float(output_a_target), float(prev_output_a_target) + EXPERIMENTAL_RELEASE_ACCEL_STEP) - def get_matched_follow_transition_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - low_speed_extension_active = ( - bool(tracking_lead_active) and - current_source == "cruise" and - LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED - ) - if float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED and not low_speed_extension_active: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 0.0)) - min_model_prob = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB - if lead_prob < min_model_prob: - return None - - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - lead_brake = max(0.0, -lead_accel) - max_lead_brake = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE - if lead_brake > max_lead_brake: - return None - - relative_speed = float(v_ego) - float(lead.vLead) - opening_radar_transition = bool( - lead_radar and - -relative_speed >= LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_LEAD_DELTA and - -relative_speed <= CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED and - lead_accel >= 0.0 - ) - relative_speed_in_range = STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED - if not relative_speed_in_range and not opening_radar_transition: - return None - - closing_speed = max(0.0, relative_speed) - max_closing_speed = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_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") - min_ttc = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_TTC - if ttc < min_ttc: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN - opening_radar_lead = bool( - low_speed_extension_active and - lead_radar and - opening_radar_transition - ) - if opening_radar_lead: - min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN - full_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN - if headway_margin < min_headway_margin: - return None - if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if low_speed_extension_active: - if float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET: - return None - if float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET: - return None - if abs(target_delta) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A: - return None - elif abs(target_delta) < 1e-3: - return None - - headway_factor = float(np.clip( - (headway_margin - min_headway_margin) / - max(full_headway_margin - min_headway_margin, 1e-3), - 0.0, - 1.0, - )) - - min_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP - max_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP - positive_step = float(np.interp( - max(float(lead.vLead) - float(v_ego), 0.0), - [0.0, 1.0], - [min_positive_step, max_positive_step], - )) - min_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP - max_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP - negative_step = float(np.interp( - closing_speed, - [0.0, max_closing_speed], - [min_negative_step, max_negative_step], - )) - - # The more space we still have, the less abrupt the comfort path should be. - positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, min_positive_step])) - negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, min_negative_step])) - - stable_radar_catchup = bool( - lead_radar and - relative_speed <= MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_CLOSING_SPEED and - lead_accel >= 0.0 and - headway_margin >= LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN - ) - if stable_radar_catchup: - positive_step = min(positive_step, MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP) - negative_step = min(negative_step, MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP) - - if float(prev_output_a_target) * float(output_a_target) < 0.0: - sign_cross_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP - positive_step = min(positive_step, sign_cross_step) - negative_step = min(negative_step, sign_cross_step) - - lower = float(prev_output_a_target) - negative_step - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(np.clip(output_a_target, lower, upper)) - return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None - - def get_near_duplicate_lead_transition_target(self, lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - if current_source not in ("cruise", "lead0", "lead1"): - return None - if current_source == "cruise" and not tracking_lead_active: - return None - 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, - vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED, - ): - return None - low_speed_extension_active = bool( - tracking_lead_active and - current_source in ("cruise", "lead0", "lead1") and - LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED - ) - if float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED: - return None - if float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED and not low_speed_extension_active: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE: - return None - - relative_speed = float(v_ego) - float(lead.vLead) - closing_speed = max(0.0, relative_speed) - 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 - 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 - - 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 - - if low_speed_extension_active and (float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET or - float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET): - return None - - positive_step = NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP - negative_step = NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP - if float(prev_output_a_target) * float(output_a_target) < 0.0: - positive_step = min(positive_step, NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP) - negative_step = min(negative_step, NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP) - - lower = float(prev_output_a_target) - negative_step - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(np.clip(output_a_target, lower, upper)) - return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None - - def get_mild_follow_zero_cross_guard_target(self, lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - if current_source not in ("cruise", "lead0", "lead1") and not tracking_lead_active: - return None - if not (MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_SPEED <= float(v_ego) <= MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_SPEED): - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if abs(target_delta) < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DELTA_A: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_LEAD_BRAKE: - return None - - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - if closing_speed > MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_CLOSING_SPEED: - return None - - ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - if ttc < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_TTC: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - if headway_margin < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_HEADWAY_MARGIN: - return None - - deadband = float(np.interp( - headway_margin, - [MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_HEADWAY_MARGIN, MILD_FOLLOW_ZERO_CROSS_GUARD_FULL_HEADWAY_MARGIN], - [MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DEADBAND, MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_DEADBAND], - )) - - if float(prev_output_a_target) * float(output_a_target) < 0.0: - return 0.0 - - if abs(float(output_a_target)) < deadband and abs(float(prev_output_a_target)) < deadband + 0.06: - return 0.0 - - return None - - @staticmethod - def get_far_opening_radar_brake_guard_target(lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - v_cruise, model_desired_accel, - current_source, tracking_lead_active): - if lead is None or not lead.status or not bool(getattr(lead, "radar", False)): - return None - if current_source != "cruise" or not tracking_lead_active: - return None - if float(v_ego) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_SPEED: - return None - if float(lead.dRel) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_DISTANCE: - return None - - lead_prob = float(getattr(lead, "modelProb", 1.0)) - lead_delta = float(lead.vLead) - float(v_ego) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - if ( - lead_prob < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_PROB or - lead_delta < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_LEAD_DELTA or - lead_brake > FAR_OPENING_RADAR_BRAKE_GUARD_MAX_LEAD_BRAKE or - headway_margin < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_HEADWAY_MARGIN - ): - return None - - if float(prev_output_a_target) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_PREV_TARGET: - return None - if float(output_a_target) > FAR_OPENING_RADAR_BRAKE_GUARD_MAX_NEW_TARGET: - return None - if float(v_cruise) < float(v_ego) - FAR_OPENING_RADAR_BRAKE_GUARD_MAX_CRUISE_DEFICIT: - return None - if float(model_desired_accel) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_ACCEL: - return None - - # A far lead that is pulling away cannot justify a one-frame braking pulse. - # Preserve real speed-target, model, and closing-lead deceleration above. - return 0.0 - - def get_duplicate_slow_lead_brake_hold_target(self, lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - if current_source not in ("cruise", "lead0", "lead1") and not tracking_lead_active: - return None - if not (self.lead_one.status and self.lead_two.status): - return None - if ( - abs(float(self.lead_one.dRel) - float(self.lead_two.dRel)) > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_DREL_DIFF or - abs(float(self.lead_one.vRel) - float(self.lead_two.vRel)) > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_VREL_DIFF - ): - return None - if float(v_ego) < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED: - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if bool(getattr(lead, "radar", False)) or lead_prob < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB: - return None - - prev_brake = max(0.0, -float(prev_output_a_target)) - if prev_brake < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL: - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if target_delta < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_DELTA_A: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_LEAD_BRAKE: - return None - - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - if closing_speed < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway > float(base_t_follow) + DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_HEADWAY_ABOVE_TARGET: - return None - - positive_step = float(np.interp( - closing_speed, - [DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED, 1.5, 4.0, 8.0], - [DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_POSITIVE_STEP, - 0.16, - 0.22, - DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_POSITIVE_STEP], - )) - if float(prev_output_a_target) * float(output_a_target) < 0.0: - positive_step = min(positive_step, DUPLICATE_SLOW_LEAD_BRAKE_HOLD_SIGN_CROSS_STEP) - - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(min(float(output_a_target), upper)) - return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None - def get_tracked_vision_model_brake_floor(self, lead, v_ego, accel_min, t_follow, model_desired): if lead is None or not lead.status or bool(getattr(lead, "radar", False)): return None @@ -3094,7 +2092,9 @@ class LongitudinalPlanner: ) # Duplicate vision tracks can share the same noisy velocity spike. Keep the # comfort path unless distance, TTC, or lead braking makes the scene urgent. - nonurgent_duplicate_vision_follow = self.is_nonurgent_duplicate_vision_follow(scene_v_ego, effective_t_follow) + nonurgent_duplicate_vision_follow = is_nonurgent_duplicate_vision_follow( + self.lead_one, self.lead_two, scene_v_ego, effective_t_follow, self.mpc, + ) if panic_bypass and nonurgent_duplicate_vision_follow: panic_bypass = False @@ -3629,18 +2629,6 @@ class LongitudinalPlanner: if close_final_guard_caps: close_final_guard_cap = min(close_final_guard_caps) - if lead_one_active: - lead_catchup_accel_cap = self.get_lead_catchup_accel_cap( - self.lead_one, - scene_v_ego, - effective_t_follow, - current_source=self.mpc.source, - tracking_lead_active=tracking_lead, - ) - if lead_catchup_accel_cap is not None: - self.a_desired = min(self.a_desired, lead_catchup_accel_cap) - output_a_target = min(output_a_target, lead_catchup_accel_cap) - if lead_control_active and np.isfinite(v_cruise) and any(lead.status for lead in (self.lead_one, self.lead_two)): # This is only an acceleration cap. A negative cap would manufacture hard # braking on abrupt cruise-target drops instead of letting the MPC decelerate. @@ -3650,165 +2638,50 @@ class LongitudinalPlanner: if vision_brake_cap_active: output_accel_min = min(output_accel_min, vision_cap_accel_min) - follow_control_lead = self.get_follow_control_lead( - lead_control_active, - scene_v_ego, - effective_t_follow, - allow_optional_far_lead_logic=True, + policy_lead = self.lead_two if self.mpc.source == "lead1" else self.lead_one + post_departure_active = self.post_departure_follow_settle_active( + policy_lead, scene_v_ego, effective_t_follow, ) - duplicate_vision_comfort_lead = self.get_duplicate_vision_comfort_lead(scene_v_ego) - comfort_follow_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None and follow_control_lead is not None else follow_control_lead - if follow_control_lead is not None and not panic_bypass: - if not output_should_stop and not vision_low_speed_stop_active: - tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( - follow_control_lead, - scene_v_ego, - output_accel_min, - effective_t_follow, - model_desired_accel, - ) - if tracked_vision_model_brake_floor is not None: - self.a_desired = min(self.a_desired, tracked_vision_model_brake_floor) - output_a_target = min(output_a_target, tracked_vision_model_brake_floor) - - matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow) - if matched_follow_brake_cap is not None: - self.a_desired = max(self.a_desired, matched_follow_brake_cap) - output_a_target = max(output_a_target, matched_follow_brake_cap) - - if not close_lead_caps and not output_should_stop and not vision_low_speed_stop_active: - low_speed_transition_brake_cap = self.get_low_speed_follow_transition_brake_cap( - follow_control_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - ) - if low_speed_transition_brake_cap is not None: - self.a_desired = max(self.a_desired, low_speed_transition_brake_cap) - output_a_target = max(output_a_target, low_speed_transition_brake_cap) - - comfort_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None else ( - self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one + follow_result = apply_follow_policy( + self.lead_one, + self.lead_two, + source=self.mpc.source, + active=lead_control_active, + v_ego=scene_v_ego, + t_follow=effective_t_follow, + previous_target=prev_output_a_target, + raw_target=output_a_target, + tracking=tracking_lead, + post_departure=post_departure_active, + blocked=bool(output_should_stop or vision_low_speed_stop_active or close_lead_caps), + panic_bypass=panic_bypass, ) - if comfort_lead is not None and not panic_bypass: - far_lead_brake_cap = self.get_far_lead_brake_cap(comfort_lead, scene_v_ego, effective_t_follow) - if far_lead_brake_cap is not None: - self.a_desired = max(self.a_desired, far_lead_brake_cap) - output_a_target = max(output_a_target, far_lead_brake_cap) + comfort_follow_lead = follow_result.lead + comfort_lead = follow_result.lead + if follow_result.target < output_a_target: + self.a_desired = min(self.a_desired, follow_result.target) + else: + self.a_desired = max(self.a_desired, follow_result.target) + output_a_target = follow_result.target - if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + # Model-backed braking remains outside the ordinary follow policy. These + # floors are safety responses, not comfort arbitration. + if comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( + comfort_follow_lead, scene_v_ego, output_accel_min, effective_t_follow, model_desired_accel, + ) + if tracked_vision_model_brake_floor is not None: + self.a_desired = min(self.a_desired, tracked_vision_model_brake_floor) + output_a_target = min(output_a_target, tracked_vision_model_brake_floor) + + if comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: tracked_vision_model_brake_cap = self.get_tracked_vision_model_brake_cap( - follow_control_lead, - scene_v_ego, - effective_t_follow, - model_desired_accel, + comfort_follow_lead, scene_v_ego, effective_t_follow, model_desired_accel, ) if tracked_vision_model_brake_cap is not None: self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap) output_a_target = max(output_a_target, tracked_vision_model_brake_cap) - if comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: - matched_follow_transition_target = self.get_matched_follow_transition_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - bool(getattr(sm["starpilotPlan"], "trackingLead", False)), - ) - if matched_follow_transition_target is not None: - if matched_follow_transition_target < output_a_target: - self.a_desired = min(self.a_desired, matched_follow_transition_target) - else: - self.a_desired = max(self.a_desired, matched_follow_transition_target) - output_a_target = matched_follow_transition_target - - if comfort_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: - near_duplicate_transition_target = self.get_near_duplicate_lead_transition_target( - comfort_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - bool(getattr(sm["starpilotPlan"], "trackingLead", False)), - ) - if near_duplicate_transition_target is not None: - if near_duplicate_transition_target < output_a_target: - self.a_desired = min(self.a_desired, near_duplicate_transition_target) - else: - self.a_desired = max(self.a_desired, near_duplicate_transition_target) - output_a_target = near_duplicate_transition_target - - duplicate_slow_lead_brake_hold_target = self.get_duplicate_slow_lead_brake_hold_target( - comfort_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - bool(getattr(sm["starpilotPlan"], "trackingLead", False)), - ) - if duplicate_slow_lead_brake_hold_target is not None: - if duplicate_slow_lead_brake_hold_target < output_a_target: - self.a_desired = min(self.a_desired, duplicate_slow_lead_brake_hold_target) - else: - self.a_desired = max(self.a_desired, duplicate_slow_lead_brake_hold_target) - output_a_target = duplicate_slow_lead_brake_hold_target - - if comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: - cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - self.mpc.source, - tracking_lead, - ) - if cruise_tracking_lead_accel_cap is not None: - self.a_desired = min(self.a_desired, cruise_tracking_lead_accel_cap) - output_a_target = min(output_a_target, cruise_tracking_lead_accel_cap) - - cruise_tracking_lead_transition_target = self.get_cruise_tracking_lead_accel_transition_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - ) - if cruise_tracking_lead_transition_target is not None: - self.a_desired = min(self.a_desired, cruise_tracking_lead_transition_target) - output_a_target = min(output_a_target, cruise_tracking_lead_transition_target) - - mild_follow_zero_cross_guard_target = self.get_mild_follow_zero_cross_guard_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - tracking_lead, - ) - if mild_follow_zero_cross_guard_target is not None: - output_a_target = mild_follow_zero_cross_guard_target - - far_opening_radar_brake_guard_target = self.get_far_opening_radar_brake_guard_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - v_cruise, - model_desired_accel, - self.mpc.source, - tracking_lead, - ) - if far_opening_radar_brake_guard_target is not None: - self.a_desired = max(self.a_desired, far_opening_radar_brake_guard_target) - output_a_target = max(output_a_target, far_opening_radar_brake_guard_target) - output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) diff --git a/selfdrive/controls/tests/test_lead_follow_policy.py b/selfdrive/controls/tests/test_lead_follow_policy.py new file mode 100644 index 000000000..5c388f80c --- /dev/null +++ b/selfdrive/controls/tests/test_lead_follow_policy.py @@ -0,0 +1,71 @@ +from types import SimpleNamespace + +import pytest + +from openpilot.selfdrive.controls.lib.lead_follow_policy import apply + + +def lead(*, d_rel=40.0, v_lead=20.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.0): + return SimpleNamespace( + status=True, + dRel=d_rel, + vLead=v_lead, + aLeadK=a_lead, + radar=radar, + modelProb=model_prob, + yRel=y_rel, + ) + + +def run(lead_one, *, lead_two=None, source="lead0", active=True, v_ego=20.0, + previous=0.0, raw=0.0, post_departure=False, blocked=False, panic=False): + return apply( + lead_one, + lead_two or SimpleNamespace(status=False), + source=source, + active=active, + v_ego=v_ego, + t_follow=1.45, + previous_target=previous, + raw_target=raw, + tracking=active, + post_departure=post_departure, + blocked=blocked, + panic_bypass=panic, + ) + + +def test_follow_policy_never_reselects_or_fuses_leads(): + lead_one = lead(d_rel=80.0, v_lead=20.0) + lead_two = lead(d_rel=35.0, v_lead=19.5) + + assert run(lead_one, lead_two=lead_two, source="lead0").lead is lead_one + assert run(lead_one, lead_two=lead_two, source="lead1").lead is lead_two + + inactive = run(lead_one, lead_two=lead_two, source="lead1", active=False, raw=0.3) + assert inactive.lead is None + assert inactive.target == pytest.approx(0.3) + + +@pytest.mark.parametrize("blocked", [True, False]) +def test_follow_policy_keeps_stop_and_panic_outputs_outside_comfort_path(blocked): + result = run(lead(d_rel=8.0, v_lead=0.0), raw=-1.2, blocked=blocked, panic=not blocked) + assert result.lead is None + assert result.target == pytest.approx(-1.2) + + +def test_follow_policy_limits_small_post_lead_reversal(): + result = run(lead(d_rel=36.0, v_lead=20.2), v_ego=20.0, previous=-0.08, raw=0.45) + assert result.target < 0.45 + assert result.target - (-0.08) <= 0.18 + + +def test_follow_policy_never_relaxes_material_braking(): + result = run(lead(d_rel=25.0, v_lead=18.0), v_ego=25.0, previous=0.30, raw=-1.2) + assert result.target == pytest.approx(-1.2) + + +def test_follow_policy_bypasses_post_departure_handoff(): + result = run(lead(d_rel=46.0, v_lead=22.0), v_ego=20.0, previous=0.0, raw=0.6, post_departure=True) + assert result.target == pytest.approx(0.6) + assert result.accel_cap is None diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index db69ab565..906fd7a07 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -796,34 +796,6 @@ def test_vision_untracked_approach_lift_releases_after_hold_or_tracking(): assert releasing_cap > previous_cap -def test_far_opening_radar_brake_guard_removes_only_harmless_pulse(): - v_ego = 28.9 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - opening_lead = make_lead(status=True, d_rel=96.5, v_lead=32.0, a_lead=-0.15, radar=True, model_prob=0.93) - - guard_target = planner.get_far_opening_radar_brake_guard_target( - opening_lead, v_ego, 1.25, 0.0, -0.41, 29.0, 0.0, "cruise", True, - ) - - assert guard_target == 0.0 - - -def test_far_opening_radar_brake_guard_preserves_close_or_requested_braking(): - v_ego = 28.9 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - close_slower_lead = make_lead(status=True, d_rel=36.9, v_lead=20.9, a_lead=-0.18, radar=True, model_prob=0.99) - far_opening_lead = make_lead(status=True, d_rel=96.5, v_lead=32.0, a_lead=-0.15, radar=True, model_prob=0.93) - - assert planner.get_far_opening_radar_brake_guard_target( - close_slower_lead, v_ego, 1.25, 0.0, -3.5, 29.0, 0.0, "lead0", True, - ) is None - assert planner.get_far_opening_radar_brake_guard_target( - far_opening_lead, v_ego, 1.25, 0.0, -0.41, 27.0, 0.0, "cruise", True, - ) 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) @@ -2794,349 +2766,6 @@ def test_no_throttle_cap_stays_at_coast_limit_until_throttle_returns(): assert planner.output_a_target == pytest.approx(accel_coast, abs=1e-3) -def test_low_speed_follow_catchup_accel_cap_limits_close_vision_catchup(): - v_ego = 7.8 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=18.4, v_lead=8.2, radar=False, model_prob=0.98) - - cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) - - assert cap is not None - assert 0.15 <= cap <= 0.45 - - -def test_route_8bc6_post_departure_catchup_cap_uses_continuous_allowance_for_accelerating_radar_lead(): - v_ego = 19.03 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, - ) - - cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) - - assert cap is not None - assert 0.1 < cap < 0.3 - - -def test_route_8bc6_cruise_tracking_cap_uses_continuous_allowance_for_accelerating_radar_follow(): - v_ego = 18.744474411010742 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.650001525878906, v_lead=19.049156188964844, - a_lead=0.4783128798007965, radar=True, model_prob=0.9989967942237854, y_rel=-0.2, - ) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.3 < cap < 0.5 - - -def test_route_687_voacc_catchup_cap_uses_continuous_spacious_allowance(): - v_ego = 12.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, - ) - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.15 < cap < 0.3 - - -def test_route_687_voacc_cruise_tracking_cap_uses_continuous_spacious_allowance(): - v_ego = 12.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, - ) - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.15 < cap < 0.3 - - -def test_low_speed_follow_catchup_uses_raw_vehicle_speed_when_cluster_runs_high(): - v_ego = 7.8 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.6, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=16.0, v_lead=8.4, radar=False, model_prob=0.99), - ) - sm["carState"].vEgoCluster = 9.2 - sm["starpilotPlan"].vCruise = v_ego + 4.0 - - for _ in range(6): - planner.update(sm, make_toggles()) - - assert planner.output_a_target <= 0.20 - - -def test_low_speed_follow_transition_brake_cap_softens_first_sign_flip(): - v_ego = 7.7 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=18.6, v_lead=7.6, radar=False, model_prob=0.98) - - cap = planner.get_low_speed_follow_transition_brake_cap(lead, v_ego, 1.45, 0.59, -0.24) - - assert cap is not None - assert -0.14 <= cap <= -0.08 - - -def test_low_speed_follow_transition_brake_cap_stays_off_when_gap_is_tight(): - v_ego = 7.7 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=13.0, v_lead=7.6, radar=False, model_prob=0.98) - - cap = planner.get_low_speed_follow_transition_brake_cap(lead, v_ego, 1.45, 0.59, -0.24) - - assert cap is None - - -def test_far_near_speed_follow_keeps_uncertainty_smoothing_active(): - v_ego = 30.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=78.0, v_lead=29.2, radar=False, model_prob=0.96), - ) - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=1.0, brake_press_prob=0.52) - toggles = make_toggles() - - for _ in range(12): - planner.update(sm, toggles) - - assert planner.mpc.filter_time_factor > 0.75 - - -def test_near_speed_follow_keeps_some_smoothing_under_high_uncertainty(): - v_ego = 31.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=53.0, v_lead=29.8, radar=False, model_prob=0.96) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=lead, - brake_press_prob=0.85, - ) - - for _ in range(16): - planner.update(sm, make_toggles()) - - assert planner.mpc.filter_time_factor >= 0.24 - - -def test_near_speed_follow_soft_brake_cap_limits_matched_follow_pulse(): - v_ego = 31.4 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=44.0, v_lead=30.1, radar=False, model_prob=0.96) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=lead, - brake_press_prob=0.85, - ) - - for _ in range(16): - planner.update(sm, make_toggles()) - - assert planner.mpc.filter_time_factor >= 0.24 - assert planner.output_a_target >= -0.33 - - -def test_near_speed_follow_soft_brake_cap_covers_slightly_opening_lead(): - v_ego = 29.58 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=44.44, v_lead=30.65, radar=False, model_prob=0.98) - - cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45) - - assert cap is not None - assert cap >= -0.14 - assert cap <= -0.06 - - -def test_near_speed_follow_soft_brake_cap_extends_to_spacious_modest_closing(): - v_ego = 23.69 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=49.48, v_lead=21.64, a_lead=-0.014, radar=False, model_prob=0.998) - - cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45) - - assert cap is not None - assert cap >= -0.33 - assert cap <= -0.22 - - -def test_near_speed_follow_soft_brake_cap_uses_raw_vehicle_speed_when_cluster_runs_high(): - v_ego = 23.0073 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=47.52, v_lead=21.68, a_lead=-0.0646, radar=False, model_prob=0.998), - ) - sm["carState"].vEgoCluster = 23.6931 - sm["starpilotPlan"].maxAcceleration = 0.61 - - for _ in range(6): - planner.update(sm, make_toggles()) - - assert not planner.lead_is_matched_follow_window(sm["radarState"].leadOne, sm["carState"].vEgoCluster, 1.45) - assert planner.output_a_target > -0.35 - assert planner.output_a_target < -0.22 - - -def test_near_speed_follow_soft_brake_cap_rejects_close_gap_even_with_modest_closing(): - v_ego = 37.19 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=34.70, v_lead=35.88, radar=False, model_prob=0.99) - - cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.0) - - assert cap is None - - -def test_follow_control_lead_prefers_active_lead1_for_matched_follow(): - v_ego = 23.3 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6) - planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98) - planner.mpc.source = "lead1" - - follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45) - - assert follow_lead is planner.lead_two - - -def test_follow_control_lead_disables_optional_matched_follow_override(): - v_ego = 23.3 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6) - planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98) - planner.mpc.source = "lead1" - - follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45, allow_optional_far_lead_logic=False) - - assert follow_lead is planner.lead_one - - -def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch(): - v_ego = 27.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99) - - follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45) - - assert follow_lead is planner.lead_one - - -def test_follow_control_lead_requires_real_lead_control_when_optional_logic_disabled(): - v_ego = 27.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99) - - follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45, allow_optional_far_lead_logic=False) - - assert follow_lead is None - - -def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead(): - v_ego = 32.37 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=82.07, v_lead=30.63, a_lead=-0.01, radar=False, model_prob=0.99) - - cap = planner.get_far_lead_brake_cap(lead, v_ego, 1.70) - - assert cap is not None - assert cap > -0.2 - assert cap < -0.05 - - -def test_far_lead_soft_brake_cap_limits_spacious_mild_closing_radar_lead(): - v_ego = 25.61 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=55.1, v_lead=24.11, a_lead=0.22, radar=True, model_prob=0.99) - - cap = planner.get_far_lead_brake_cap(lead, v_ego, 1.13) - - assert cap is not None - assert -0.11 < cap < -0.04 - - -@pytest.mark.parametrize( - "v_lead,a_lead", - [ - (22.9, 0.0), - (24.11, -0.5), - ], -) -def test_far_lead_soft_brake_cap_rejects_urgent_radar_lead(v_lead, a_lead): - v_ego = 25.61 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=55.1, v_lead=v_lead, a_lead=a_lead, radar=True, model_prob=0.99) - - assert planner.get_far_lead_brake_cap(lead, v_ego, 1.13) is None - - def test_experimental_release_state_arms_only_on_falling_edge(): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP) @@ -3248,317 +2877,6 @@ def test_planner_arms_experimental_release_accel_only_on_mode_exit(): assert release_states == [False, True] -def test_matched_follow_transition_target_damps_large_comfort_sign_flip(): - v_ego = 20.3 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=45.6, v_lead=19.19, a_lead=0.0, radar=False, model_prob=0.99) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.12, - output_a_target=-0.40, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed > -0.05 - assert smoothed < 0.12 - - -def test_matched_follow_transition_target_skips_urgent_closure(): - v_ego = 31.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=35.0, v_lead=28.0, a_lead=0.0, radar=False, model_prob=0.99) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.10, - output_a_target=-0.60, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_matched_follow_transition_target_damps_low_speed_tracking_cruise_throttle_jitter(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.08, - output_a_target=0.46, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(0.14, abs=1e-6) - - -def test_route_8bc6_opening_radar_catchup_cap_does_not_stab_out_throttle(): - v_ego = 15.60 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=24.3, v_lead=16.61, a_lead=0.82, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=1.31, - output_a_target=0.32, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert 1.20 < smoothed < 1.31 - - -def test_opening_radar_catchup_smoothing_yields_immediately_if_lead_brakes(): - v_ego = 15.60 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=24.3, v_lead=16.61, a_lead=-0.50, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=1.31, - output_a_target=0.32, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_opening_radar_catchup_smoothing_has_no_pullaway_boundary_jump(): - v_ego = 16.30 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=25.5, v_lead=17.54, a_lead=0.68, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=0.51, - output_a_target=0.87, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed == pytest.approx(0.56, abs=1e-6) - - -def test_route_8bc6_radar_lead_source_handoff_keeps_catchup_transition_smooth(): - v_ego = 24.44 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=37.1, v_lead=24.46, a_lead=0.33, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.15, - prev_output_a_target=0.42, - output_a_target=0.26, - current_source="lead0", - tracking_lead_active=True, - ) - - assert smoothed == pytest.approx(0.37, abs=1e-6) - - -def test_opening_radar_catchup_smoothing_stays_off_inside_requested_headway(): - v_ego = 15.60 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=21.0, v_lead=16.61, a_lead=0.82, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=1.31, - output_a_target=0.32, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_matched_follow_transition_target_skips_low_speed_without_tracking(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.08, - output_a_target=0.46, - current_source="cruise", - tracking_lead_active=False, - ) - - assert smoothed is None - - -def test_matched_follow_transition_target_skips_low_speed_real_braking(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=29.0, v_lead=13.6, a_lead=0.0, radar=False, model_prob=0.999) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.08, - output_a_target=-0.30, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_cruise_tracking_lead_accel_cap_limits_mid_speed_follow_nibble(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.3 <= cap <= 0.5 - - -def test_cruise_tracking_lead_accel_cap_blocks_unresolved_raw_close_lead_burst(): - v_ego = 17.6 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=41.9, v_lead=14.2, a_lead=0.0, radar=True, model_prob=0.99, y_rel=-0.97) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=False, - ) - - assert cap is not None - assert 0.0 <= cap <= 0.05 - - -def test_cruise_tracking_lead_accel_cap_skips_when_lead_clearly_pulls_away(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=35.0, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.1) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead(): - v_ego = 11.4 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=20.5, v_lead=12.4, a_lead=0.46, radar=True, model_prob=1.0, y_rel=0.1) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_cruise_tracking_lead_accel_cap_continuously_limits_spacious_tracking_only_follow(): - v_ego = 18.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=45.0, v_lead=17.4, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.1) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.3 < cap < 0.55 - - -def test_route_8bc6_radar_follow_caps_do_not_flip_between_bypass_and_hold(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=17.35) - route_states = ( - (17.35, 36.2, 16.55, 0.60), - (17.26, 34.8, 17.36, 0.40), - (18.78, 35.2, 18.58, 0.40), - (18.87, 35.0, 19.07, 0.40), - ) - - caps = [] - for v_ego, d_rel, v_lead, a_lead in route_states: - lead = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, - radar=True, model_prob=1.0, y_rel=0.2) - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - assert cap is not None - caps.append(cap) - - assert min(caps) > 0.25 - assert max(caps) < 0.65 - assert max(caps) - min(caps) < 0.35 - - def test_inside_gap_closing_lead_cap_blocks_route_accel_burst(): v_ego = 17.1 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -3605,244 +2923,6 @@ def test_inside_gap_closing_lead_cap_does_not_touch_standstill_departure(): assert planner.get_inside_gap_closing_lead_accel_cap(lead, 0.0, -1.0, 1.25) is None -def test_route_8bc6_post_departure_cruise_cap_uses_continuous_allowance_for_accelerating_radar_lead(): - v_ego = 19.03 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, - ) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.2 < cap < 0.35 - - -def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_radar_follow_outside_cap_window(): - v_ego = 24.108949661254883 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=36.75, v_lead=24.234233856201172, - a_lead=0.3050585687160492, radar=True, model_prob=0.9995405077934265, y_rel=-0.2, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.160530924797058, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelerates_away(): - v_ego = 22.293441772460938 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=36.5, v_lead=22.264942169189453, - a_lead=0.2780209183692932, radar=True, model_prob=0.999357283115387, y_rel=0.35, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.2014657258987427, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_route_8bc6_catchup_cap_continuously_limits_accelerating_lead_when_source_flips_to_lead0(): - v_ego = 24.361867904663086 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=38.5, v_lead=24.46365737915039, - a_lead=0.4625629186630249, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.3549551963806152, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.03 < cap < 0.10 - - -def test_route_8bc6_radar_matched_follow_catchup_cap_continuously_limits_mild_pullaway_after_lead_lock(): - v_ego = 23.96 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.6, v_lead=24.23, a_lead=0.16, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.16, - current_source="lead0", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) - assert cap is not None - assert 0.1 < cap < 0.25 - - -def test_route_8bc6_radar_matched_follow_catchup_cap_keeps_cap_when_pullaway_is_not_confirmed(): - v_ego = 23.96 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.6, v_lead=24.23, a_lead=0.08, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.16, - current_source="lead0", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) - assert cap is not None - - -def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave(): - v_ego = 22.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=41.558, v_lead=22.3, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) - assert cap is None - - -def test_route_8bc6_radar_matched_follow_catchup_cap_holds_small_cap_for_slower_lead_on_cruise(): - v_ego = 22.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=39.108, v_lead=21.2, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) - assert cap == pytest.approx(0.04, abs=1e-6) - - -def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_catchup_cap(): - v_ego = 16.4023914337 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=25.75, v_lead=15.9339208603, - a_lead=0.1164954603, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap_without_latch = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - cap_with_latch = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap_without_latch == pytest.approx(0.03, abs=1e-6) - assert cap_with_latch is None - - -@pytest.mark.parametrize("v_ego,d_rel,v_lead,a_lead", [ - (8.05, 14.75, 8.56, 0.25), - (10.65, 14.35, 11.59, 1.38), - (8.34, 11.25, 8.33, 0.97), -]) -def test_route_8bc6_rolling_departure_arms_settle_latch(v_ego, d_rel, v_lead, a_lead): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=d_rel, v_lead=v_lead, - a_lead=a_lead, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap is None - assert planner.post_departure_follow_settle_until > time.monotonic() - - -def test_route_8bc6_radar_catchup_cap_enters_continuously_above_min_speed(): - v_ego = 8.69 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=11.25, v_lead=8.73, - a_lead=1.10, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert planner.post_departure_follow_settle_until == 0.0 - assert 1.20 < cap < 1.30 - - def test_rolling_departure_settle_latch_stays_active_through_headway_hysteresis(): v_ego = 10.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -3907,251 +2987,6 @@ def test_rolling_departure_settle_latch_does_not_arm_without_safe_departure(v_eg assert planner.post_departure_follow_settle_until == 0.0 -def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_cruise_cap(): - v_ego = 19.2975330353 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=28.9, v_lead=19.6019687653, - a_lead=0.1241656989, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap_without_latch = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - cap_with_latch = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap_without_latch is not None - assert 0.10 < cap_without_latch < 0.25 - assert cap_with_latch is None - - -def test_post_departure_settle_latch_does_not_bypass_when_lead_brakes_again(): - v_ego = 19.192998192 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - lead = make_lead( - status=True, d_rel=30.5, v_lead=19.147550216, - a_lead=-0.16, radar=True, model_prob=1.0, y_rel=0.2, - ) - - catchup_cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - cruise_cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - assert catchup_cap is not None - assert cruise_cap is not None - assert planner.post_departure_follow_settle_until == 0.0 - - -def test_post_departure_settle_latch_clears_once_follow_has_settled_near_target(): - v_ego = 16.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - lead = make_lead( - status=True, d_rel=20.48, v_lead=16.25, - a_lead=0.18, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap is not None - assert planner.post_departure_follow_settle_until == 0.0 - - -def test_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again(): - v_ego = 19.03 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.85, v_lead=18.90, a_lead=-0.35, radar=True, model_prob=1.0, y_rel=-0.30, - ) - - catchup_cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) - cruise_cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert catchup_cap is not None - assert cruise_cap is not None - - -def test_cruise_tracking_lead_accel_transition_target_damps_mid_speed_reacquisition_burst(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=20.0, v_lead=17.4, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.18) - - smoothed = planner.get_cruise_tracking_lead_accel_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.10, - output_a_target=1.10, - current_source="cruise", - ) - - assert smoothed is not None - assert smoothed == pytest.approx(0.23, abs=1e-2) - - -def test_cruise_tracking_lead_accel_transition_target_skips_clear_pullaway(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=34.0, v_lead=19.2, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) - - smoothed = planner.get_cruise_tracking_lead_accel_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.10, - output_a_target=1.10, - current_source="cruise", - ) - - assert smoothed is None - - -def test_mild_follow_zero_cross_guard_coasts_on_cruise_sign_flip(): - v_ego = 19.8 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=42.0, v_lead=19.0, a_lead=-0.15, radar=False, model_prob=0.99, y_rel=0.10) - - guarded = planner.get_mild_follow_zero_cross_guard_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.18, - output_a_target=-0.12, - current_source="cruise", - tracking_lead_active=True, - ) - - assert guarded == pytest.approx(0.0, abs=1e-6) - - -def test_mild_follow_zero_cross_guard_coasts_on_near_duplicate_sign_flip(): - v_ego = 24.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=51.0, v_lead=21.7, a_lead=-0.25, radar=False, model_prob=0.99, y_rel=0.08) - - guarded = planner.get_mild_follow_zero_cross_guard_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.33, - output_a_target=-0.13, - current_source="lead1", - tracking_lead_active=True, - ) - - assert guarded == pytest.approx(0.0, abs=1e-6) - - -def test_mild_follow_zero_cross_guard_skips_urgent_close_follow(): - v_ego = 17.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=16.0, v_lead=12.0, a_lead=-0.45, radar=False, model_prob=0.99, y_rel=0.05) - - guarded = planner.get_mild_follow_zero_cross_guard_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.22, - output_a_target=-0.28, - current_source="cruise", - tracking_lead_active=True, - ) - - assert guarded is None - - -def test_post_097_follow_logic_is_always_active(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) - - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.8, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=lead, - ) - sm["starpilotPlan"].vCruise = v_ego + 8.0 - - calls = [] - - def record(name, return_value=None): - def _inner(self, *args, **kwargs): - calls.append(name) - return return_value - return types.MethodType(_inner, planner) - - planner.get_follow_control_lead = record("get_follow_control_lead", lead) - planner.get_lead_catchup_accel_cap = record("get_lead_catchup_accel_cap", None) - planner.get_tracked_vision_model_brake_floor = record("get_tracked_vision_model_brake_floor", None) - planner.get_low_speed_follow_transition_brake_cap = record("get_low_speed_follow_transition_brake_cap", None) - planner.get_tracked_vision_model_brake_cap = record("get_tracked_vision_model_brake_cap", None) - planner.get_cruise_tracking_lead_accel_cap = record("get_cruise_tracking_lead_accel_cap", None) - planner.get_cruise_tracking_lead_accel_transition_target = record("get_cruise_tracking_lead_accel_transition_target", None) - - planner.update(sm, make_toggles()) - - expected_calls = { - "get_lead_catchup_accel_cap", - "get_follow_control_lead", - "get_tracked_vision_model_brake_floor", - "get_low_speed_follow_transition_brake_cap", - "get_tracked_vision_model_brake_cap", - "get_cruise_tracking_lead_accel_cap", - "get_cruise_tracking_lead_accel_transition_target", - } - - assert expected_calls.issubset(set(calls)) - - def test_near_duplicate_lead_source_hysteresis_prefers_previous_source(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -4453,351 +3288,3 @@ def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads(): assert lead_0_bias == 0.0 assert lead_1_bias == 0.0 - - -def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_offset_candidate(): - 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=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) - lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) - 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 - 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) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) - lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) - 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 - - initial = planner.get_duplicate_vision_comfort_lead(v_ego) - - lead_one.yRel = 1.25 - lead_two.yRel = 0.05 - lead_one.dRel = 44.4 - lead_two.dRel = 44.5 - latched = planner.get_duplicate_vision_comfort_lead(v_ego) - - assert initial is lead_one - assert latched is lead_one - assert planner.duplicate_vision_comfort_lead_source == "lead0" - - -def test_duplicate_vision_comfort_lead_resets_when_duplicate_cluster_clears(): - 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=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) - lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) - 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 - - assert planner.get_duplicate_vision_comfort_lead(v_ego) is lead_one - - lead_two.dRel = 49.5 - assert planner.get_duplicate_vision_comfort_lead(v_ego) is None - assert planner.duplicate_vision_comfort_lead_source is None - - -def test_nonurgent_duplicate_vision_follow_keeps_comfort_smoothing(): - 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=35.0, v_lead=20.5, a_lead=-0.05, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=35.2, v_lead=20.52, a_lead=-0.04, 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 - - assert planner.is_nonurgent_duplicate_vision_follow(v_ego, 1.45) - - -@pytest.mark.parametrize("d_rel,v_lead,a_lead", [ - (24.0, 20.0, -0.05), # Low TTC. - (22.0, 23.0, -0.05), # Headway materially below the requested gap. - (35.0, 20.5, -0.50), # The lead is braking. -]) -def test_duplicate_vision_follow_preserves_urgent_panic_bypass(d_rel, v_lead, a_lead): - 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=d_rel, v_lead=v_lead, a_lead=a_lead, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=d_rel + 0.2, v_lead=v_lead + 0.02, a_lead=a_lead, 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 - - assert not planner.is_nonurgent_duplicate_vision_follow(v_ego, 1.45) - - -def test_radar_duplicates_preserve_panic_bypass(): - 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=35.0, v_lead=20.5, a_lead=-0.05, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=35.0, v_lead=20.5, a_lead=-0.05, radar=True, model_prob=1.0) - lead_one.radarTrackId = 17 - lead_two.radarTrackId = 17 - 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 - - assert not planner.is_nonurgent_duplicate_vision_follow(v_ego, 1.45) - - -def test_near_duplicate_lead_transition_target_damps_same_source_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=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) - lead_one.vRel = -0.95 - lead_two.vRel = -1.00 - 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=-1.10, - output_a_target=0.13, - current_source="lead1", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(-0.92, abs=1e-6) - - -def test_near_duplicate_lead_transition_target_damps_tracking_cruise_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=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) - lead_one.vRel = -0.95 - lead_two.vRel = -1.00 - 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=-1.10, - output_a_target=0.13, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(-0.92, abs=1e-6) - - -def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_handoff(): - v_ego = 17.61 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=41.9, v_lead=16.85, a_lead=0.0, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=41.9, v_lead=16.85, a_lead=0.0, radar=True, model_prob=1.0) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_one, - v_ego, - 1.45, - prev_output_a_target=0.89, - output_a_target=0.05, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - 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) - 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) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998) - lead_two = make_lead(status=True, d_rel=19.25, v_lead=20.02, a_lead=-0.41, radar=False, model_prob=0.996) - 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_duplicate_slow_lead_brake_hold_target( - lead_one, - v_ego, - 1.0, - prev_output_a_target=-3.50, - output_a_target=0.0, - current_source="lead0", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(-3.28, abs=1e-6) - - -def test_duplicate_slow_lead_brake_hold_skips_distinct_leads(): - v_ego = 24.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=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998) - lead_two = make_lead(status=True, d_rel=24.0, v_lead=21.5, a_lead=-0.10, radar=False, model_prob=0.996) - 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_duplicate_slow_lead_brake_hold_target( - lead_one, - v_ego, - 1.0, - prev_output_a_target=-3.50, - output_a_target=0.0, - current_source="lead0", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_near_duplicate_lead_transition_target_skips_plain_cruise_without_tracking(): - 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=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) - lead_one.vRel = -0.95 - lead_two.vRel = -1.00 - 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=-1.10, - output_a_target=0.13, - current_source="cruise", - tracking_lead_active=False, - ) - - assert smoothed is None