mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-10 02:03:57 +08:00
Add HEM telemetry exporter and update forensic analysis tools
- Introduced `export_hem_telemetry.py` to extract and save telemetry data from target segments into a JSON file, including key selfdriveState fields. - Modified `hem_forensic.py` to incorporate new diagnostic keys and update the logic for authority and stop detection. - Enhanced `hem_stop_analyzer.py` to analyze stop detection failures with new metrics and improved comments for clarity. - Updated `mode_sim.py` to reflect changes in hybrid mode updates and authority handling.
This commit is contained in:
Prabhaav Pillai
@@ -0,0 +1,198 @@
|
||||
#!/usr/bin/env python3
|
||||
"""HEM Telemetry Exporter.
|
||||
|
||||
Extracts real telemetry from target segments and saves them to a portable JSON file,
|
||||
including authoritative selfdriveState fields (state, active, experimentalMode, alert).
|
||||
"""
|
||||
import os
|
||||
import sys
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
# Add openpilot paths
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[2]))
|
||||
|
||||
from openpilot.tools.lib.logreader import LogReader, ReadMode
|
||||
|
||||
ROUTES_TO_EXPORT = [
|
||||
("afb7ef2ed593d651/000000b3--9c3d58d585", 10, "Pure Exp Stop Baseline"),
|
||||
("afb7ef2ed593d651/000000b3--9c3d58d585", 11, "Pure Exp Stop Baseline 2"),
|
||||
("afb7ef2ed593d651/000000b3--9c3d58d585", 3, "Stop Sign"),
|
||||
("afb7ef2ed593d651/000000b5--8ee86fef97", 0, "Stop Sign"),
|
||||
("afb7ef2ed593d651/000000b7--9bfbaf247a", 1, "Stop Sign"),
|
||||
("afb7ef2ed593d651/000000b9--976a3b50cb", 6, "Red Light"),
|
||||
]
|
||||
|
||||
def serialize_model(model_msg):
|
||||
"""Extracts the velocity, position, and acceleration lists from modelV2."""
|
||||
try:
|
||||
v_list = list(model_msg.velocity.x)
|
||||
except Exception:
|
||||
v_list = []
|
||||
try:
|
||||
x_list = list(model_msg.position.x)
|
||||
except Exception:
|
||||
x_list = []
|
||||
try:
|
||||
a_list = list(model_msg.acceleration.x)
|
||||
except Exception:
|
||||
a_list = []
|
||||
return {"velocity": v_list, "position": x_list, "acceleration": a_list}
|
||||
|
||||
def export_routes():
|
||||
exported_data = {}
|
||||
|
||||
for route, seg, label in ROUTES_TO_EXPORT:
|
||||
route_clean = route.replace("|", "/")
|
||||
print(f"\nProcessing {route_clean} segment {seg} ({label})...")
|
||||
|
||||
local_paths = [
|
||||
Path(f"/data/media/0/realdata/{route_clean}--{seg}"),
|
||||
Path(os.path.expanduser(f"~/.comma/media/0/realdata/{route_clean}--{seg}")),
|
||||
Path(f"./{route_clean}--{seg}"),
|
||||
]
|
||||
|
||||
lr = None
|
||||
for path in local_paths:
|
||||
rlog_file = path / "rlog"
|
||||
if rlog_file.exists():
|
||||
lr = LogReader(str(rlog_file))
|
||||
break
|
||||
|
||||
if lr is None:
|
||||
dongle_id, log_id = route_clean.split("/", 1)
|
||||
comma_id = f"{dongle_id}|{log_id}/{seg}"
|
||||
try:
|
||||
lr = LogReader(comma_id, default_mode=ReadMode.RLOG)
|
||||
except Exception as e:
|
||||
print(f"Failed to fetch {comma_id}: {e}")
|
||||
continue
|
||||
|
||||
car_state_msgs = []
|
||||
model_msgs = []
|
||||
splan_msgs = []
|
||||
lplan_msgs = []
|
||||
selfdrive_msgs = []
|
||||
controls_state_msgs = []
|
||||
car_control_msgs = []
|
||||
|
||||
try:
|
||||
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))
|
||||
elif which == "selfdriveState":
|
||||
selfdrive_msgs.append((t, msg.selfdriveState))
|
||||
elif which == "controlsState":
|
||||
controls_state_msgs.append((t, msg.controlsState))
|
||||
elif which == "carControl":
|
||||
car_control_msgs.append((t, msg.carControl))
|
||||
except Exception as e:
|
||||
print(f"Error reading log: {e}")
|
||||
continue
|
||||
|
||||
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])
|
||||
selfdrive_msgs.sort(key=lambda x: x[0])
|
||||
controls_state_msgs.sort(key=lambda x: x[0])
|
||||
car_control_msgs.sort(key=lambda x: x[0])
|
||||
|
||||
frames = []
|
||||
for t_model, model_msg in model_msgs:
|
||||
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)
|
||||
sd = next((m for t, m in reversed(selfdrive_msgs) if t <= t_model), None)
|
||||
ctrl = next((m for t, m in reversed(controls_state_msgs) if t <= t_model), None)
|
||||
cc = next((m for t, m in reversed(car_control_msgs) if t <= t_model), None)
|
||||
|
||||
if cs is None:
|
||||
continue
|
||||
|
||||
# Engagement & State flags
|
||||
enabled = False
|
||||
state = 0
|
||||
active = False
|
||||
exp_mode = False
|
||||
alert = ""
|
||||
|
||||
if sd is not None:
|
||||
enabled = bool(sd.enabled)
|
||||
state = int(sd.state.raw)
|
||||
active = bool(sd.active)
|
||||
exp_mode = bool(getattr(sd, "experimentalMode", False))
|
||||
alert = str(getattr(sd, "alertText1", ""))
|
||||
elif ctrl is not None:
|
||||
enabled = bool(ctrl.enabled)
|
||||
state = 2 if enabled else 0
|
||||
active = enabled
|
||||
|
||||
# Fallback starpilotPlan experimental mode check if available
|
||||
if splan is not None and hasattr(splan, "experimentalMode"):
|
||||
exp_mode = exp_mode or bool(splan.experimentalMode)
|
||||
|
||||
cc_enabled = bool(getattr(cc, "enabled", False))
|
||||
cc_brake = 0.0
|
||||
cc_accel = None
|
||||
if cc is not None:
|
||||
act = getattr(cc, "actuators", None)
|
||||
if act is not None:
|
||||
cc_brake = float(getattr(act, "brake", 0.0) or 0.0)
|
||||
cc_accel = getattr(act, "accel", None)
|
||||
|
||||
op_braking = bool(cc_enabled and cc_brake > 0.05)
|
||||
|
||||
# Manual driver brake (pedal pressed and not openpilot commanding the brake)
|
||||
if cc is not None:
|
||||
manual_brake = bool(cs.brakePressed) and not op_braking
|
||||
else:
|
||||
manual_brake = bool(cs.brakePressed)
|
||||
|
||||
frames.append({
|
||||
"t": t_model,
|
||||
"v_ego": float(cs.vEgo),
|
||||
"v_cruise": float(getattr(splan, "vCruise", getattr(cs, "vCruise", 0.0))),
|
||||
"a_chill": float(getattr(lplan, "aTarget", 0.0)),
|
||||
"a_exp": float(getattr(model_msg.action, "desiredAcceleration", 0.0)),
|
||||
"model_trajectory": serialize_model(model_msg),
|
||||
"enabled": enabled,
|
||||
"state": state,
|
||||
"active": active,
|
||||
"exp_mode": exp_mode,
|
||||
"alert": alert,
|
||||
"brake_pressed": manual_brake,
|
||||
"brake_raw": bool(cs.brakePressed),
|
||||
"op_braking": op_braking,
|
||||
"op_brake_cmd": cc_brake,
|
||||
"op_accel_cmd": None if cc_accel is None else float(cc_accel),
|
||||
"gas_pressed": bool(cs.gasPressed),
|
||||
})
|
||||
|
||||
if frames:
|
||||
t0 = frames[0]["t"]
|
||||
for f in frames:
|
||||
f["t"] = f["t"] - t0
|
||||
|
||||
key_name = f"{route_clean.split('/')[-1][:8]}_s{seg}"
|
||||
exported_data[key_name] = {
|
||||
"label": label,
|
||||
"frames": frames,
|
||||
}
|
||||
print(f"Successfully processed {len(frames)} frames for {key_name}.")
|
||||
|
||||
output_file = "hem_routes_telemetry.json"
|
||||
with open(output_file, "w") as f:
|
||||
json.dump(exported_data, f, indent=2)
|
||||
print(f"\nSaved export payload to {output_file}")
|
||||
|
||||
if __name__ == "__main__":
|
||||
export_routes()
|
||||
@@ -68,27 +68,19 @@ SERVICES = {
|
||||
DIAG_KEYS = [
|
||||
# inputs
|
||||
"v_ego", "v_cruise", "a_chill", "a_exp",
|
||||
"should_stop_chill", "should_stop_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",
|
||||
"has_full_trajectory", "v_horizon", "v_short", "v_min",
|
||||
"speed_drop_ratio", "model_decel_strength",
|
||||
"raw_vision_metric", "w_target", "w_vision",
|
||||
# departure / override
|
||||
"lead_departing", "vision_departing", "driver_override", "is_departing",
|
||||
"horizon_stopping",
|
||||
# fusion
|
||||
"a_brake_fused", "a_throttle_fused", "is_stopping_event", "exp_dominant",
|
||||
# standstill handshake
|
||||
"standstill_intent", "should_stop_fused", "a_out",
|
||||
]
|
||||
REGIME_KEYS = ["regime", "standstill"]
|
||||
|
||||
@@ -152,12 +144,18 @@ def run_forensic(grid, bufs, toggles):
|
||||
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"):
|
||||
if k in ("should_stop_chill", "should_stop_exp", "lead_status", "has_full_trajectory",
|
||||
"lead_departing", "vision_departing", "driver_override", "is_departing",
|
||||
"horizon_stopping", "is_stopping_event", "exp_dominant", "standstill_intent",
|
||||
"should_stop_fused"):
|
||||
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)
|
||||
# Gates dropped in the new HEM design map onto the surviving output signal.
|
||||
out["hem_a_anchored"] = np.full(n, np.nan)
|
||||
out["hem_a_safe"] = np.zeros(n)
|
||||
|
||||
real_monotonic = time.monotonic
|
||||
fake_clock = [0.0]
|
||||
@@ -298,7 +296,18 @@ def run_forensic(grid, bufs, toggles):
|
||||
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)
|
||||
should_stop_exp = False
|
||||
if model_v2 is not None:
|
||||
try:
|
||||
should_stop_exp = bool(getattr(getattr(model_v2, "action", None), "shouldStop", False))
|
||||
except Exception:
|
||||
should_stop_exp = False
|
||||
a_hem, should_stop_fused = hybrid.update(
|
||||
v_ego=v_ego, v_cruise=v_cruise, lead_one=lead, model_v2=model_v2,
|
||||
a_chill=a_chill, a_exp=a_exp,
|
||||
should_stop_exp=should_stop_exp,
|
||||
should_stop_chill=bool(getattr(lplan, "shouldStop", False)),
|
||||
)
|
||||
|
||||
model_v0 = v_ego
|
||||
if model_v2 is not None:
|
||||
@@ -315,7 +324,7 @@ def run_forensic(grid, bufs, toggles):
|
||||
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["hem_authority"][i] = hybrid.w_vision
|
||||
out["lead_status"][i] = lead.status
|
||||
out["lead_d_rel"][i] = lead.dRel
|
||||
out["lead_v_lead"][i] = lead.vLead
|
||||
@@ -335,6 +344,8 @@ def run_forensic(grid, bufs, toggles):
|
||||
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))
|
||||
out["hem_a_anchored"][i] = out["hem_a"][i] if out["hem_standstill"][i] else np.nan
|
||||
out["hem_a_safe"][i] = out["hem_a"][i]
|
||||
finally:
|
||||
time.monotonic = real_monotonic
|
||||
|
||||
@@ -357,7 +368,7 @@ def detect_incidents(out, 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]
|
||||
ae_eff = out["hem_a_exp"][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)
|
||||
@@ -380,7 +391,7 @@ def classify_failure(out, sl, i_brake):
|
||||
if pre.stop <= pre.start:
|
||||
return "UNKNOWN", {}
|
||||
v = out["v_ego"][pre]
|
||||
exp_eff = out["hem_a_exp_effective"][pre]
|
||||
exp_eff = out["hem_a_exp"][pre]
|
||||
wv = out["hem_w_vision"][pre]
|
||||
hem = out["hem_a"][pre]
|
||||
chill = out["hem_a_chill"][pre]
|
||||
@@ -478,9 +489,9 @@ def print_forensic_log(out, sl, t0):
|
||||
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_a_exp'][i]:6.2f}",
|
||||
f"{out['hem_w_vision'][i]:5.2f}",
|
||||
f"{out['hem_alpha_exp'][i]:5.2f}",
|
||||
f"{out['hem_authority'][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}",
|
||||
@@ -606,8 +617,8 @@ def plot_results(out, t0, args, incident_idxs):
|
||||
|
||||
# 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].plot(t0, out["hem_authority"], color="tab:purple", lw=1.3, label="exp authority")
|
||||
axs[2].plot(t0, out["hem_a_exp"], color="tab:olive", lw=1.0, ls="--", label="exp 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")
|
||||
|
||||
@@ -128,7 +128,7 @@ def simulate_hem(data_frames):
|
||||
lead = MockLead(status=False)
|
||||
|
||||
# Execute state update
|
||||
a_out = controller.update(
|
||||
a_out, should_stop_fused = controller.update(
|
||||
v_ego=frame["v_ego"],
|
||||
v_cruise=frame["v_cruise"],
|
||||
lead_one=lead,
|
||||
@@ -139,6 +139,7 @@ def simulate_hem(data_frames):
|
||||
|
||||
diag = dict(controller.diag)
|
||||
diag["a_out"] = float(a_out)
|
||||
diag["should_stop_fused"] = bool(should_stop_fused)
|
||||
diag["t_rel"] = frame["t"] - data_frames[0]["t"]
|
||||
sim_results.append(diag)
|
||||
|
||||
@@ -155,10 +156,10 @@ def analyze_failures(route_str, segment, label, 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)
|
||||
# Identify frames with a stop visible to vision (horizon stop or exp stop flag)
|
||||
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):
|
||||
if r.get("horizon_stopping", False) or r.get("should_stop_exp", False) or r.get("w_vision", 0.0) > 0.1:
|
||||
active_frames.append(idx)
|
||||
|
||||
if not active_frames:
|
||||
@@ -181,22 +182,22 @@ def analyze_failures(route_str, segment, label, results):
|
||||
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:
|
||||
if r.get("w_vision", 0.0) < 0.2 and r.get("v_ego", 0.0) > 2.0 and r.get("horizon_stopping", False):
|
||||
latch_decays += 1
|
||||
|
||||
# 2. Check for tracked stop distance wiping out/clearing while speed is high
|
||||
# 2. Check for stop detection being cleared while speed is still 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):
|
||||
if prev_r.get("horizon_stopping", False) and not r.get("horizon_stopping", False):
|
||||
if r.get("v_ego", 0.0) > 1.0 and not r.get("is_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:
|
||||
# 3. Check for departure lockout firing while still approaching a predicted stop
|
||||
if r.get("is_departing", False) and r.get("v_ego", 0.0) > 1.5 and r.get("horizon_stopping", False):
|
||||
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:
|
||||
# 4. Check if the brake floor collapsed near the stop line
|
||||
if r.get("a_brake_fused", 0.0) > -0.1 and r.get("v_ego", 0.0) > 1.0 and r.get("horizon_stopping", False):
|
||||
kinematic_collapses += 1
|
||||
|
||||
# Determine primary failure mode
|
||||
@@ -204,11 +205,11 @@ def analyze_failures(route_str, segment, label, results):
|
||||
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")
|
||||
failure_modes.append("Stop Detection Cleared Early")
|
||||
if early_departures > 5:
|
||||
failure_modes.append("Early Departure Lockout Bypass (departing=True while approaching)")
|
||||
failure_modes.append("Early Departure Lockout Bypass (is_departing while approaching)")
|
||||
if kinematic_collapses > 5:
|
||||
failure_modes.append("Kinematic Stop Floor Collapse near stop line")
|
||||
failure_modes.append("Kinematic Brake 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."
|
||||
@@ -255,17 +256,13 @@ def run_suite():
|
||||
# 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("1. Stop Detection Cleared on Model Re-acceleration:\n")
|
||||
f.write(" When the trajectory velocity endpoint flips positive (a_exp > 0.1, v_horizon > 1.2)\n")
|
||||
f.write(" the 'vision_departing' / 'is_departing' signal fires TRUE and clears 'horizon_stopping'\n")
|
||||
f.write(" while the car is still traveling at speed close to the line, dropping the brake floor.\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(" If the model stops outputting a low velocity endpoint, even briefly, the vision weight\n")
|
||||
f.write(" decays toward zero and unlocks cruise throttle while still closing on the stop.\n\n")
|
||||
|
||||
f.write("DETAILED SEGMENT TELEMETRY BREAKDOWN:\n")
|
||||
f.write("-------------------------------------\n")
|
||||
@@ -294,7 +291,7 @@ def run_suite():
|
||||
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]
|
||||
a_kin = [r["a_brake_fused"] for r in results]
|
||||
|
||||
# Left column: Speeds and Latch activations
|
||||
ax_l = axes[idx, 0]
|
||||
|
||||
@@ -128,7 +128,7 @@ def simulate_hem(data_frames):
|
||||
lead = MockLead(status=False)
|
||||
|
||||
# Execute state update
|
||||
a_out = controller.update(
|
||||
a_out, should_stop_fused = controller.update(
|
||||
v_ego=frame["v_ego"],
|
||||
v_cruise=frame["v_cruise"],
|
||||
lead_one=lead,
|
||||
@@ -139,6 +139,7 @@ def simulate_hem(data_frames):
|
||||
|
||||
diag = dict(controller.diag)
|
||||
diag["a_out"] = float(a_out)
|
||||
diag["should_stop_fused"] = bool(should_stop_fused)
|
||||
diag["t_rel"] = frame["t"] - data_frames[0]["t"]
|
||||
sim_results.append(diag)
|
||||
|
||||
@@ -155,10 +156,10 @@ def analyze_failures(route_str, segment, label, 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)
|
||||
# Identify frames with a stop visible to vision (horizon stop or exp stop flag)
|
||||
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):
|
||||
if r.get("horizon_stopping", False) or r.get("should_stop_exp", False) or r.get("w_vision", 0.0) > 0.1:
|
||||
active_frames.append(idx)
|
||||
|
||||
if not active_frames:
|
||||
@@ -181,22 +182,22 @@ def analyze_failures(route_str, segment, label, results):
|
||||
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:
|
||||
if r.get("w_vision", 0.0) < 0.2 and r.get("v_ego", 0.0) > 2.0 and r.get("horizon_stopping", False):
|
||||
latch_decays += 1
|
||||
|
||||
# 2. Check for tracked stop distance wiping out/clearing while speed is high
|
||||
# 2. Check for stop detection being cleared while speed is still 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):
|
||||
if prev_r.get("horizon_stopping", False) and not r.get("horizon_stopping", False):
|
||||
if r.get("v_ego", 0.0) > 1.0 and not r.get("is_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:
|
||||
# 3. Check for departure lockout firing while still approaching a predicted stop
|
||||
if r.get("is_departing", False) and r.get("v_ego", 0.0) > 1.5 and r.get("horizon_stopping", False):
|
||||
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:
|
||||
# 4. Check if the brake floor collapsed near the stop line
|
||||
if r.get("a_brake_fused", 0.0) > -0.1 and r.get("v_ego", 0.0) > 1.0 and r.get("horizon_stopping", False):
|
||||
kinematic_collapses += 1
|
||||
|
||||
# Determine primary failure mode
|
||||
@@ -204,11 +205,11 @@ def analyze_failures(route_str, segment, label, results):
|
||||
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")
|
||||
failure_modes.append("Stop Detection Cleared Early")
|
||||
if early_departures > 5:
|
||||
failure_modes.append("Early Departure Lockout Bypass (departing=True while approaching)")
|
||||
failure_modes.append("Early Departure Lockout Bypass (is_departing while approaching)")
|
||||
if kinematic_collapses > 5:
|
||||
failure_modes.append("Kinematic Stop Floor Collapse near stop line")
|
||||
failure_modes.append("Kinematic Brake 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."
|
||||
@@ -255,17 +256,13 @@ def run_suite():
|
||||
# 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("1. Stop Detection Cleared on Model Re-acceleration:\n")
|
||||
f.write(" When the trajectory velocity endpoint flips positive (a_exp > 0.1, v_horizon > 1.2)\n")
|
||||
f.write(" the 'vision_departing' / 'is_departing' signal fires TRUE and clears 'horizon_stopping'\n")
|
||||
f.write(" while the car is still traveling at speed close to the line, dropping the brake floor.\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(" If the model stops outputting a low velocity endpoint, even briefly, the vision weight\n")
|
||||
f.write(" decays toward zero and unlocks cruise throttle while still closing on the stop.\n\n")
|
||||
|
||||
f.write("DETAILED SEGMENT TELEMETRY BREAKDOWN:\n")
|
||||
f.write("-------------------------------------\n")
|
||||
@@ -294,7 +291,7 @@ def run_suite():
|
||||
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]
|
||||
a_kin = [r["a_brake_fused"] for r in results]
|
||||
|
||||
# Left column: Speeds and Latch activations
|
||||
ax_l = axes[idx, 0]
|
||||
|
||||
@@ -532,8 +532,11 @@ def run_simulation(grid, bufs, toggles):
|
||||
a_exp = 0.0
|
||||
|
||||
# Hybrid Experimental Mode continuous fusion
|
||||
a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp, t_follow=t_follow)
|
||||
authority = hybrid.exp_authority
|
||||
a_hem, _should_stop_fused = hybrid.update(
|
||||
v_ego=v_ego, v_cruise=v_cruise, lead_one=lead, model_v2=model_v2,
|
||||
a_chill=a_chill, a_exp=a_exp,
|
||||
)
|
||||
authority = hybrid.w_vision
|
||||
|
||||
model_v0 = v_ego
|
||||
if model_v2 is not None:
|
||||
|
||||
Reference in New Issue
Block a user