diff --git a/tools/replay/hem_inspector.py b/tools/replay/hem_inspector.py new file mode 100644 index 000000000..1ab756202 --- /dev/null +++ b/tools/replay/hem_inspector.py @@ -0,0 +1,893 @@ +#!/usr/bin/env python3 +"""HEM Live Inspector & Diagnostic Studio. + +An interactive, live-scrubbing telemetry debugger for Hybrid Experimental Mode (HEM) +and Pure Experimental Mode comparisons. +""" +from __future__ import annotations + +import argparse +import bisect +import http.server +import json +import os +import socketserver +import sys +import threading +import time +import urllib.parse +import webbrowser +from pathlib import Path +from types import SimpleNamespace +from typing import Any + +import numpy as np + +# Ensure openpilot root is in sys.path +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.lib.logreader import LogReader, ReadMode, parse_direct, parse_indirect +from openpilot.tools.lib.route import SegmentRange + +SERVICES = { + "carState", "radarState", "starpilotRadarState", "modelV2", + "longitudinalPlan", "selfdriveState", "starpilotPlan", "starpilotCarState", + "controlsState", "carParams", "liveParameters", "carControl", +} + + +def as_lead(raw): + if raw is None: + return SimpleNamespace(status=False, dRel=150.0, yRel=0.0, vRel=0.0, aRel=0.0, + vLead=0.0, vLeadK=0.0, dPath=0.0, vLat=0.0, aLeadK=0.0, + aLeadTau=1.5, fcw=False, modelProb=0.0, radar=False) + return SimpleNamespace( + status=bool(getattr(raw, "status", False)), + dRel=float(getattr(raw, "dRel", 150.0)), + yRel=float(getattr(raw, "yRel", 0.0)), + vRel=float(getattr(raw, "vRel", 0.0)), + aRel=float(getattr(raw, "aRel", 0.0)), + vLead=float(getattr(raw, "vLead", 0.0)), + vLeadK=float(getattr(raw, "vLeadK", getattr(raw, "vLead", 0.0))), + dPath=float(getattr(raw, "dPath", 0.0)), + vLat=float(getattr(raw, "vLat", 0.0)), + aLeadK=float(getattr(raw, "aLeadK", 0.0)), + aLeadTau=float(getattr(raw, "aLeadTau", 1.5)), + fcw=bool(getattr(raw, "fcw", False)), + modelProb=float(getattr(raw, "modelProb", 0.0)), + radar=bool(getattr(raw, "radar", False)), + ) + + +def load_route_data(route_str: str, segment: int, data_dir: str | None = None): + print(f"[HEM Inspector] Resolving route: {route_str} (segment {segment})...") + direct = parse_direct(route_str) + if direct is not None: + identifiers = [str(direct)] + else: + parsed = parse_indirect(route_str) + sr = SegmentRange(parsed) + route_name = sr.route_name.replace("/", "|") + identifiers = [f"{route_name}--{segment}"] + + bufs = {s: [] for s in SERVICES} + for identifier in identifiers: + try: + lr = LogReader(identifier, default_mode=ReadMode.RLOG) + except Exception as exc: + print(f"[HEM Inspector] Error reading {identifier}: {exc}", file=sys.stderr) + return None + + for evt in lr: + which = evt.which() + if which in SERVICES: + t = evt.logMonoTime * 1e-9 + bufs[which].append((t, getattr(evt, which))) + + for s in SERVICES: + bufs[s].sort(key=lambda x: x[0]) + + if not any(bufs[s] for s in SERVICES): + print("[HEM Inspector] No telemetry messages found in segment.", file=sys.stderr) + return None + + return bufs + + +def run_hem_replay(bufs, start_sec: float | None, end_sec: float | None, toggles: Any): + 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, + ) + + req_services = [s for s in ("modelV2", "carState") if len(bufs.get(s, []))] + all_min = max(bufs[s][0][0] for s in req_services) if req_services else min(t for s in SERVICES for t, _ in bufs[s] if len(bufs[s])) + all_max = max(t for s in SERVICES for t, _ in bufs[s] if len(bufs[s])) + start = all_min if start_sec is None else all_min + start_sec + end = all_max if end_sec is None else min(all_max, all_min + end_sec) + grid = np.arange(start, end, DT_MDL) + + planner = LongitudinalPlanner(cp_candidate, dt=DT_MDL) + hybrid = HybridExperimentalMode() + hybrid.record_diag = True + hybrid.set_tuning(getattr(toggles, "hybrid_exp_bias", 0.0), getattr(toggles, "hybrid_vision_brake_sensitivity", 1.0)) + + frames = [] + stop_events = [] + + real_monotonic = time.monotonic + fake_clock = [0.0] + time.monotonic = lambda: fake_clock[0] + + try: + for i, t in enumerate(grid): + fake_clock[0] = float(i) * DT_MDL + t_rel = round(t - all_min, 3) + + car_raw = latest("carState", t) + model_v2 = latest("modelV2", t) + if model_v2 is None or car_raw is None: + continue + v_ego = max(float(getattr(car_raw, "vEgo", 0.0)), 0.0) + car = SimpleNamespace( + vEgo=v_ego, + vEgoCluster=max(float(getattr(car_raw, "vEgoCluster", v_ego)), 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)), + ) + + radar = latest("radarState", t) + lead1 = as_lead(getattr(radar, "leadOne", None) if radar else None) + lead2 = as_lead(getattr(radar, "leadTwo", None) if radar else None) + + lplan = latest("longitudinalPlan", t) + sds = latest("selfdriveState", t) + splan = latest("starpilotPlan", t) + scs = latest("starpilotCarState", t) + live_params = latest("liveParameters", t) + car_control = latest("carControl", t) + controls_state = latest("controlsState", t) + + # Logged in-drive states + logged_exp_mode = bool(getattr(sds, "experimentalMode", False)) + logged_a_target = float(getattr(lplan, "aTarget", 0.0)) if lplan else 0.0 + logged_should_stop = bool(getattr(lplan, "shouldStop", False)) if lplan else False + + 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) + v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS if 0 < v_cruise_kph < V_CRUISE_UNSET else v_ego + + tracking_lead = bool(getattr(splan, "trackingLead", False) or getattr(lplan, "hasLead", False)) + t_follow = float(getattr(splan, "tFollow", 1.45)) + forcing_stop = bool(getattr(splan, "forcingStop", False)) + + sm_dict = { + "carState": car, + "radarState": SimpleNamespace(leadOne=lead1, leadTwo=lead2), + "starpilotRadarState": SimpleNamespace(leadLeft=as_lead(None), leadRight=as_lead(None)), + "starpilotCarState": SimpleNamespace( + trafficModeEnabled=bool(getattr(scs, "trafficModeEnabled", False)), + alwaysOnLateralEnabled=bool(getattr(scs, "alwaysOnLateralEnabled", False)), + accelPressed=bool(getattr(scs, "accelPressed", False)), + ), + "selfdriveState": SimpleNamespace( + enabled=bool(getattr(sds, "enabled", False)), + experimentalMode=False, + personality=int(getattr(getattr(sds, "personality", None), "raw", 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=forcing_stop, + 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=getattr(controls_state, "longControlState", LongCtrlState.pid), + forceDecel=bool(getattr(controls_state, "forceDecel", False)), + curvature=float(getattr(controls_state, "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": cp_candidate, + } + + # 1. Update Chill Planner (ACC MPC Baseline) + planner.update(sm_dict, toggles) + a_chill = float(planner.output_a_target) + should_stop_chill = bool(planner.output_should_stop) + + # 2. Extract Vision NN Action Intent & Horizon Metrics + a_exp = 0.0 + should_stop_exp = False + stop_line_prob = 0.0 + stop_line_dist = 0.0 + if model_v2 is not None: + act = getattr(model_v2, "action", None) + if act is not None: + a_exp = float(getattr(act, "desiredAcceleration", 0.0)) + should_stop_exp = bool(getattr(act, "shouldStop", False)) + + stop_line = getattr(model_v2, "stopLine", None) + if stop_line is not None: + stop_line_prob = float(getattr(stop_line, "prob", 0.0)) + stop_line_dist = float(getattr(stop_line, "distance", 0.0)) + + # 3. Step HEM Fusion Machine + active_lead = lead2 if planner.mpc.source == "lead1" else lead1 + a_fused, should_stop_fused = hybrid.update( + v_ego=v_ego, v_cruise=v_cruise, lead_one=active_lead, + model_v2=model_v2, a_chill=a_chill, a_exp=a_exp, + should_stop_exp=should_stop_exp, should_stop_chill=should_stop_chill, + gas_pressed=car.gasPressed, + ) + + diag_dump = dict(hybrid.diag) if hasattr(hybrid, "diag") else {} + exp_dominant = bool(getattr(hybrid, "last_exp_dominant", False) or hybrid.w_vision > 0.65) + + if should_stop_exp or (exp_dominant and a_exp < -1.0) or (logged_should_stop and logged_exp_mode): + if not stop_events or (t_rel - stop_events[-1]["time"]) > 3.0: + stop_events.append({"time": t_rel, "reason": "Stop Detected" if (should_stop_exp or logged_should_stop) else "Vision Decel"}) + + frame_payload = { + "idx": i, + "t": t_rel, + "v_ego": round(v_ego, 2), + "v_ego_mph": round(v_ego * CV.MS_TO_MPH, 1), + "v_cruise_mph": round(v_cruise * CV.MS_TO_MPH, 1), + "a_ego": round(car.aEgo, 2), + "gas_pressed": car.gasPressed, + "brake_pressed": car.brakePressed, + "standstill": car.standstill, + "logged_drive": { + "experimental_mode": logged_exp_mode, + "a_target": round(logged_a_target, 2), + "should_stop": logged_should_stop, + }, + "lead": { + "status": active_lead.status, + "dRel": round(active_lead.dRel, 1), + "vLead": round(active_lead.vLead, 1), + "aLeadK": round(active_lead.aLeadK, 2), + "radar": active_lead.radar, + }, + "model": { + "should_stop_exp": should_stop_exp, + "a_exp": round(a_exp, 2), + "stop_line_prob": round(stop_line_prob, 3), + "stop_line_dist": round(stop_line_dist, 1), + }, + "planner": { + "a_chill": round(a_chill, 2), + "should_stop_chill": should_stop_chill, + "mpc_source": planner.mpc.source, + "v_desired_now": round(float(planner.v_desired_trajectory[0]), 2) if len(planner.v_desired_trajectory) else 0.0, + }, + "hem": { + "a_fused": round(a_fused, 2), + "should_stop_fused": should_stop_fused, + "w_vision": round(float(hybrid.w_vision), 3), + "exp_dominant": exp_dominant, + "diag": diag_dump, + }, + } + frames.append(frame_payload) + finally: + time.monotonic = real_monotonic + + return frames, stop_events + + +INDEX_HTML = """ + + + +HEM & Pure EXP Live Inspector + + + + +
+
+ 🚘 HEM & Pure EXP Inspector + DRIVE: CHILL + CHILL DOMINANT + +
+
+
+ + + to + + + +
+ + +
+
+ +
+
+
+ + + + + +
+ +
+ 0.00s + 0.00s +
+
+ +
+ +
+
Jump to Events / Stop Candidates
+
+
+ +
+
+ Acceleration Trajectory Comparison (m/s²) + + ━ a_logged (Car) + ■ a_exp (Model) + ■ a_chill (MPC) + ━ a_fused (HEM) + +
+
+ +
+
+ +
+
+
Speed (Ego / Set)
+
0 / 0 mph
+
+
+
Logged Drive Accel
+
0.00 m/s²
+
+
+
Simulated HEM Accel
+
0.00 m/s²
+
+
+
Lead Distance
+
None
+
+
+
+ +
+
+
HEM Authority (Vision Weight)
+
+ 0.00 + Chill Cruise Mode +
+
+
+ +
+
Stop Intent Matrix
+ + + + + + + +
Logged In-Car ModeCHILL
Logged In-Car StopFalse
model.shouldStop (Exp)False
planner.shouldStop (Chill)False
HEM Fused shouldStopFalse
Stop Line Prob / Dist0.00 / 0.0m
+
+ +
+
HEM Internal Diag State Dump
+
+
+
+
+ + + + +""" + + +class HEMServerHandler(http.server.SimpleHTTPRequestHandler): + frames_data: dict[str, Any] = {} + + def do_GET(self): + parsed = urllib.parse.urlparse(self.path) + if parsed.path == "/" or parsed.path == "/index.html": + self.send_response(200) + self.send_header("Content-type", "text/html") + self.end_headers() + self.wfile.write(INDEX_HTML.encode("utf-8")) + elif parsed.path == "/api/data": + self.send_response(200) + self.send_header("Content-type", "application/json") + self.end_headers() + self.wfile.write(json.dumps(self.frames_data).encode("utf-8")) + else: + self.send_response(404) + self.end_headers() + + +def start_server(port: int, data_payload: dict[str, Any]): + HEMServerHandler.frames_data = data_payload + socketserver.TCPServer.allow_reuse_address = True + with socketserver.TCPServer(("127.0.0.1", port), HEMServerHandler) as httpd: + url = f"http://127.0.0.1:{port}" + print(f"\n[HEM Inspector] UI running live at: {url}") + threading.Timer(0.8, lambda: webbrowser.open(url)).start() + try: + httpd.serve_forever() + except KeyboardInterrupt: + print("\n[HEM Inspector] Server stopped.") + + +def main(argv=None): + parser = argparse.ArgumentParser(description="HEM Live Inspector & Diagnostics") + parser.add_argument("route", help="Route identifier (e.g. dongle|2023-07-27--13-01-19 or dongle/log_id/segment)") + parser.add_argument("--segment", type=int, default=0, help="Segment index") + parser.add_argument("--start", type=float, default=None, help="Start time in seconds (segment-relative)") + parser.add_argument("--end", type=float, default=None, help="End time in seconds (segment-relative)") + parser.add_argument("--port", type=int, default=8090, help="Web server port (default 8090)") + parser.add_argument("--data_dir", default=None, help="Local directory containing routes") + parser.add_argument("--bias", type=float, default=0.0, help="Override hybrid_exp_bias") + parser.add_argument("--sensitivity", type=float, default=1.0, help="Override hybrid_vision_brake_sensitivity") + + args = parser.parse_args(argv) + + toggles = SimpleNamespace( + hybrid_exp_bias=args.bias, + hybrid_vision_brake_sensitivity=args.sensitivity, + taco_tune=False, + classic_model=False, + tinygrad_model=False, + vEgoStopping=0.05, + radar_takeoffs=False, + lane_change_close_gap=False, + minimum_lane_change_speed=0.0, + model_version=None, + ) + + bufs = load_route_data(args.route, args.segment, args.data_dir) + if bufs is None: + return 1 + + print("[HEM Inspector] Replaying route segment through HEM & Longitudinal Planner...") + frames, stop_events = run_hem_replay(bufs, args.start, args.end, toggles) + print(f"[HEM Inspector] Processed {len(frames)} frames. Identified {len(stop_events)} stop/decel event candidates.") + + start_server(args.port, {"frames": frames, "events": stop_events}) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) \ No newline at end of file