diff --git a/tools/replay/hem_stop_analyzer.py.py b/tools/replay/hem_stop_analyzer.py.py new file mode 100644 index 000000000..469bace27 --- /dev/null +++ b/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() \ No newline at end of file