mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-31 13:13:44 +08:00
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.
This commit is contained in:
Prabhaav Pillai
@@ -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
|
||||
@@ -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
|
||||
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}"
|
||||
@@ -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 <dongleId>/<routeId> [--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: <dongleId>/<logId> (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())
|
||||
Reference in New Issue
Block a user