From 3265a98d914ae88657aa446ecb43d5cfffc6f5d3 Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Wed, 26 Aug 2026 01:58:23 -0400 Subject: [PATCH] Add forensic analysis tool for Hybrid Experimental Mode decisions - Introduced `hem_forensic.py` to analyze and log the decision-making process of Hybrid Experimental Mode (HEM) during route playback. - The tool captures internal states, detects stop roll-through incidents, and provides detailed per-frame logs. - Added regression tests for HEM behavior in specific scenarios, ensuring correct braking behavior during partial slowdowns and gentle high-horizon slowdowns. --- .../controls/lib/hybrid_experimental_mode.py | 146 ++-- .../tests/test_hybrid_experimental_mode.py | 30 +- tools/replay/hem_forensic.py | 772 ++++++++++++++++++ 3 files changed, 899 insertions(+), 49 deletions(-) create mode 100644 tools/replay/hem_forensic.py diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index c4618f3c2..22da9bf57 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -3,6 +3,7 @@ import numpy as np from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE + def lerp(a: float, b: float, t: float) -> float: """Linear interpolation / blend between a and b by weight t (0.0 to 1.0).""" return float((1.0 - t) * a + t * b) @@ -53,6 +54,9 @@ class HybridExperimentalMode: self.last_standstill = False self.last_exp_dominant = False + self.diag = {} + self.record_diag = False + # User tuning self.HYBRID_EXP_BIAS = 0.2 # [-1.0, 1.0] self.VISION_BRAKE_SENSITIVITY = 1.2 # [0.0, 2.0] @@ -70,14 +74,18 @@ class HybridExperimentalMode: self.last_regime = "throttle" self.last_standstill = False self.last_exp_dominant = False + self.diag = {} def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float, t_follow=None, jerk_factor=None): self.HYBRID_EXP_BIAS = float(np.clip(exp_bias, -1.0, 1.0)) self.VISION_BRAKE_SENSITIVITY = float(np.clip(vision_brake_sensitivity, 0.0, 2.0)) if t_follow is not None or jerk_factor is not None: - self._update_profile_limits(t_follow, jerk_factor) + self._update_profile_limits( + t_follow if t_follow is not None else self.t_follow, + jerk_factor if jerk_factor is not None else self.jerk_factor + ) - def _update_profile_limits(self, t_follow, jerk_factor=1.0): + def _update_profile_limits(self, t_follow=None, jerk_factor=None): """Updates safety headway floor and jerk limits based on active driving profile.""" self.t_follow = float(t_follow) if t_follow is not None else self.BASE_T_FOLLOW self.jerk_factor = float(np.clip(jerk_factor if jerk_factor is not None else 1.0, 0.25, 2.0)) @@ -91,26 +99,20 @@ class HybridExperimentalMode: self.MAX_JERK_BRAKE = float(np.clip(self.BASE_MAX_JERK_BRAKE * self.jerk_factor, 1.8, 6.0)) @staticmethod - def _get_model_trajectory_v(model_v2, v_ego: float) -> np.ndarray: - velocity = getattr(model_v2, "velocity", None) - traj_v = getattr(velocity, "x", None) if velocity is not None else None - if traj_v is None or len(traj_v) == 0: - return np.array([v_ego], dtype=float) - traj_v = np.asarray(traj_v, dtype=float) - if not np.all(np.isfinite(traj_v)): - return np.array([v_ego], dtype=float) - return traj_v + def _get_traj_array(model_v2, attr: str) -> np.ndarray: + obj = getattr(model_v2, attr, None) + arr = getattr(obj, "x", None) if obj is not None else None + if arr is None or len(arr) == 0: + return np.array([], dtype=float) + arr = np.asarray(arr, dtype=float) + return arr if np.all(np.isfinite(arr)) else np.array([], dtype=float) - @staticmethod - def _get_model_trajectory_x(model_v2) -> np.ndarray: - position = getattr(model_v2, "position", None) - traj_x = getattr(position, "x", None) if position is not None else None - if traj_x is None or len(traj_x) == 0: - return np.array([], dtype=float) - traj_x = np.asarray(traj_x, dtype=float) - if not np.all(np.isfinite(traj_x)): - return np.array([], dtype=float) - return traj_x + def _get_model_trajectory_v(self, model_v2, v_ego: float) -> np.ndarray: + traj_v = self._get_traj_array(model_v2, "velocity") + return traj_v if len(traj_v) > 0 else np.array([v_ego], dtype=float) + + def _get_model_trajectory_x(self, model_v2) -> np.ndarray: + return self._get_traj_array(model_v2, "position") def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp, t_follow=None, jerk_factor=None): @@ -138,22 +140,26 @@ class HybridExperimentalMode: # Detect deceleration profile or low-speed stop line target speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref) - stop_target_active = sigmoid(1.2 - v_horizon, k=4.0, x0=0.0) + stop_target_active = sigmoid(1.2 - v_horizon, k=4.0) model_decel_strength = max(0.0, -a_exp / 2.0) raw_vision_metric = max(speed_drop_ratio, stop_target_active, model_decel_strength) w_vision = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0)) - # Kinematic stopping calculation when approaching a stop line - if v_min < 1.2 and v_horizon < 2.0 and v_ego > 0.1 and len(traj_x) > min_idx and traj_x[min_idx] > 0.2: - d_min = float(traj_x[min_idx]) - d_stop_effective = max(d_min - 1.5, 2.0) + # Kinematic stopping calculation + d_min = float(traj_x[min_idx]) if len(traj_x) > min_idx else float("inf") + d_stop_effective = max(d_min - 1.5, 2.0) + a_kinematic_stop = 0.0 + + slow_horizon = sigmoid(3.0 - v_horizon, k=2.0) + stop_confidence = max(stop_target_active, slow_horizon * speed_drop_ratio) + + if v_ego > 0.1 and 0.2 < d_min < float("inf") and stop_confidence > 0.15: a_kinematic_stop = float(np.clip(- (v_ego ** 2) / (2.0 * d_stop_effective), -3.5, 0.0)) if d_stop_effective < 6.0 and v_ego < 3.0: a_kinematic_stop = min(a_kinematic_stop, -0.6) a_exp_effective = min(a_exp, a_kinematic_stop) elif v_horizon < 1.0 and v_ego > 0.05: - # Final roll-in: enforce negative acceleration to complete stop a_exp_effective = min(a_exp, -0.5) else: a_exp_effective = a_exp @@ -163,8 +169,9 @@ class HybridExperimentalMode: self.exp_authority = alpha_exp # 2. ACCELERATION FUSION (Throttle vs Braking Regimes) - # Throttle Regime: Snappy pickup on open roads with clean cruise setpoint clamp a_throttle_raw = smooth_max(a_chill, a_exp, k=4.0) + overshoot_risk = 0.0 + a_throttle_capped = a_throttle_raw if v_ego >= v_cruise: a_throttle_optimal = min(a_throttle_raw, a_chill) else: @@ -176,15 +183,17 @@ class HybridExperimentalMode: a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision) # Braking Regime: Never dilute Exp stop braking with Chill's 0.0 m/s^2 + a_chill_brake = 0.0 if a_exp_effective < 0.0: a_chill_brake = min(a_chill, 0.0) a_brake_fused = min(a_exp_effective, a_chill_brake) if a_chill_brake < a_exp_effective \ - else lerp(a_exp_effective, a_chill_brake, 1.0 - alpha_exp) + else lerp(a_chill_brake, a_exp_effective, alpha_exp) else: a_brake_fused = min(a_chill, a_exp_effective) - # Regime Selection: If vision sees a stop or braking is requested, lock out positive throttle + # Regime Selection: Lock out positive throttle during stopping/braking is_braking_phase = (w_vision > 0.3) or (a_exp_effective < -0.2) or (a_chill < -0.2) + phase_metric = 0.0 if is_braking_phase: w_accel = 0.0 else: @@ -193,14 +202,12 @@ class HybridExperimentalMode: a_fused = lerp(a_brake_fused, a_throttle_fused, w_accel) - # 3. STANDSTILL ANCHOR (Hold at 0 mph, release cleanly on green departure or gas tap) - is_stopped = sigmoid(0.4 - v_ego, k=8.0, x0=0.0) - is_staying_stopped = sigmoid(0.5 - v_horizon, k=6.0, x0=0.0) + # 3. STANDSTILL ANCHOR + is_stopped = sigmoid(0.4 - v_ego, k=8.0) + is_staying_stopped = sigmoid(0.5 - v_horizon, k=6.0) lead_departing = lead_status and (getattr(lead_one, "vLead", 0.0) > 0.5) vision_departing = (v_horizon > 0.5) and (a_exp > 0.1) driver_departing = (a_chill > 0.4) and (not lead_status or lead_d_rel > 10.0) - # A real trajectory still predicting a stop line (red light / stop sign) keeps - # the anchor engaged: cruise creep must not release the brake at a stop it can't see. model_stop_predicted = len(traj_v) > 1 and v_horizon < 0.5 departing = (lead_departing or vision_departing or driver_departing) and not model_stop_predicted standstill_weight = (0.0 if departing else 1.0) * is_stopped * is_staying_stopped @@ -211,21 +218,16 @@ class HybridExperimentalMode: d_safe = (v_ego * self.T_FOLLOW_SAFE) + d_static_effective distance_ratio = (lead_d_rel - d_static_effective) / max(d_safe - d_static_effective, 1.0) - lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0, x0=0.0) * float(lead_status) + lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0) * float(lead_status) - # Enforce hard ceiling when lead is within safety envelope or Chill is braking for lead lead_safety_active = lead_status and (lead_d_rel < d_safe or a_chill < 0.0) - if lead_safety_active and a_chill < a_anchored: - a_safe = min(a_anchored, a_chill) - else: - a_safe = a_anchored + a_safe = min(a_anchored, a_chill) if lead_safety_active else a_anchored # 5. ASYMMETRIC DIRECTIONAL SLEW FILTER (Limit Jerk) da = a_safe - self.prev_a_target if da >= 0.0: jerk_limit = self.MAX_JERK_ACCEL else: - # Bypass comfort brake rate for lead collision or committed vision-stop emergencies. is_lead_emergency = (lead_safety_risk > 0.5) and (a_chill < -1.5) is_vision_emergency = (w_vision > 0.7) and (a_exp < -2.0) jerk_limit = self.EMERGENCY_JERK_BRAKE if (is_lead_emergency or is_vision_emergency) else self.MAX_JERK_BRAKE @@ -237,9 +239,57 @@ class HybridExperimentalMode: self.last_w_vision = w_vision self.last_regime = "brake" if is_braking_phase else "throttle" self.last_standstill = standstill_weight > 0.0 - # Border hint: True when the fused output tracks the E2E/vision input more - # closely than chill ACC. Setting-independent, unlike raw exp_authority which - # includes the E2E Authority Bias baseline. - out = self.prev_a_target - self.last_exp_dominant = abs(out - a_exp) < abs(out - a_chill) - 0.03 - return self.prev_a_target + self.last_exp_dominant = abs(self.prev_a_target - a_exp) < abs(self.prev_a_target - a_chill) - 0.03 + + if self.record_diag: + self.diag = { + # inputs + "v_ego": v_ego, "v_cruise": v_cruise, + "a_chill": a_chill, "a_exp": a_exp, + "lead_status": lead_status, "lead_d_rel": lead_d_rel, + "lead_v_lead": float(getattr(lead_one, "vLead", 0.0)), + "t_follow": self.t_follow, + # vision intent + "v_min": v_min, "v_horizon": v_horizon, "v_ref": v_ref, + "min_idx": min_idx, "speed_drop_ratio": speed_drop_ratio, + "stop_target_active": stop_target_active, + "stop_confidence": stop_confidence, + "model_decel_strength": model_decel_strength, + "raw_vision_metric": raw_vision_metric, "w_vision": w_vision, + # kinematic stop + "d_min": d_min, "d_stop_effective": d_stop_effective, + "a_kinematic_stop": a_kinematic_stop, "a_exp_effective": a_exp_effective, + # authority + "base_auth": base_auth, "alpha_exp": alpha_exp, + # throttle path + "a_throttle_raw": a_throttle_raw, + "a_throttle_optimal": a_throttle_optimal, + "a_throttle_capped": a_throttle_capped, + "overshoot_risk": overshoot_risk, + "a_throttle_conservative": a_throttle_conservative, + "a_throttle_fused": a_throttle_fused, + # brake path + "a_chill_brake": a_chill_brake, "a_brake_fused": a_brake_fused, + # regime selection + "is_braking_phase": is_braking_phase, + "phase_metric": phase_metric, "w_accel": w_accel, + "a_fused": a_fused, + # standstill anchor + "is_stopped": is_stopped, "is_staying_stopped": is_staying_stopped, + "lead_departing": lead_departing, "vision_departing": vision_departing, + "driver_departing": driver_departing, + "model_stop_predicted": model_stop_predicted, + "departing": departing, "standstill_weight": standstill_weight, + "a_anchored": a_anchored, + # safety barrier + "d_static_effective": d_static_effective, "d_safe": d_safe, + "distance_ratio": distance_ratio, "lead_safety_risk": lead_safety_risk, + "lead_safety_active": lead_safety_active, "a_safe": a_safe, + # slew filter + "da": da, "jerk_limit": jerk_limit, "max_delta": max_delta, + "a_out": a_out, "prev_a_target": self.prev_a_target, + # regime labels + "regime": self.last_regime, + "standstill": self.last_standstill, + } + return self.prev_a_target \ No newline at end of file diff --git a/starpilot/controls/tests/test_hybrid_experimental_mode.py b/starpilot/controls/tests/test_hybrid_experimental_mode.py index 9daf986c4..313a0ad53 100644 --- a/starpilot/controls/tests/test_hybrid_experimental_mode.py +++ b/starpilot/controls/tests/test_hybrid_experimental_mode.py @@ -281,4 +281,32 @@ def test_last_exp_dominant_false_when_chill_brakes_for_lead(): def test_last_exp_dominant_false_at_neutral_cruise(): controller = make_controller(prev=0.0) run(controller, a_chill=0.0, a_exp=0.05, frames=10) - assert not controller.last_exp_dominant \ No newline at end of file + assert not controller.last_exp_dominant + + +def test_red_light_approach_brakes_kinematically_on_partial_slowdown(): + # Regression: the model predicts only a PARTIAL slowdown (v_min ~ 3 m/s), not a + # full stop, so the old v_min<1.2 gate never fired and HEM mirrored the model's + # weak a_exp (-0.1) -> it would roll the red light. HEM must now apply the + # kinematic -v^2/2d floor toward the closing stop point regardless. + controller = make_controller(prev=0.0) + traj_v = np.linspace(15.0, 3.0, 33) + traj_x = np.linspace(0.0, 40.0, 33) + model = FakeModel(velocity=traj_v, position=traj_x) + a = run(controller, v_ego=15.0, v_cruise=20.0, lead=FakeLead(status=False), + model=model, a_chill=0.0, a_exp=-0.1, frames=10) + # kinematic = -15^2/(2*38.5) ~ -2.9; even with jerk slew HEM must brake hard, + # not track the -0.1 exp input. + assert a < -1.0, f"HEM must apply kinematic stop on partial-slowdown approach, got {a}" + + +def test_no_kinematic_brake_on_gentle_high_horizon_slowdown(): + # A gentle slowdown that KEEPS a high horizon speed (curve / slower traffic, + # ends at 15 m/s) must NOT trigger kinematic stop-to-zero braking. + controller = make_controller(prev=0.0) + traj_v = np.linspace(20.0, 15.0, 33) + traj_x = np.linspace(0.0, 100.0, 33) + model = FakeModel(velocity=traj_v, position=traj_x) + a = run(controller, v_ego=20.0, v_cruise=20.0, lead=FakeLead(status=False), + model=model, a_chill=0.0, a_exp=-0.1, frames=5) + assert a > -0.5, f"Gentle high-horizon slowdown must not hard-brake, got {a}" \ No newline at end of file diff --git a/tools/replay/hem_forensic.py b/tools/replay/hem_forensic.py new file mode 100644 index 000000000..078a5fb7a --- /dev/null +++ b/tools/replay/hem_forensic.py @@ -0,0 +1,772 @@ +#!/usr/bin/env python3 +"""HEM forensic analyzer: figure out exactly what HybridExperimentalMode decided +and why, on a logged route, down to the per-frame input params and every internal +gate. Designed to answer "why did HEM roll through that stop sign / red light?" + +It replays a route segment(s) through the *real* LongitudinalPlanner (Chill / ACC) +and the *real* HybridExperimentalMode (using the logged modelV2 action for Exp), +and records HybridExperimentalMode's full internal decision state each frame +(vision weight, stop detection, throttle/brake fusion, standstill anchor, safety +barrier, slew filter). It then: + + * auto-detects "stop roll-through" incidents (vehicle creeping/coasting through + a detected stop with no braking authority), + * prints a per-frame forensic log of the decision chain around the incident, + * prints a root-cause narrative naming the exact gate that failed, and + * writes a graph of the Exp input, Chill input, fused output, authority, vision + weight and regime. + +Usage: + ./dev python tools/replay/hem_forensic.py / [--segments 0,1] + [--start 30] [--end 120] [--data_dir /path/to/routes] [--out hem_forensic.png] + [--show] [--csv out.csv] [--window 12] + [--set hybrid_exp_bias=0.2 --set hybrid_vision_brake_sensitivity=1.2] + +Examples: + ./dev python tools/replay/hem_forensic.py afb7ef2ed593d651/000000af--414a758637 --segments 0,1 + ./dev python tools/replay/hem_forensic.py afb7ef2ed593d651/000000af--414a758637 --segments 0,1 --window 20 --set hybrid_exp_bias=0.2 +""" +from __future__ import annotations + +import argparse +import bisect +import sys +import time +from pathlib import Path +from types import SimpleNamespace +from typing import Any + +import numpy as np + +ROOT_DIR = Path(__file__).resolve().parents[2] +if str(ROOT_DIR) not in sys.path: + sys.path.insert(0, str(ROOT_DIR)) + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode +from openpilot.tools.replay.mode_sim import ( + FakeParams, + TOGGLE_DEFAULTS, + as_lead, + load_buffers, + parse_toggle_overrides, + resolve_segment_identifier, +) + +# Services we need to reconstruct the Chill (ACC) planner + Exp input + HEM. +SERVICES = { + "carState", "radarState", "starpilotRadarState", "modelV2", + "longitudinalPlan", "selfdriveState", "starpilotPlan", "starpilotCarState", + "controlsState", "carParams", "liveParameters", "carControl", +} + +# Fields in HybridExperimentalMode.diag that are part of the decision chain. +DIAG_KEYS = [ + # inputs + "v_ego", "v_cruise", "a_chill", "a_exp", + "lead_status", "lead_d_rel", "lead_v_lead", + # vision intent + "v_min", "v_horizon", "speed_drop_ratio", "stop_target_active", "stop_confidence", + "model_decel_strength", "w_vision", + # kinematic stop + "d_min", "d_stop_effective", "a_kinematic_stop", "a_exp_effective", + # authority + "base_auth", "alpha_exp", + # throttle path + "a_throttle_raw", "a_throttle_optimal", "a_throttle_fused", "overshoot_risk", + # brake path + "a_chill_brake", "a_brake_fused", + # regime + "is_braking_phase", "w_accel", "a_fused", + # standstill anchor + "is_stopped", "is_staying_stopped", "model_stop_predicted", + "departing", "standstill_weight", "a_anchored", + # safety + "d_safe", "distance_ratio", "lead_safety_risk", "lead_safety_active", "a_safe", + # slew + "da", "jerk_limit", "max_delta", "a_out", "prev_a_target", +] +REGIME_KEYS = ["regime", "standstill"] + + +def merge_buffers(buffers_list): + """Merge per-segment {service: [(t, msg), ...]} into one dict sorted by time.""" + merged = {s: [] for s in SERVICES} + for bufs in buffers_list: + for s in SERVICES: + merged[s].extend(bufs.get(s, [])) + for s in SERVICES: + merged[s].sort(key=lambda x: x[0]) + return merged + + +def run_forensic(grid, bufs, toggles): + """Replay through the real planner + HEM, recording every HEM internal gate.""" + ts = {s: [t for t, _ in m] for s, m in bufs.items()} + ms = {s: [m for _, m in bufs[s]] for s in bufs} + + def latest(service, t): + times = ts.get(service) + if not times: + return None + idx = bisect.bisect_right(times, t) - 1 + return ms[service][idx] if idx >= 0 else None + + cp_candidate = None + for _, cp in bufs.get("carParams", []): + if cp is not None: + cp_candidate = cp + break + if cp_candidate is None: + cp_candidate = SimpleNamespace( + brand="toyota", carFingerprint="TOYOTA_RAV4", openpilotLongitudinalControl=True, + pcmCruise=False, steerRatio=15.0, wheelbase=2.7, longitudinalActuatorDelay=0.2, flags=0, + ) + + planner_state = SimpleNamespace( + params=FakeParams(), + params_memory=FakeParams(), + starpilot_following=SimpleNamespace(following_lead=False, slower_lead=False), + starpilot_vcruise=SimpleNamespace( + slc=SimpleNamespace(experimental_mode=False), + stop_sign_confirmed=False, + forcing_stop=False, + ), + ) + + hybrid = HybridExperimentalMode() + hybrid.record_diag = True # capture full per-frame decision state for the trace + hybrid.set_tuning(toggles.hybrid_exp_bias, toggles.hybrid_vision_brake_sensitivity) + chill_long_planner = LongitudinalPlanner(cp_candidate, dt=DT_MDL) + + n = len(grid) + out = {k: np.zeros(n) for k in [ + "v_ego", "v_cruise", "model_v0", "a_chill", "a_exp", "hem_a", "hem_authority", + "lead_d_rel", "lead_v_lead", "logged_aTarget", "a_ego", + ]} + out["lead_status"] = np.zeros(n, dtype=bool) + out["brake_pressed"] = np.zeros(n, dtype=bool) + out["gas_pressed"] = np.zeros(n, dtype=bool) + for k in DIAG_KEYS: + if k in ("is_braking_phase", "model_stop_predicted", "lead_safety_active", "departing"): + out["hem_" + k] = np.zeros(n, dtype=bool) + else: + out["hem_" + k] = np.zeros(n) + out["hem_regime"] = np.zeros(n, dtype=bool) + out["hem_standstill"] = np.zeros(n, dtype=bool) + + real_monotonic = time.monotonic + fake_clock = [0.0] + time.monotonic = lambda: fake_clock[0] + + last_model_v2 = None + for _, m in bufs.get("modelV2", []): + if m is not None: + last_model_v2 = m + break + + try: + for i, t in enumerate(grid): + fake_clock[0] = float(i) * DT_MDL + + car_raw = latest("carState", t) + v_ego_raw = max(float(getattr(car_raw, "vEgo", 0.0)), 0.0) + car = SimpleNamespace( + vEgo=v_ego_raw, + vEgoCluster=max(float(getattr(car_raw, "vEgoCluster", v_ego_raw)), 0.0), + vCruise=float(getattr(car_raw, "vCruise", 0.0)), + standstill=bool(getattr(car_raw, "standstill", False)), + leftBlinker=bool(getattr(car_raw, "leftBlinker", False)), + rightBlinker=bool(getattr(car_raw, "rightBlinker", False)), + gasPressed=bool(getattr(car_raw, "gasPressed", False)), + brakePressed=bool(getattr(car_raw, "brakePressed", False)), + steeringAngleDeg=float(getattr(car_raw, "steeringAngleDeg", 0.0)), + aEgo=float(getattr(car_raw, "aEgo", 0.0)), + leftBlindspot=bool(getattr(car_raw, "leftBlindspot", False)), + rightBlindspot=bool(getattr(car_raw, "rightBlindspot", False)), + ) + v_ego = car.vEgo + + radar = latest("radarState", t) + lead = as_lead(getattr(radar, "leadOne", None) if radar is not None else None) + lead2 = as_lead(getattr(radar, "leadTwo", None) if radar is not None else None) + sradar = latest("starpilotRadarState", t) + lead_left = as_lead(getattr(sradar, "leadLeft", None) if sradar is not None else None) + lead_right = as_lead(getattr(sradar, "leadRight", None) if sradar is not None else None) + + lplan = latest("longitudinalPlan", t) + sds = latest("selfdriveState", t) + splan = latest("starpilotPlan", t) + scs = latest("starpilotCarState", t) + model_v2 = latest("modelV2", t) + if model_v2 is not None: + last_model_v2 = model_v2 + else: + model_v2 = last_model_v2 # carry last vision frame across a replay gap + live_params = latest("liveParameters", t) + car_control = latest("carControl", t) + controls_state = latest("controlsState", t) + car_params = latest("carParams", t) or cp_candidate + + v_cruise = float(getattr(splan, "vCruise", 0.0) or 0.0) + if not (v_cruise > 0): + v_cruise_kph = float(getattr(car_raw, "vCruise", 0.0) or 0.0) + if 0 < v_cruise_kph < V_CRUISE_UNSET: + v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS + else: + v_cruise = v_ego + + tracking_lead = bool(getattr(splan, "trackingLead", False)) + if not tracking_lead: + tracking_lead = bool(getattr(lplan, "hasLead", False)) + + t_follow = float(getattr(splan, "tFollow", 1.45)) + following_lead = tracking_lead and lead.dRel < (t_follow * 2) * v_ego + planner_state.lead_one = lead + planner_state.tracking_lead = tracking_lead + + sm_dict = { + "carState": car, + "radarState": SimpleNamespace(leadOne=lead, leadTwo=lead2), + "starpilotRadarState": SimpleNamespace(leadLeft=lead_left, leadRight=lead_right), + "starpilotCarState": SimpleNamespace( + trafficModeEnabled=bool(getattr(scs, "trafficModeEnabled", False)), + alwaysOnLateralEnabled=bool(getattr(scs, "alwaysOnLateralEnabled", False)), + dashboardStopSign=int(getattr(scs, "dashboardStopSign", 0)), + accelPressed=bool(getattr(scs, "accelPressed", False)), + ), + "selfdriveState": SimpleNamespace( + enabled=bool(getattr(sds, "enabled", False)), + experimentalMode=False, # force ACC/Chill evaluation in MPC + personality=0, + ), + "longitudinalPlan": SimpleNamespace( + hasLead=bool(getattr(lplan, "hasLead", False)), + allowThrottle=bool(getattr(lplan, "allowThrottle", True)), + shouldStop=bool(getattr(lplan, "shouldStop", False)), + aTarget=float(getattr(lplan, "aTarget", 0.0)), + ), + "starpilotPlan": SimpleNamespace( + vCruise=v_cruise, + tFollow=t_follow, + trackingLead=tracking_lead, + redLight=bool(getattr(splan, "redLight", False)), + forcingStop=bool(getattr(splan, "forcingStop", False)), + forcingStopLength=float(getattr(splan, "forcingStopLength", 100.0)), + minAcceleration=float(getattr(splan, "minAcceleration", -3.5)), + maxAcceleration=float(getattr(splan, "maxAcceleration", 1.5)), + accelerationJerk=float(getattr(splan, "accelerationJerk", 1.0)), + dangerJerk=float(getattr(splan, "dangerJerk", 1.0)), + speedJerk=float(getattr(splan, "speedJerk", 1.0)), + dangerFactor=float(getattr(splan, "dangerFactor", 1.0)), + disableThrottle=bool(getattr(splan, "disableThrottle", False)), + ), + "controlsState": SimpleNamespace( + longControlState=LongCtrlState.pid, + forceDecel=bool(getattr(controls_state, "forceDecel", False)), + curvature=0.0, + ), + "liveParameters": SimpleNamespace( + angleOffsetDeg=float(getattr(live_params, "angleOffsetDeg", 0.0)) if live_params else 0.0, + ), + "carControl": SimpleNamespace( + orientationNED=getattr(car_control, "orientationNED", [0.0, 0.0, 0.0]) if car_control else [0.0, 0.0, 0.0], + ), + "modelV2": model_v2, + "carParams": car_params, + } + + # Chill (ACC) acceleration target via the real planner. + chill_long_planner.update(sm_dict, toggles) + a_chill = float(chill_long_planner.output_a_target) + + # Exp (vision) acceleration target from the logged model action. + a_exp = 0.0 + if model_v2 is not None: + try: + action_obj = getattr(model_v2, "action", None) + if action_obj is not None and hasattr(action_obj, "desiredAcceleration"): + a_exp = float(action_obj.desiredAcceleration) + else: + accel_x = getattr(model_v2, "acceleration", None) + if accel_x is not None and len(accel_x.x): + a_exp = float(accel_x.x[0]) + except Exception: + a_exp = 0.0 + + a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp, t_follow=t_follow) + + model_v0 = v_ego + if model_v2 is not None: + try: + vel_x = getattr(model_v2, "velocity", None) + if vel_x is not None and len(vel_x.x): + model_v0 = float(vel_x.x[0]) + except Exception: + model_v0 = v_ego + + out["v_ego"][i] = v_ego + out["v_cruise"][i] = v_cruise + out["model_v0"][i] = model_v0 + out["a_chill"][i] = a_chill + out["a_exp"][i] = a_exp + out["hem_a"][i] = a_hem + out["hem_authority"][i] = hybrid.exp_authority + out["lead_status"][i] = lead.status + out["lead_d_rel"][i] = lead.dRel + out["lead_v_lead"][i] = lead.vLead + out["logged_aTarget"][i] = float(getattr(lplan, "aTarget", 0.0)) + out["brake_pressed"][i] = car.brakePressed + out["gas_pressed"][i] = car.gasPressed + out["a_ego"][i] = car.aEgo + + for k in DIAG_KEYS: + val = hybrid.diag.get(k) + if isinstance(val, bool): + out["hem_" + k][i] = bool(val) + else: + try: + out["hem_" + k][i] = float(val) + except Exception: + out["hem_" + k][i] = 0.0 + out["hem_regime"][i] = hybrid.diag.get("regime") == "brake" + out["hem_standstill"][i] = bool(hybrid.diag.get("standstill", False)) + finally: + time.monotonic = real_monotonic + + return out + + +def detect_incidents(out, t): + """Find frames where a detected stop was rolled/crept through for lack of braking. + + A roll-through frame: the car is actually moving (v_ego > 0.5 m/s), the vision + side has flagged a stop (w_vision > 0.3), yet the fused output is essentially + coasting because BOTH chill and exp inputs failed to request a stop: + a_chill (ACC) is not braking -> a_chill > -0.3 + exp effective accel is non-negative -> a_exp_effective >= 0 + Under those conditions a_brake_fused = min(a_chill, a_exp) lands at ~0 and HEM + coasts through the stop it claims to see. + """ + idxs = [] + for i in range(len(t)): + v = out["v_ego"][i] + w = out["hem_w_vision"][i] + ac = out["hem_a_chill"][i] + ae_eff = out["hem_a_exp_effective"][i] + aout = out["hem_a"][i] + if v > 0.5 and w > 0.3 and ac > -0.3 and ae_eff >= 0.0 and abs(aout) < 0.25: + idxs.append(i) + return idxs + + +def hard_brake_clusters(out, gap_sec=1.5): + """Clusters of frames where the DRIVER slammed the brakes (a_ego < -2.0). + + The driver's own hard brake is the ground-truth marker that HEM failed to stop + the car. Each cluster is a discrete human-intervention event.""" + hb = hard_brake_idxs(out) + return cluster_indices(hb, gap_sec=gap_sec) + + +def classify_failure(out, sl, i_brake): + """Classify WHY the driver had to brake, from the frames just before the brake.""" + lead = int(3.0 / DT_MDL) + pre = slice(max(sl.start, i_brake - lead), i_brake) + if pre.stop <= pre.start: + return "UNKNOWN", {} + v = out["v_ego"][pre] + exp_eff = out["hem_a_exp_effective"][pre] + wv = out["hem_w_vision"][pre] + hem = out["hem_a"][pre] + chill = out["hem_a_chill"][pre] + # out["hem_regime"] is True for the BRAKE regime. + brake_share = float(np.mean(out["hem_regime"][pre])) + throttle_share = 1.0 - brake_share + + stats = { + "v_before": float(v[-1]), "v_max": float(v.max()), + "exp_eff_mean": float(exp_eff.mean()), "exp_eff_min": float(exp_eff.min()), + "w_vis_max": float(wv.max()), "w_vis_mean": float(wv.mean()), + "hem_min": float(hem.min()), "hem_mean": float(hem.mean()), + "chill_min": float(chill.min()), + "throttle_share": throttle_share, + } + + if stats["hem_min"] > -0.35 and stats["w_vis_max"] < 0.5 and stats["exp_eff_mean"] >= 0.0: + mode = "NO_STOP_REQUEST" # HEM never braked; neither input requested a stop + elif stats["throttle_share"] > 0.6 and stats["exp_eff_mean"] < -0.2: + mode = "THROTTLE_OVERRIDE" # vision wanted to brake but HEM output positive throttle + elif stats["w_vis_max"] > 0.4 and stats["exp_eff_mean"] < -0.2 and stats["hem_min"] > -1.2: + mode = "WEAK_BRAKE" # vision saw the stop but braking authority was too weak + elif stats["w_vis_max"] > 0.3 and stats["exp_eff_mean"] < -0.1: + mode = "LATE_BRAKE" # HEM braked but too late / not enough distance + else: + mode = "UNKNOWN" + return mode, stats + + +def cluster_indices(idxs, gap_sec=2.0): + """Split incident indices into clusters separated by > gap_sec of quiet time.""" + if not idxs: + return [] + gap = int(gap_sec / DT_MDL) + clusters = [[idxs[0]]] + for idx in idxs[1:]: + if idx - clusters[-1][-1] > gap: + clusters.append([idx]) + else: + clusters[-1].append(idx) + return clusters + + +def hard_brake_idxs(out): + """Frames where the DRIVER slammed the brakes (a_ego strongly negative) — a + human-intervention marker proving HEM was about to run the stop.""" + return [i for i in range(len(out["a_ego"])) if out["a_ego"][i] < -2.0] + + +def select_main_cluster(clusters, hb): + """Pick the incident cluster the driver actually intervened on (nearest hard + brake); fall back to the densest cluster when no driver braking occurred.""" + if not clusters: + return [] + if not hb: + return max(clusters, key=len) + best, best_gap = None, float("inf") + for cl in clusters: + gap = min(min(abs(i - j) for j in hb) for i in cl) + if gap < best_gap: + best_gap, best = gap, cl + return best + + +def window_around(idxs, t, out, span_sec=6.0, max_frames=400): + """Select the incident cluster the driver braked on, return (slice, cluster).""" + if not idxs: + return None, [] + clusters = cluster_indices(idxs) + main = select_main_cluster(clusters, hard_brake_idxs(out)) + start = max(0, main[0] - int(span_sec / DT_MDL)) + end = min(len(t) - 1, main[-1] + int(span_sec / DT_MDL)) + if end - start > max_frames: + mid = (start + end) // 2 + start = max(0, mid - max_frames // 2) + end = min(len(t) - 1, start + max_frames) + main = [i for i in main if start <= i <= end] + return slice(start, end + 1), main + + +def print_forensic_log(out, sl, t0): + print("\n" + "-" * 122) + print(" PER-FRAME HEM DECISION CHAIN") + print("-" * 122) + hdr = (f" {'t':>5} {'vEgo':>5} {'aEgo':>6} {'dvrBrk':>6} {'chill':>6} {'exp':>6} {'expEff':>6} " + f"{'wVis':>5} {'auth':>5} {'regime':>8} {'stand':>5} {'brakeF':>6} {'throtF':>6} {'hem_a':>6}") + print(hdr) + print(" " + "-" * 122) + for i in range(sl.start, sl.stop): + dt = t0[i] + row = [ + f"{dt:5.1f}", + f"{out['v_ego'][i]:5.2f}", + f"{out['a_ego'][i]:6.2f}", + f"{'<<<<' if out['a_ego'][i] < -2.0 else ('Y' if out['brake_pressed'][i] else '-'):>6}", + f"{out['hem_a_chill'][i]:6.2f}", + f"{out['hem_a_exp'][i]:6.2f}", + f"{out['hem_a_exp_effective'][i]:6.2f}", + f"{out['hem_w_vision'][i]:5.2f}", + f"{out['hem_alpha_exp'][i]:5.2f}", + f"{'brake' if out['hem_regime'][i] else 'throttle':>8}", + f"{'Y' if out['hem_standstill'][i] else '-':>5}", + f"{out['hem_a_brake_fused'][i]:6.2f}", + f"{out['hem_a_throttle_fused'][i]:6.2f}", + f"{out['hem_a'][i]:6.2f}", + ] + print(" " + " ".join(row)) + + +def print_root_cause(out, t0, brake_cluster, sl, mode, stats): + print("\n" + "=" * 78) + print(" ROOT-CAUSE DIAGNOSIS (driver-brake anchored)") + print("=" * 78) + if not brake_cluster: + print(" No hard driver brake (a_ego < -2.0) found; no human intervention to explain.") + print(" If a stop was still rolled through, pass --start/--end around it.") + return + + i = brake_cluster[0] + dt = t0[i] + v = out["v_ego"][i] + print(f" Driver slammed the brakes at t = {dt:.1f}s (segment-relative), v_ego = {v:.1f} m/s " + f"({v * CV.MS_TO_MPH:.1f} mph), a_ego = {out['a_ego'][i]:.2f} m/s².") + print(f" HEM fused output at that instant: {out['hem_a'][i]:+.2f} m/s².") + + labels = { + "NO_STOP_REQUEST": "HEM NEVER REQUESTED A STOP", + "THROTTLE_OVERRIDE": "HEM ACCELERATED DESPITE VISION STOP", + "WEAK_BRAKE": "HEM BRAKED BUT TOO WEAKLY", + "LATE_BRAKE": "HEM BRAKED BUT TOO LATE", + "UNKNOWN": "NO CLEAR FAILURE MODE (see trace)", + } + print(f"\n FAILURE MODE: {labels.get(mode, mode)}") + + print("\n [HEM STATE IN THE ~3s BEFORE THE DRIVER BRAKE]") + print(f" Speed just before brake : {stats.get('v_before', 0):.1f} m/s (max {stats.get('v_max', 0):.1f})") + print(f" Vision weight w_vision : mean {stats.get('w_vis_mean', 0):.2f} | max {stats.get('w_vis_max', 0):.2f} (0.3+ = stop detected)") + print(f" Exp effective a : mean {stats.get('exp_eff_mean', 0):+.2f} | min {stats.get('exp_eff_min', 0):+.2f} m/s²") + print(f" Chill (ACC) a_chill : min {stats.get('chill_min', 0):+.2f} m/s²") + print(f" HEM output a_hem : mean {stats.get('hem_mean', 0):+.2f} | min {stats.get('hem_min', 0):+.2f} m/s²") + print(f" Throttle regime share : {100 * stats.get('throttle_share', 0):.0f}%") + + print("\n [INTERPRETATION]") + if mode == "NO_STOP_REQUEST": + print(" • Vision never flagged a stop (w_vision stayed < 0.5) and Exp requested no brake") + print(" (a_exp_eff >= 0). Chill/ACC also requested no brake. HEM coasted, so the driver") + print(" had to brake. The stop/light was missed at the perception level (model predicted go).") + elif mode == "THROTTLE_OVERRIDE": + print(" • Vision DID see the stop (exp_eff < -0.2) but HEM spent >60% of the approach in the") + print(" THROTTLE regime and output positive accel. The throttle path (smooth_max of chill/exp)") + print(" or a departing flag won over the brake path, so HEM drove toward the light.") + elif mode == "WEAK_BRAKE": + print(" • Vision saw the stop (w_vision > 0.4) and Exp requested a brake, but HEM's output") + print(f" never went harder than {stats.get('hem_min', 0):.2f} m/s². That decel was not enough to") + print(" stop in time at the approach speed, so the driver had to brake hard.") + elif mode == "LATE_BRAKE": + print(" • HEM did brake but only started hard late in the approach (or the needed stop distance") + print(" exceeded what its braking could cover), forcing the driver to intervene.") + else: + print(" • Review the per-frame trace above; the inputs/state were mixed.") + + print("\n [FIX DIRECTIONS]") + if mode in ("NO_STOP_REQUEST", "THROTTLE_OVERRIDE"): + print(" • When w_vision or exp_eff indicates a stop but neither input produces a brake, HEM") + print(" should synthesize its own kinematic brake (a_kinematic_stop) instead of blending 0's.") + print(" • Lock the braking regime (w_accel=0) harder when a stop is detected, so the throttle") + print(" path cannot override a vision brake.") + elif mode in ("WEAK_BRAKE", "LATE_BRAKE"): + print(" • HEM currently mirrors the model's Exp brake (a_exp_eff) and the ACC brake; when those") + print(" are too gentle for the required stop distance, HEM should apply a stronger floor based") + print(" on the kinematic stop calculation (-v²/2d) and vision brake sensitivity.") + print(" • Raise VISION_BRAKE_SENSITIVITY or lower the kinematic-stop distance threshold so the") + print(" vision stop requests decel earlier and harder.") + + +def plot_results(out, t0, args, incident_idxs): + import matplotlib + if not args.show: + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + n = len(t0) + fig, axs = plt.subplots(5, 1, figsize=(15, 17), sharex=True, + gridspec_kw={"height_ratios": [2, 2, 2, 2, 1.4]}) + fig.suptitle(f"HEM forensic {args.route} segs {args.segments} " + f"(bias={args_set(args.set, 'hybrid_exp_bias', 0.0):+.2f}, " + f"vis_brake={args_set(args.set, 'hybrid_vision_brake_sensitivity', 1.0):.2f})", + fontsize=11) + + # shade incident region + if incident_idxs: + for ax in axs: + x0 = t0[max(0, incident_idxs[0] - int(2 / DT_MDL))] + x1 = t0[min(n - 1, incident_idxs[-1] + int(2 / DT_MDL))] + ax.axvspan(x0, x1, color="red", alpha=0.08) + + # speed + axs[0].plot(t0, out["v_ego"] * CV.MS_TO_MPH, color="black", lw=1.5, label="v_ego") + axs[0].plot(t0, out["v_cruise"] * CV.MS_TO_MPH, color="tab:blue", lw=1.0, ls="--", label="v_cruise") + axs[0].plot(t0, out["model_v0"] * CV.MS_TO_MPH, color="tab:green", lw=1.0, alpha=0.7, label="model v[0]") + axs[0].set_ylabel("mph") + axs[0].set_title("1) Speed") + axs[0].legend(loc="upper right", fontsize=8) + axs[0].grid(alpha=0.3) + + # accel inputs vs output + axs[1].axhline(0, color="gray", lw=0.6) + hard_brake = out["a_ego"] < -2.0 + if np.any(hard_brake): + axs[1].fill_between(t0, -3.5, 2.0, where=hard_brake, step="post", + color="crimson", alpha=0.15, label="driver hard brake") + axs[1].plot(t0, out["a_chill"], color="tab:blue", lw=1.1, ls="--", label="Chill (ACC) input") + axs[1].plot(t0, out["a_exp"], color="tab:orange", lw=1.1, ls=":", label="Exp (vision) input") + axs[1].plot(t0, out["hem_a"], color="tab:cyan", lw=1.6, label="HEM fused output") + axs[1].plot(t0, out["a_ego"], color="crimson", lw=1.0, alpha=0.8, label="a_ego (actual)") + axs[1].plot(t0, out["logged_aTarget"], color="gray", lw=0.9, alpha=0.6, label="logged aTarget") + axs[1].set_ylim(-3.5, 2.0) + axs[1].set_ylabel("accel (m/s²)") + axs[1].set_title("2) HEM inputs vs output vs actual (crimson = driver brake)") + axs[1].legend(loc="upper right", fontsize=7, ncol=2) + axs[1].legend(loc="upper right", fontsize=8, ncol=2) + axs[1].grid(alpha=0.3) + + # vision weight, authority, exp_effective + axs[2].plot(t0, out["hem_w_vision"], color="tab:red", lw=1.3, label="w_vision") + axs[2].plot(t0, out["hem_alpha_exp"], color="tab:purple", lw=1.3, label="exp authority") + axs[2].plot(t0, out["hem_a_exp_effective"], color="tab:olive", lw=1.0, ls="--", label="exp effective a") + axs[2].axhline(0.3, color="tab:red", lw=0.6, ls=":") + axs[2].set_ylim(-1, 1.5) + axs[2].set_ylabel("weight") + axs[2].set_title("3) Vision stop detection vs Exp effective accel") + axs[2].legend(loc="upper right", fontsize=8) + axs[2].grid(alpha=0.3) + + # decision gates + axs[3].plot(t0, out["hem_a_brake_fused"], color="tab:blue", lw=1.2, label="brake-fused") + axs[3].plot(t0, out["hem_a_throttle_fused"], color="tab:orange", lw=1.2, label="throttle-fused") + axs[3].plot(t0, out["hem_a_anchored"], color="tab:green", lw=1.2, ls="--", label="after standstill") + axs[3].plot(t0, out["hem_a_safe"], color="tab:red", lw=1.2, ls="--", label="after safety") + axs[3].plot(t0, out["hem_a"], color="black", lw=1.6, label="final output") + axs[3].axhline(0, color="gray", lw=0.6) + axs[3].set_ylabel("accel (m/s²)") + axs[3].set_title("4) Fusion pipeline: which gate produced the output") + axs[3].legend(loc="upper right", fontsize=8, ncol=2) + axs[3].grid(alpha=0.3) + + # regime + standstill + axs[4].fill_between(t0, 0.0, 1.0, where=out["hem_regime"], step="post", + color="tab:red", alpha=0.5, label="brake regime") + axs[4].fill_between(t0, 1.0, 2.0, where=out["hem_standstill"], step="post", + color="tab:green", alpha=0.5, label="standstill anchor") + axs[4].set_yticks([0.5, 1.5]) + axs[4].set_yticklabels(["brake", "standstill"]) + axs[4].set_ylim(-0.3, 2.3) + axs[4].set_xlabel("time (s, segment-relative)") + axs[4].set_title("5) Regime") + axs[4].legend(loc="upper right", fontsize=8) + axs[4].grid(axis="y", alpha=0.3) + + fig.tight_layout() + if args.out: + p = Path(args.out) + p.parent.mkdir(parents=True, exist_ok=True) + fig.savefig(p, dpi=150) + print(f"Saved graph to {p}") + if args.show: + plt.show() + plt.close(fig) + + +def args_set(overrides, key, default): + for item in overrides: + k, sep, raw = item.partition("=") + if sep and k.strip() == key: + try: + return float(raw.strip()) + except ValueError: + return default + return default + + +def main(argv=None): + parser = argparse.ArgumentParser( + description="Forensic analysis of what HybridExperimentalMode decided, and why, on a route.") + parser.add_argument("route", help="route: / (the part after connect.comma.ai/), " + "e.g. afb7ef2ed593d651/000000af--414a758637") + parser.add_argument("--segments", default="0", help="comma-separated segment indices, e.g. 0,1") + parser.add_argument("--data_dir", help="local directory containing route files") + parser.add_argument("--start", type=float, default=None, help="start seconds (segment-relative)") + parser.add_argument("--end", type=float, default=None, help="end seconds (segment-relative)") + parser.add_argument("--window", type=float, default=8.0, + help="seconds of context to print around each detected incident (default 8)") + parser.add_argument("--out", default="hem_forensic.png", help="output PNG path") + parser.add_argument("--show", action="store_true", help="show plot window") + parser.add_argument("--csv", default=None, help="optional CSV output of the time series") + parser.add_argument("--set", action="append", default=[], metavar="KEY=value", + help="override a toggle, e.g. --set hybrid_exp_bias=0.2 --set hybrid_vision_brake_sensitivity=1.2") + args = parser.parse_args(argv) + + overrides = parse_toggle_overrides(args.set) + defaults = dict(TOGGLE_DEFAULTS) + defaults.update(overrides) + toggles = SimpleNamespace(**defaults) + for key, value in overrides.items(): + print(f"toggle override: {key} = {value}") + + segments = [int(s) for s in args.segments.split(",") if s.strip() != ""] + buffers_list = [] + for seg in segments: + identifiers = resolve_segment_identifier(args.route, seg, args.data_dir) + if not identifiers: + print(f"No data found for {args.route} seg {seg}", file=sys.stderr) + return 1 + print(f"Loading seg {seg}: {identifiers}") + bufs = load_buffers(identifiers, SERVICES) + if not any(bufs[s] for s in SERVICES): + print(f"No messages loaded for seg {seg}.", file=sys.stderr) + return 1 + buffers_list.append(bufs) + + bufs = merge_buffers(buffers_list) + all_min = min(t for s in SERVICES for t, _ in bufs[s]) + all_max = max(t for s in SERVICES for t, _ in bufs[s]) + start = all_min if args.start is None else all_min + args.start + end = all_max if args.end is None else min(all_max, all_min + args.end) + if end - start < DT_MDL: + print(f"Empty time window [{start:.1f}, {end:.1f}].", file=sys.stderr) + return 1 + grid = np.arange(start, end, DT_MDL) + print(f"Window: {start - all_min:.1f}s -> {end - all_min:.1f}s ({len(grid)} frames at {1 / DT_MDL:.0f} Hz)") + t0 = grid - start + + out = run_forensic(grid, bufs, toggles) + + if args.csv: + import csv + csv_path = Path(args.csv) + csv_path.parent.mkdir(parents=True, exist_ok=True) + top_cols = ["v_ego", "v_cruise", "a_chill", "a_exp", "hem_a", "hem_authority", + "lead_status", "lead_d_rel", "lead_v_lead", "a_ego", + "brake_pressed", "gas_pressed", "logged_aTarget"] + diag_cols = ["hem_" + k for k in DIAG_KEYS] + ["hem_regime", "hem_standstill"] + seen = set() + unique_cols = [] + for c in ["t"] + top_cols + diag_cols: + if c not in seen: + seen.add(c) + unique_cols.append(c) + with open(csv_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow(unique_cols) + for i in range(len(t0)): + row = [f"{t0[i]:.3f}"] + for c in unique_cols[1:]: + row.append(f"{out[c][i]:.4f}") + writer.writerow(row) + print(f"Saved CSV to {csv_path}") + + brake_clusters = hard_brake_clusters(out) + if brake_clusters: + main_brake = max(brake_clusters, key=len) + lead_sec = int(args.window / DT_MDL) + sl = slice(max(0, main_brake[0] - lead_sec), + min(len(t0), main_brake[-1] + int(3 / DT_MDL))) + if sl.stop - sl.start > 600: + mid = (sl.start + sl.stop) // 2 + sl = slice(max(0, mid - 300), min(len(t0), mid + 300)) + print(f"\nDetected {len(brake_clusters)} driver hard-brake interventions.") + print(f"Focusing on the main one at t={t0[main_brake[0]]:.1f}s..{t0[main_brake[-1]]:.1f}s; " + "use --start/--end to zoom elsewhere.") + mode, stats = classify_failure(out, sl, main_brake[0]) + focus_incidents = main_brake + else: + print("\nNo hard driver brake (a_ego < -2.0) found in the window.") + print("Falling back to coast-through detection (vision saw a stop but HEM coasted).") + incident_idxs = detect_incidents(out, t0) + sl, focus_incidents = window_around(incident_idxs, t0, out, span_sec=args.window) + if sl is None: + sl = slice(0, len(t0)) + mode, stats = None, {} + + print_forensic_log(out, sl, t0) + print_root_cause(out, t0, focus_incidents, sl, mode, stats) + plot_results(out, t0, args, focus_incidents if focus_incidents else []) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())