diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 743e0b65f..47ee34836 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -398,7 +398,8 @@ class LongitudinalMpc: def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0, - uncertainty=0.0, accel_reengage=False, panic_bypass=False): + uncertainty=0.0, accel_reengage=False, panic_bypass=False, + filter_time_factor_floor=0.0): # Update parameters based on current speed with interpolation for smooth scaling speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph @@ -446,6 +447,8 @@ class LongitudinalMpc: # Hard bypass of smoothing when approaching fast or magnitude trips if panic_bypass: tgt_factor = 0.0 + else: + tgt_factor = max(tgt_factor, float(filter_time_factor_floor)) # Slew-limit changes to avoid step-wise filter jumps max_step = self.slew_per_sec * self.dt diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1ebdc19cb..2a7517266 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -118,6 +118,23 @@ LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15 # Uncertainty-based filter disable thresholds UNCERT_SLOPE_TRIG = 0.12 # per second UNCERT_MAG_TRIG = 0.50 +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 +STEADY_FOLLOW_SMOOTHING_MIN_SPEED = 22.0 +STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED = 0.15 +STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED = 1.8 +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_DECEL = 0.18 +STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL = 0.32 # Lookup table for turns _A_TOTAL_MAX_V = [3.5, 3.5, 3.2] @@ -286,6 +303,7 @@ class LongitudinalPlanner: # Uncertainty slope tracking self._uncert_last = 0.0 self._uncert_last_t = None + self._panic_bypass_log_t = 0.0 self.effective_t_follow = None self.vision_low_speed_stop_hold_until = 0.0 self.vision_lead_approach_confirm_t = 0.0 @@ -638,6 +656,37 @@ class LongitudinalPlanner: gap_factor = float(np.clip(max(gap_error, 0.0) / max(gap_buffer, 0.1), 0.0, 1.0)) return float(np.interp(gap_factor, [0.0, 1.0], [near_cap, edge_cap])) + def get_matched_follow_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 + + closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) + if not (STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED): + return None + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE: + 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 < STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB: + return None + + 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 None + if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: + return None + + cap_decel = float(np.interp( + closing_speed, + [STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED, STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED], + [STEADY_FOLLOW_BRAKE_CAP_MIN_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 + @staticmethod def raw_close_lead_needs_control(lead, v_ego): if lead is None or not lead.status: @@ -720,8 +769,10 @@ class LongitudinalPlanner: if not self.allow_throttle: clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) - clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) - accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) + # Hold the output cap to the physical coasting limit until throttle is + # allowed again. Relaxing back toward positive accel while the gate is + # still closed can stall downhill coastdown well above the target speed. + accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast) no_throttle_output_max = accel_limits_turns[1] if force_slow_decel: @@ -840,15 +891,57 @@ class LongitudinalPlanner: self._uncert_last = uncertainty self._uncert_last_t = now_t - closing_fast = lead_one_active and (v_ego - self.lead_one.vLead) > 0.5 - # Trigger if either slope is high or magnitude is high; require a valid lead and closing - panic_bypass = closing_fast and (uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG) + panic_close_window = False + closing_fast = False + desired_gap = None + closing_speed = 0.0 + if lead_one_active: + desired_gap = float(desired_follow_distance(v_ego, self.lead_one.vLead, effective_t_follow)) + close_gap_window = max(UNCERT_PANIC_MAX_GAP_BUFFER_MIN, + UNCERT_PANIC_MAX_GAP_BUFFER_GAIN * float(v_ego)) + panic_close_window = float(self.lead_one.dRel) <= desired_gap + close_gap_window + closing_speed = max(0.0, v_ego - self.lead_one.vLead) + closing_fast = closing_speed >= max( + UNCERT_PANIC_MIN_CLOSING_SPEED, + UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN * float(v_ego), + ) + + # Only bypass lead smoothing when we're closing meaningfully and already + # near the follow window. Far or nearly pace-matched leads should stay on + # the smoothed path so the planner doesn't flip-flop between accel and brake. + panic_bypass = panic_close_window and closing_fast and ( + uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG + ) + + steady_follow_filter_floor = 0.0 + if lead_one_active and desired_gap is not None and not panic_bypass: + lead_brake = max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0))) + lead_prob = float(getattr(self.lead_one, "modelProb", 1.0 if bool(getattr(self.lead_one, "radar", False)) else 0.0)) + actual_headway = float(self.lead_one.dRel) / max(float(v_ego), 1e-3) + matched_follow_window = ( + v_ego >= STEADY_FOLLOW_SMOOTHING_MIN_SPEED and + STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED and + actual_headway >= max(STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY, + effective_t_follow - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET) and + actual_headway <= effective_t_follow + STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET and + lead_brake <= STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE and + (bool(getattr(self.lead_one, "radar", False)) or lead_prob >= STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB) + ) + if matched_follow_window: + steady_follow_filter_floor = STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR if panic_bypass: - try: - cloudlog.error(f"LON_SLOPE; slope={uncert_slope:.3f}/s; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f}; v_rel={(v_ego - self.lead_one.vLead) if lead_one_active else 0.0:.2f}; lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}; trigger=True") - except Exception: - pass + if now_t - self._panic_bypass_log_t > 5.0: + self._panic_bypass_log_t = now_t + try: + cloudlog.warning( + "LON_SLOPE close bypass: " + f"slope={uncert_slope:.3f}/s uncertainty={uncertainty:.3f} " + f"v_ego={v_ego:.2f} v_rel={(v_ego - self.lead_one.vLead) if lead_one_active else 0.0:.2f} " + f"lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}" + ) + except Exception: + pass personality = get_longitudinal_personality(sm) @@ -860,7 +953,8 @@ class LongitudinalPlanner: v_ego=v_ego, lead_dist=self.lead_dist_f if lead_one_active and self.lead_dist_f is not None else 50.0, uncertainty=uncertainty, - panic_bypass=panic_bypass) + panic_bypass=panic_bypass, + filter_time_factor_floor=steady_follow_filter_floor) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) # After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim @@ -1040,6 +1134,12 @@ class LongitudinalPlanner: if vision_brake_cap_active: output_accel_min = min(output_accel_min, vision_cap_accel_min) + if lead_one_active and not panic_bypass: + matched_follow_brake_cap = self.get_matched_follow_brake_cap(self.lead_one, v_ego, sm['starpilotPlan'].tFollow) + 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) + 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_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index f1efe75bc..bf23c5348 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -9,7 +9,7 @@ from cereal import log from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -29,7 +29,7 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead return lead -def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0): +def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0): model = log.ModelDataV2.new_message() t_idxs = ModelConstants.T_IDXS @@ -49,6 +49,7 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0): model.acceleration.t = [float(t) for t in t_idxs] model.meta.disengagePredictions.gasPressProbs = [float(gas_press_prob)] * 6 + model.meta.disengagePredictions.brakePressProbs = [float(brake_press_prob)] * 6 model.action.desiredAcceleration = desired_accel model.action.shouldStop = False return model @@ -56,7 +57,7 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0): def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True, tracking_lead: bool = False, lead_one=None, lead_two=None, - gas_press_prob: float = 1.0, disable_throttle: bool = False): + gas_press_prob: float = 1.0, brake_press_prob: float = 0.0, disable_throttle: bool = False): return { "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]), "carState": SimpleNamespace( @@ -72,7 +73,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta forceDecel=False, ), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), - "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob), + "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob, brake_press_prob=brake_press_prob), "radarState": SimpleNamespace( leadOne=lead_one if lead_one is not None else make_lead(status=False), leadTwo=lead_two if lead_two is not None else make_lead(status=False), @@ -912,3 +913,85 @@ def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): planner.update(sm, toggles) assert planner.model_allow_throttle assert planner.allow_throttle + + +def test_no_throttle_cap_stays_at_coast_limit_until_throttle_returns(): + v_ego = 8.5 + + 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=-3.0, experimental_mode=False, gas_press_prob=0.0) + sm["carControl"].orientationNED = [0.0, 0.1, 0.0] + toggles = make_toggles() + + planner.update(sm, toggles) + + accel_coast = max(get_vehicle_min_accel(CP, v_ego), get_coast_accel(sm["carControl"].orientationNED[1])) + + assert not planner.allow_throttle + assert planner.output_a_target == pytest.approx(accel_coast, abs=1e-3) + + +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 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index cc3d6cc3b..d40039940 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -465,7 +465,8 @@ CONFIGS = [ ), ProcessConfig( proc_name="plannerd", - pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"], + pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState", + "starpilotCarState", "starpilotPlan"], subs=["longitudinalPlan", "driverAssistance"], ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], init_callback=get_car_params_callback, diff --git a/selfdrive/test/process_replay/test_process_replay_starpilot.py b/selfdrive/test/process_replay/test_process_replay_starpilot.py new file mode 100644 index 000000000..f84d10d93 --- /dev/null +++ b/selfdrive/test/process_replay/test_process_replay_starpilot.py @@ -0,0 +1,12 @@ +import os + +os.environ["DEBUG"] = "0" + +from openpilot.selfdrive.test.process_replay.process_replay import get_process_config + + +def test_plannerd_replay_includes_starpilot_inputs(): + cfg = get_process_config("plannerd") + + assert "starpilotPlan" in cfg.pubs + assert "starpilotCarState" in cfg.pubs diff --git a/tools/longitudinal/score_route_longitudinal.py b/tools/longitudinal/score_route_longitudinal.py new file mode 100644 index 000000000..489452054 --- /dev/null +++ b/tools/longitudinal/score_route_longitudinal.py @@ -0,0 +1,441 @@ +#!/usr/bin/env python3 +import argparse +import json +import math +import os +from collections import defaultdict +from dataclasses import asdict, dataclass +from types import SimpleNamespace + +import numpy as np + +os.environ["DEBUG"] = "0" +os.environ.setdefault("FILEREADER_CACHE", "1") + +from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.lib import longitudinal_planner as longitudinal_planner_mod +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE +from openpilot.tools.lib.logreader import LogReader + + +for level_name in ("info", "warning", "error", "exception", "event"): + if hasattr(cloudlog, level_name): + setattr(cloudlog, level_name, lambda *args, **kwargs: None) + + +NEAR_SPEED_HIGHWAY_MIN_SPEED = 22.0 +NEAR_SPEED_LEAD_DELTA = 1.5 +BRAKE_STAB_START = -0.35 +BRAKE_STAB_END = -0.10 +BRAKE_STAB_MAX_DURATION = 1.0 +VISION_SLOW_LEAD_MIN_DELTA = 3.0 +VISION_SLOW_LEAD_MIN_DIST = 35.0 +VISION_SLOW_LEAD_MAX_DIST = 120.0 +VISION_SLOW_LEAD_MIN_MODEL_PROB = 0.9 +SLOW_LEAD_DECEL_TRIGGER = -0.2 +REQUIRED_SERVICES = { + "carControl", + "carState", + "controlsState", + "liveParameters", + "radarState", + "selfdriveState", + "starpilotPlan", +} + + +@dataclass +class RouteMetrics: + route: str + replayed_samples: int = 0 + near_speed_samples: int = 0 + highway_follow_samples: int = 0 + vision_slow_lead_samples: int = 0 + brake_stab_count: int = 0 + false_far_lead_brake_count: int = 0 + highway_accel_std: float = 0.0 + highway_rms_jerk: float = 0.0 + near_speed_accel_std: float = 0.0 + near_speed_rms_jerk: float = 0.0 + slower_lead_event_count: int = 0 + slower_lead_mean_response_delay: float | None = None + slower_lead_worst_response_delay: float | None = None + slower_lead_min_a_target: float | None = None + slower_lead_min_ttc: float | None = None + slower_lead_min_gap: float | None = None + + +def configure_planner_profile(profile: str) -> None: + if profile == "current": + return + + if profile == "legacy_panic_bypass": + longitudinal_planner_mod.UNCERT_PANIC_MIN_CLOSING_SPEED = 0.5 + longitudinal_planner_mod.UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN = 0.0 + longitudinal_planner_mod.UNCERT_PANIC_MAX_GAP_BUFFER_MIN = 1e6 + longitudinal_planner_mod.UNCERT_PANIC_MAX_GAP_BUFFER_GAIN = 0.0 + return + + raise ValueError(f"Unsupported planner profile: {profile}") + + +def default_toggles() -> SimpleNamespace: + return SimpleNamespace( + taco_tune=False, + classic_model=False, + tinygrad_model=True, + model_version="v11", + stop_distance=float(STOP_DISTANCE), + vEgoStopping=0.5, + ) + + +def parse_toggles(serialized: str, previous: SimpleNamespace | None) -> SimpleNamespace: + if not serialized: + return previous if previous is not None else default_toggles() + + payload = vars(default_toggles()) + try: + payload.update(json.loads(serialized)) + except Exception: + return previous if previous is not None else default_toggles() + return SimpleNamespace(**payload) + + +def finite_min(current: float | None, value: float | None) -> float | None: + if value is None or not math.isfinite(value): + return current + if current is None: + return value + return min(current, value) + + +def finite_mean(values: list[float]) -> float | None: + if not values: + return None + return float(np.mean(values)) + + +def finite_max(values: list[float]) -> float | None: + if not values: + return None + return float(np.max(values)) + + +def is_near_speed_follow(v_ego: float, lead_status: bool, v_lead: float) -> bool: + return lead_status and v_ego > NEAR_SPEED_HIGHWAY_MIN_SPEED and abs(v_ego - v_lead) <= NEAR_SPEED_LEAD_DELTA + + +def is_highway_follow(v_ego: float, lead_status: bool) -> bool: + return lead_status and v_ego > NEAR_SPEED_HIGHWAY_MIN_SPEED + + +def is_vision_slow_lead(v_ego: float, lead_status: bool, lead_radar: bool, lead_prob: float, lead_dist: float, v_lead: float) -> bool: + return ( + lead_status and + not lead_radar and + lead_prob >= VISION_SLOW_LEAD_MIN_MODEL_PROB and + (v_ego - v_lead) >= VISION_SLOW_LEAD_MIN_DELTA and + VISION_SLOW_LEAD_MIN_DIST <= lead_dist <= VISION_SLOW_LEAD_MAX_DIST + ) + + +def score_route(route: str, capture_events: bool = False, max_events: int = 200) -> tuple[RouteMetrics, list[dict]]: + metrics = RouteMetrics(route=route) + planner = None + toggles = default_toggles() + state: dict[str, object] = {} + events: list[dict] = [] + + highway_accels: list[float] = [] + highway_jerks: list[float] = [] + near_speed_accels: list[float] = [] + near_speed_jerks: list[float] = [] + slower_response_delays: list[float] = [] + active_slow_event: dict | None = None + active_brake_stab: dict | None = None + active_false_far_brake: dict | None = None + prev_plan_t: int | None = None + prev_a_target: float | None = None + + for msg in LogReader(route, sort_by_time=True): + which = msg.which() + if which == "carParams" and planner is None: + planner = LongitudinalPlanner(msg.carParams) + continue + + if which not in REQUIRED_SERVICES and which != "modelV2": + continue + + state[which] = getattr(msg, which) + if which == "starpilotPlan": + toggles = parse_toggles(state["starpilotPlan"].starpilotToggles, toggles) + + if which != "modelV2" or planner is None or not REQUIRED_SERVICES.issubset(state): + continue + + planner.update(state, toggles) + metrics.replayed_samples += 1 + + car_state = state["carState"] + radar_state = state["radarState"] + lead = radar_state.leadOne + v_ego = float(car_state.vEgo) + lead_status = bool(lead.status) + lead_dist = float(lead.dRel) if lead_status else float("inf") + v_lead = float(lead.vLead) if lead_status else v_ego + lead_prob = float(getattr(lead, "modelProb", 0.0)) + lead_radar = bool(getattr(lead, "radar", False)) + a_target = float(planner.output_a_target) + mono_time = int(msg.logMonoTime) + + dt = None if prev_plan_t is None else max((mono_time - prev_plan_t) / 1e9, 1e-3) + jerk = None if dt is None or prev_a_target is None else (a_target - prev_a_target) / dt + + near_speed_follow = is_near_speed_follow(v_ego, lead_status, v_lead) + highway_follow = is_highway_follow(v_ego, lead_status) + vision_slow_lead = is_vision_slow_lead(v_ego, lead_status, lead_radar, lead_prob, lead_dist, v_lead) + + if near_speed_follow: + metrics.near_speed_samples += 1 + near_speed_accels.append(a_target) + if jerk is not None: + near_speed_jerks.append(jerk) + if highway_follow: + metrics.highway_follow_samples += 1 + highway_accels.append(a_target) + if jerk is not None: + highway_jerks.append(jerk) + if vision_slow_lead: + metrics.vision_slow_lead_samples += 1 + + if near_speed_follow: + if active_brake_stab is None and a_target < BRAKE_STAB_START: + active_brake_stab = { + "start_t": mono_time / 1e9, + "start_mono_time": mono_time, + "min_a_target": a_target, + "start_lead_dist": lead_dist, + "start_v_ego": v_ego, + "start_v_lead": v_lead, + "source": str(planner.mpc.source), + "filter_time_factor": float(getattr(planner.mpc, "filter_time_factor", 1.0)), + } + elif active_brake_stab is not None and a_target > BRAKE_STAB_END: + duration = mono_time / 1e9 - active_brake_stab["start_t"] + if duration < BRAKE_STAB_MAX_DURATION: + metrics.brake_stab_count += 1 + if capture_events and len(events) < max_events: + events.append({ + "route": route, + "kind": "brake_stab", + "start_mono_time": active_brake_stab["start_mono_time"], + "duration": duration, + "min_a_target": active_brake_stab["min_a_target"], + "start_lead_dist": active_brake_stab["start_lead_dist"], + "start_v_ego": active_brake_stab["start_v_ego"], + "start_v_lead": active_brake_stab["start_v_lead"], + "source": active_brake_stab["source"], + "filter_time_factor": active_brake_stab["filter_time_factor"], + }) + active_brake_stab = None + elif active_brake_stab is not None: + active_brake_stab["min_a_target"] = min(active_brake_stab["min_a_target"], a_target) + else: + active_brake_stab = None + + lead_source_active = str(planner.mpc.source) in ("lead0", "lead1") or bool(getattr(state["starpilotPlan"], "trackingLead", False)) + false_far_brake = ( + highway_follow and + lead_source_active and + lead_status and + not lead_radar and + lead_dist > 80.0 and + 0.1 < (v_ego - v_lead) < 2.0 and + a_target < -0.2 + ) + if false_far_brake: + if active_false_far_brake is None: + active_false_far_brake = { + "start_mono_time": mono_time, + "start_t": mono_time / 1e9, + "min_a_target": a_target, + "min_lead_dist": lead_dist, + "max_closing_speed": max(0.0, v_ego - v_lead), + "start_v_ego": v_ego, + "start_v_lead": v_lead, + "lead_prob": lead_prob, + "source": str(planner.mpc.source), + "filter_time_factor": float(getattr(planner.mpc, "filter_time_factor", 1.0)), + } + else: + active_false_far_brake["min_a_target"] = min(active_false_far_brake["min_a_target"], a_target) + active_false_far_brake["min_lead_dist"] = min(active_false_far_brake["min_lead_dist"], lead_dist) + active_false_far_brake["max_closing_speed"] = max(active_false_far_brake["max_closing_speed"], max(0.0, v_ego - v_lead)) + elif active_false_far_brake is not None: + if capture_events and len(events) < max_events: + events.append({ + "route": route, + "kind": "false_far_lead_brake", + "start_mono_time": active_false_far_brake["start_mono_time"], + "duration": mono_time / 1e9 - active_false_far_brake["start_t"], + "min_a_target": active_false_far_brake["min_a_target"], + "min_lead_dist": active_false_far_brake["min_lead_dist"], + "max_closing_speed": active_false_far_brake["max_closing_speed"], + "start_v_ego": active_false_far_brake["start_v_ego"], + "start_v_lead": active_false_far_brake["start_v_lead"], + "lead_prob": active_false_far_brake["lead_prob"], + "source": active_false_far_brake["source"], + "filter_time_factor": active_false_far_brake["filter_time_factor"], + }) + metrics.false_far_lead_brake_count += 1 + active_false_far_brake = None + + if vision_slow_lead: + ttc = lead_dist / max(v_ego - v_lead, 0.1) + if active_slow_event is None: + active_slow_event = { + "start_t": mono_time / 1e9, + "response_delay": None, + "min_a_target": a_target, + "min_ttc": ttc, + "min_gap": lead_dist, + } + if active_slow_event["response_delay"] is None and a_target <= SLOW_LEAD_DECEL_TRIGGER: + active_slow_event["response_delay"] = mono_time / 1e9 - active_slow_event["start_t"] + active_slow_event["min_a_target"] = min(active_slow_event["min_a_target"], a_target) + active_slow_event["min_ttc"] = min(active_slow_event["min_ttc"], ttc) + active_slow_event["min_gap"] = min(active_slow_event["min_gap"], lead_dist) + elif active_slow_event is not None: + metrics.slower_lead_event_count += 1 + if active_slow_event["response_delay"] is not None: + slower_response_delays.append(active_slow_event["response_delay"]) + metrics.slower_lead_min_a_target = finite_min(metrics.slower_lead_min_a_target, active_slow_event["min_a_target"]) + metrics.slower_lead_min_ttc = finite_min(metrics.slower_lead_min_ttc, active_slow_event["min_ttc"]) + metrics.slower_lead_min_gap = finite_min(metrics.slower_lead_min_gap, active_slow_event["min_gap"]) + active_slow_event = None + + prev_plan_t = mono_time + prev_a_target = a_target + + if active_slow_event is not None: + metrics.slower_lead_event_count += 1 + if active_slow_event["response_delay"] is not None: + slower_response_delays.append(active_slow_event["response_delay"]) + metrics.slower_lead_min_a_target = finite_min(metrics.slower_lead_min_a_target, active_slow_event["min_a_target"]) + metrics.slower_lead_min_ttc = finite_min(metrics.slower_lead_min_ttc, active_slow_event["min_ttc"]) + metrics.slower_lead_min_gap = finite_min(metrics.slower_lead_min_gap, active_slow_event["min_gap"]) + if active_false_far_brake is not None and capture_events and len(events) < max_events: + events.append({ + "route": route, + "kind": "false_far_lead_brake", + "start_mono_time": active_false_far_brake["start_mono_time"], + "duration": prev_plan_t / 1e9 - active_false_far_brake["start_t"] if prev_plan_t is not None else 0.0, + "min_a_target": active_false_far_brake["min_a_target"], + "min_lead_dist": active_false_far_brake["min_lead_dist"], + "max_closing_speed": active_false_far_brake["max_closing_speed"], + "start_v_ego": active_false_far_brake["start_v_ego"], + "start_v_lead": active_false_far_brake["start_v_lead"], + "lead_prob": active_false_far_brake["lead_prob"], + "source": active_false_far_brake["source"], + "filter_time_factor": active_false_far_brake["filter_time_factor"], + }) + if active_false_far_brake is not None: + metrics.false_far_lead_brake_count += 1 + + if highway_accels: + metrics.highway_accel_std = float(np.std(highway_accels)) + if highway_jerks: + metrics.highway_rms_jerk = float(np.sqrt(np.mean(np.square(highway_jerks)))) + if near_speed_accels: + metrics.near_speed_accel_std = float(np.std(near_speed_accels)) + if near_speed_jerks: + metrics.near_speed_rms_jerk = float(np.sqrt(np.mean(np.square(near_speed_jerks)))) + + metrics.slower_lead_mean_response_delay = finite_mean(slower_response_delays) + metrics.slower_lead_worst_response_delay = finite_max(slower_response_delays) + return metrics, events + + +def route_weight(route: str, priority_routes: set[str], priority_weight: float, normal_count: int, priority_count: int) -> float: + if priority_count == 0 or normal_count == 0: + return 1.0 / max(priority_count + normal_count, 1) + if route in priority_routes and priority_count > 0: + return priority_weight / priority_count + if route not in priority_routes and normal_count > 0: + return (1.0 - priority_weight) / normal_count + return 0.0 + + +def aggregate_summary(results: list[RouteMetrics], priority_routes: set[str], priority_weight: float) -> dict: + priority_count = sum(1 for result in results if result.route in priority_routes) + normal_count = len(results) - priority_count + weighted = defaultdict(float) + for result in results: + weight = route_weight(result.route, priority_routes, priority_weight, normal_count, priority_count) + weighted["route_count"] += weight + weighted["brake_stab_count"] += weight * result.brake_stab_count + weighted["false_far_lead_brake_count"] += weight * result.false_far_lead_brake_count + weighted["highway_accel_std"] += weight * result.highway_accel_std + weighted["highway_rms_jerk"] += weight * result.highway_rms_jerk + weighted["near_speed_accel_std"] += weight * result.near_speed_accel_std + weighted["near_speed_rms_jerk"] += weight * result.near_speed_rms_jerk + if result.slower_lead_mean_response_delay is not None: + weighted["slower_lead_mean_response_delay"] += weight * result.slower_lead_mean_response_delay + if result.slower_lead_worst_response_delay is not None: + weighted["slower_lead_worst_response_delay"] += weight * result.slower_lead_worst_response_delay + return dict(weighted) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Replay the longitudinal planner directly on local route logs and score planner behavior. Route identifiers are runtime-only inputs." + ) + parser.add_argument("routes", nargs="+", help="Route or segment identifiers to replay") + parser.add_argument("--priority-route", action="append", default=[], + help="Route identifier to weight in the priority subset") + parser.add_argument("--priority-weight", type=float, default=0.60, + help="Total weight assigned to the priority subset") + parser.add_argument("--planner-profile", choices=("current", "legacy_panic_bypass"), default="current", + help="Planner behavior profile used for local comparison runs") + parser.add_argument("--json-out", type=str, + help="Optional local output path for JSON results") + parser.add_argument("--events-out", type=str, + help="Optional local output path for captured debug events") + parser.add_argument("--max-events-per-route", type=int, default=200, + help="Maximum number of debug events to capture per route") + return parser.parse_args() + + +def main() -> None: + args = parse_args() + configure_planner_profile(args.planner_profile) + + scored = [score_route(route, capture_events=bool(args.events_out), max_events=args.max_events_per_route) for route in args.routes] + results = [result for result, _ in scored] + events = {route: route_events for (result, route_events), route in zip(scored, args.routes)} + payload = { + "summary": aggregate_summary(results, set(args.priority_route), args.priority_weight), + "routes": [asdict(result) for result in results], + } + + if args.json_out: + out_dir = os.path.dirname(args.json_out) + if out_dir: + os.makedirs(out_dir, exist_ok=True) + with open(args.json_out, "w", encoding="utf-8") as f: + json.dump(payload, f, indent=2, sort_keys=True) + + if args.events_out: + out_dir = os.path.dirname(args.events_out) + if out_dir: + os.makedirs(out_dir, exist_ok=True) + with open(args.events_out, "w", encoding="utf-8") as f: + json.dump(events, f, indent=2, sort_keys=True) + + print(json.dumps(payload, indent=2, sort_keys=True)) + + +if __name__ == "__main__": + main()