LongyLongNeck

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

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))
 

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

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,

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

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()