trying to figure out why rolling stop signs

tools/replay/hem_stop_analyzer.py.py

@@ -0,0 +1,328 @@
+#!/usr/bin/env python3
+import os
+import sys
+import numpy as np
+import matplotlib.pyplot as plt
+from pathlib import Path
+
+# Add openpilot paths if running from tools/ or other subdirectories
+sys.path.insert(0, str(Path(__file__).resolve().parents[2]))
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.tools.lib.logreader import LogReader
+from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode
+
+# Define the specific routes and segments where stop-sign or red-light failures occurred
+ROUTES_TO_ANALYZE = [
+  ("afb7ef2ed593d651/000000b3--9c3d58d585", 3, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b3--9c3d58d585", 8, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b3--9c3d58d585", 10, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b3--9c3d58d585", 11, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b4--1a67659212", 1, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b5--8ee86fef97", 0, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b6--711631f3fd", 0, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b7--9bfbaf247a", 1, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b8--e3147dbfc3", 0, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b9--976a3b50cb", 0, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b9--976a3b50cb", 1, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b9--976a3b50cb", 2, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b9--976a3b50cb", 3, "Stop Sign"),
+  ("afb7ef2ed593d651/000000b9--976a3b50cb", 6, "Red Light"),
+]
+
+class MockLead:
+  def __init__(self, status=False, d_rel=150.0, v_lead=0.0):
+    self.status = status
+    self.dRel = float(d_rel)
+    self.vLead = float(v_lead)
+
+def fetch_telemetry(route_str, segment):
+  """Loads specified segment logs. Attempts local lookups first, then falls back to public API."""
+  route_clean = route_str.replace("|", "/")
+  segment_str = f"{segment:02d}" if isinstance(segment, int) else str(segment)
+
+  # Search local paths first
+  local_paths = [
+    Path(f"/data/media/0/realdata/{route_clean}--{segment}"),
+    Path(os.path.expanduser(f"~/.comma/media/0/realdata/{route_clean}--{segment}")),
+    Path(f"./{route_clean}--{segment}"),
+  ]
+
+  lr = None
+  for path in local_paths:
+    rlog_file = path / "rlog"
+    if rlog_file.exists():
+      print(f"Loading local data: {rlog_file}")
+      lr = LogReader(str(rlog_file))
+      break
+
+  if lr is None:
+    # Construct comma public database URL fallback
+    dongle_id, route_sig = route_clean.split("/", 1)
+    remote_url = f"https://commadata2.blob.core.windows.net/commadata2/{dongle_id}/{route_sig}/{segment}/rlog"
+    print(f"Loading remote data: {remote_url}")
+    try:
+      lr = LogReader(remote_url)
+    except Exception as e:
+      print(f"Failed to fetch {remote_url}: {e}")
+      return []
+
+  print("Parsing messages...")
+  car_state_msgs = []
+  model_msgs = []
+  splan_msgs = []
+  lplan_msgs = []
+
+  for msg in lr:
+    which = msg.which()
+    t = msg.logMonoTime * 1e-9
+    if which == "carState":
+      car_state_msgs.append((t, msg.carState))
+    elif which == "modelV2":
+      model_msgs.append((t, msg.modelV2))
+    elif which == "starpilotPlan":
+      splan_msgs.append((t, msg.starpilotPlan))
+    elif which == "longitudinalPlan":
+      lplan_msgs.append((t, msg.longitudinalPlan))
+
+  car_state_msgs.sort(key=lambda x: x[0])
+  model_msgs.sort(key=lambda x: x[0])
+  splan_msgs.sort(key=lambda x: x[0])
+  lplan_msgs.sort(key=lambda x: x[0])
+
+  # Synchronize messages on model 20Hz time-grid
+  data_frames = []
+  for t_model, model_msg in model_msgs:
+    # Find latest corresponding messages
+    cs = next((m for t, m in reversed(car_state_msgs) if t <= t_model), None)
+    splan = next((m for t, m in reversed(splan_msgs) if t <= t_model), None)
+    lplan = next((m for t, m in reversed(lplan_msgs) if t <= t_model), None)
+
+    if cs is None:
+      continue
+
+    v_ego = cs.vEgo
+    v_cruise = getattr(splan, "vCruise", getattr(cs, "vCruise", 0.0))
+    a_chill = getattr(lplan, "aTarget", 0.0)
+    a_exp = getattr(model_msg.action, "desiredAcceleration", 0.0)
+
+    data_frames.append({
+      "t": t_model,
+      "v_ego": v_ego,
+      "v_cruise": v_cruise,
+      "a_chill": a_chill,
+      "a_exp": a_exp,
+      "model": model_msg,
+    })
+
+  return data_frames
+
+def simulate_hem(data_frames):
+  """Simulates the controller over the synchronized messages."""
+  controller = HybridExperimentalMode()
+  controller.record_diag = True
+
+  sim_results = []
+  for frame in data_frames:
+    # Lead initialization
+    lead = MockLead(status=False)
+
+    # Execute state update
+    a_out = controller.update(
+      v_ego=frame["v_ego"],
+      v_cruise=frame["v_cruise"],
+      lead_one=lead,
+      model_v2=frame["model"],
+      a_chill=frame["a_chill"],
+      a_exp=frame["a_exp"],
+    )
+
+    diag = dict(controller.diag)
+    diag["a_out"] = float(a_out)
+    diag["t_rel"] = frame["t"] - data_frames[0]["t"]
+    sim_results.append(diag)
+
+  return sim_results
+
+def analyze_failures(route_str, segment, label, results):
+  """Analyzes decisions inside the critical deceleration window."""
+  total_frames = len(results)
+  if total_frames == 0:
+    return None
+
+  # Find the primary deceleration zone (where v_ego drops, or should have dropped)
+  v_speeds = [r["v_ego"] for r in results]
+  max_v = max(v_speeds)
+  min_v = min(v_speeds)
+
+  # Identify frames with stop signs visible in model (high stop confidence or tracked distance exists)
+  active_frames = []
+  for idx, r in enumerate(results):
+    if (r.get("stop_confidence", 0.0) > 0.1) or (r.get("tracked_stop_dist") is not None):
+      active_frames.append(idx)
+
+  if not active_frames:
+    return {
+      "route": route_str, "segment": segment, "label": label,
+      "outcome": "No stop event detected by model.",
+      "failure_mode": "Model did not predict stop point.",
+    }
+
+  start_idx = min(active_frames)
+  end_idx = max(active_frames)
+
+  # Analyze tracking variables inside the event window
+  latch_decays = 0
+  tracking_resets = 0
+  early_departures = 0
+  kinematic_collapses = 0
+
+  for idx in range(start_idx, end_idx + 1):
+    r = results[idx]
+
+    # 1. Check for premature latch decay (decaying while still moving fast)
+    if r.get("w_vision", 0.0) < 0.2 and r.get("v_ego", 0.0) > 2.0 and r.get("stop_confidence", 0.0) > 0.4:
+      latch_decays += 1
+
+    # 2. Check for tracked stop distance wiping out/clearing while speed is high
+    if idx > start_idx:
+      prev_r = results[idx - 1]
+      if prev_r.get("tracked_stop_dist") is not None and r.get("tracked_stop_dist") is None:
+        if r.get("v_ego", 0.0) > 1.0 and not r.get("vision_departing", False):
+          tracking_resets += 1
+
+    # 3. Check for early departure trigger causing positive creep acceleration override
+    if r.get("departing", False) and r.get("v_ego", 0.0) > 1.5 and r.get("d_stop_calc", 999) > 0.5:
+      early_departures += 1
+
+    # 4. Check if the kinematic decel collapsed near the stop line
+    if r.get("a_kinematic_stop", 0.0) > -0.1 and r.get("v_ego", 0.0) > 1.0 and 0.5 < r.get("tracked_stop_dist", 999) < 8.0:
+      kinematic_collapses += 1
+
+  # Determine primary failure mode
+  failure_modes = []
+  if latch_decays > 5:
+    failure_modes.append("Premature Latch Decay (w_vision collapsed)")
+  if tracking_resets > 0:
+    failure_modes.append("Stop Distance Tracker Cleared Early")
+  if early_departures > 5:
+    failure_modes.append("Early Departure Lockout Bypass (departing=True while approaching)")
+  if kinematic_collapses > 5:
+    failure_modes.append("Kinematic Stop Floor Collapse near stop line")
+
+  failure_mode = " / ".join(failure_modes) if failure_modes else "Weak general deceleration tracking"
+  outcome = f"Blew past stop line. Min speed reached: {min_v:.2f} m/s." if min_v > 0.5 else "Stopped but late/harsh."
+
+  return {
+    "route": route_str,
+    "segment": segment,
+    "label": label,
+    "max_v": max_v,
+    "min_v": min_v,
+    "latch_decays": latch_decays,
+    "tracking_resets": tracking_resets,
+    "early_departures": early_departures,
+    "kinematic_collapses": kinematic_collapses,
+    "outcome": outcome,
+    "failure_mode": failure_mode,
+  }
+
+def run_suite():
+  all_analyses = []
+  plot_data = {}
+
+  for route, seg, label in ROUTES_TO_ANALYZE:
+    print(f"\nAnalyzing {route} segment {seg} ({label})...")
+    frames = fetch_telemetry(route, seg)
+    if not frames:
+      print("No frames retrieved. Skipping.")
+      continue
+
+    results = simulate_hem(frames)
+    analysis = analyze_failures(route, seg, label, results)
+    if analysis:
+      all_analyses.append(analysis)
+      plot_data[f"{route.split('/')[-1][:8]}_s{seg}"] = (frames, results)
+
+  # 1. Export forensic summary text report
+  report_path = "hem_forensic_report.txt"
+  print(f"\nWriting forensic report to {report_path}...")
+  with open(report_path, "w") as f:
+    f.write("=== HYBRID EXPERIMENTAL MODE (HEM) FAILURE CASE STUDY SUMMARY ===\n")
+    f.write(f"Analyzed {len(all_analyses)} routes where vehicle rolled stop-lines\n")
+    f.write("==================================================================\n\n")
+
+    # General findings
+    f.write("COMMON STRUCTURAL ROOT CAUSES IDENTIFIED:\n")
+    f.write("------------------------------------------\n")
+    f.write("1. Stop Tracker Resetting on Model Re-acceleration:\n")
+    f.write("   When stop lines approach index 0, the model's trajectory velocity endpoint\n")
+    f.write("   flip positive (a_exp > 0.1, v_horizon > 0.5). Because 'model_stop_predicted'\n")
+    f.write("   evaluates to False, the 'vision_departing' or 'departing' signal fires TRUE.\n")
+    f.write("   This instantly triggers 'self.tracked_stop_dist = None', deleting the\n")
+    f.write("   kinematic decel floor while the car is still traveling at speed close to the line.\n\n")
+    f.write("2. Insufficient Latch Sustainability (w_vision decays):\n")
+    f.write("   If the model stops outputting a highly confident slow-down endpoint, even briefly,\n")
+    f.write("   the soft latch 'w_vision_filtered' is multiplied by 0.90 or 0.97. If it decays\n")
+    f.write("   below 0.25, the system unlocks the throttle override lockouts, reverting to CCM/chill\n")
+    f.write("   creep commands.\n\n")
+
+    f.write("DETAILED SEGMENT TELEMETRY BREAKDOWN:\n")
+    f.write("-------------------------------------\n")
+    for a in all_analyses:
+      f.write(f"Route: {a['route']} | Seg: {a['segment']} ({a['label']})\n")
+      f.write(f"  • Velocity Profile : {a['max_v']:.1f} m/s -> {a['min_v']:.1f} m/s\n")
+      f.write(f"  • Tracker Resets   : {a['tracking_resets']} frames\n")
+      f.write(f"  • Early Departures : {a['early_departures']} frames\n")
+      f.write(f"  • Outcome          : {a['outcome']}\n")
+      f.write(f"  • Primary Fault    : {a['failure_mode']}\n")
+      f.write("------------------------------------------------------------------\n")
+
+  # 2. Generate tiled plot of critical variables for a subset of failures
+  print("Generating diagnostic plots...")
+  plot_keys = list(plot_data.keys())[:4]  # Plot up to 4 significant failures for space
+  if not plot_keys:
+    return
+
+  fig, axes = plt.subplots(len(plot_keys), 2, figsize=(15, 3 * len(plot_keys)), sharex="col")
+  if len(plot_keys) == 1:
+    axes = np.expand_dims(axes, axis=0)
+
+  for idx, key in enumerate(plot_keys):
+    frames, results = plot_data[key]
+    t = [r["t_rel"] for r in results]
+    v_ego = [r["v_ego"] for r in results]
+    w_vis = [r["w_vision"] for r in results]
+    a_out = [r["a_out"] for r in results]
+    a_kin = [r["a_kinematic_stop"] for r in results]
+
+    # Left column: Speeds and Latch activations
+    ax_l = axes[idx, 0]
+    ax_l.plot(t, v_ego, color="black", lw=1.5, label="v_ego (m/s)")
+    ax_l_twin = ax_l.twinx()
+    ax_l_twin.plot(t, w_vis, color="orange", alpha=0.7, ls="--", label="w_vision (latch)")
+    ax_l.set_title(f"Run: {key} - Speeds", fontsize=10)
+    ax_l.set_ylabel("Speed (m/s)")
+    ax_l_twin.set_ylabel("Latch Active", color="orange")
+    ax_l.grid(alpha=0.3)
+    if idx == 0:
+      ax_l.legend(loc="upper left")
+      ax_l_twin.legend(loc="upper right")
+
+    # Right column: Acceleration commands
+    ax_r = axes[idx, 1]
+    ax_r.plot(t, a_out, color="blue", lw=1.5, label="a_out (fused)")
+    ax_r.plot(t, a_kin, color="red", alpha=0.6, ls=":", label="a_kinematic_stop")
+    ax_r.set_title(f"Run: {key} - Acceleration Commands", fontsize=10)
+    ax_r.set_ylabel("Accel (m/s²)")
+    ax_r.grid(alpha=0.3)
+    if idx == 0:
+      ax_r.legend(loc="upper right")
+
+  plt.tight_layout()
+  plot_out_path = "hem_failures_analysis.png"
+  plt.savefig(plot_out_path, dpi=150)
+  print(f"Diagnostic graph saved to {plot_out_path}")
+
+if __name__ == "__main__":
+  run_suite()