mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-22 17:52:07 +08:00
firestar5683
677 lines
28 KiB
Python
677 lines
28 KiB
Python
#!/usr/bin/env python3
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import math
|
|
from dataclasses import asdict, dataclass, field
|
|
from pathlib import Path
|
|
from typing import Any
|
|
from urllib.parse import urlparse
|
|
|
|
import numpy as np
|
|
|
|
from openpilot.tools.lib.logreader import LogReader, ReadMode, parse_direct, parse_indirect
|
|
from openpilot.tools.lib.route import SegmentRange
|
|
|
|
|
|
ACTIVE_DEADBAND = 0.08
|
|
SOURCE_SWITCH_SCORE = 1.5
|
|
DEFAULT_BOOKMARK_BEFORE = 12.0
|
|
DEFAULT_BOOKMARK_AFTER = 3.0
|
|
|
|
|
|
@dataclass
|
|
class LongitudinalSample:
|
|
segment: int
|
|
time_s: float
|
|
mono_time: int
|
|
long_active: bool = False
|
|
v_ego: float = 0.0
|
|
a_ego: float = 0.0
|
|
gas_pressed: bool = False
|
|
brake_pressed: bool = False
|
|
plan_accel: float = 0.0
|
|
command_accel: float = 0.0
|
|
output_accel: float = 0.0
|
|
p_term: float = 0.0
|
|
i_term: float = 0.0
|
|
f_term: float = 0.0
|
|
long_state: str = "unknown"
|
|
source: str = "unknown"
|
|
should_stop: bool = False
|
|
allow_throttle: bool = True
|
|
allow_brake: bool = True
|
|
has_lead: bool = False
|
|
lead_status: bool = False
|
|
lead_distance: float | None = None
|
|
lead_relative_velocity: float = 0.0
|
|
lead_velocity: float = 0.0
|
|
lead_acceleration: float = 0.0
|
|
lead_probability: float = 0.0
|
|
lead_radar: bool = False
|
|
lead_track_id: int = -1
|
|
lead_two_status: bool = False
|
|
lead_two_distance: float | None = None
|
|
experimental_mode: bool = False
|
|
forcing_stop: bool = False
|
|
red_light: bool = False
|
|
tracking_lead: bool = False
|
|
curve_controller_active: bool = False
|
|
cruise_target: float = 0.0
|
|
desired_follow_distance: float = 0.0
|
|
personality: str = "unknown"
|
|
|
|
|
|
@dataclass
|
|
class Finding:
|
|
kind: str
|
|
severity: str
|
|
score: float
|
|
evidence: list[str] = field(default_factory=list)
|
|
|
|
|
|
@dataclass
|
|
class WindowReport:
|
|
label: str
|
|
segment: int
|
|
event_time_s: float
|
|
start_time_s: float
|
|
end_time_s: float
|
|
sample_count: int
|
|
summary: dict[str, Any]
|
|
findings: list[Finding]
|
|
|
|
|
|
@dataclass
|
|
class SegmentData:
|
|
segment: int
|
|
source: str
|
|
samples: list[LongitudinalSample]
|
|
bookmarks: list[float]
|
|
car_params: dict[str, Any] | None
|
|
settings: dict[str, Any]
|
|
software: dict[str, Any] | None
|
|
|
|
|
|
def safe_float(value: Any, default: float = 0.0) -> float:
|
|
try:
|
|
result = float(value)
|
|
except (TypeError, ValueError):
|
|
return default
|
|
return result if math.isfinite(result) else default
|
|
|
|
|
|
def safe_attr(obj: Any, name: str, default: Any = None) -> Any:
|
|
if obj is None:
|
|
return default
|
|
try:
|
|
return getattr(obj, name)
|
|
except (AttributeError, RuntimeError):
|
|
return default
|
|
|
|
|
|
def state_changes(values: list[Any]) -> int:
|
|
return sum(current != previous for previous, current in zip(values, values[1:], strict=False))
|
|
|
|
|
|
def threshold_sign_changes(values: list[float], deadband: float = ACTIVE_DEADBAND) -> int:
|
|
signs: list[int] = []
|
|
for value in values:
|
|
sign = 1 if value > deadband else -1 if value < -deadband else 0
|
|
if sign and (not signs or sign != signs[-1]):
|
|
signs.append(sign)
|
|
return max(0, len(signs) - 1)
|
|
|
|
|
|
def direction_reversals(values: list[float], minimum_step: float = 0.06) -> int:
|
|
directions: list[int] = []
|
|
for previous, current in zip(values, values[1:], strict=False):
|
|
delta = current - previous
|
|
direction = 1 if delta > minimum_step else -1 if delta < -minimum_step else 0
|
|
if direction and (not directions or direction != directions[-1]):
|
|
directions.append(direction)
|
|
return max(0, len(directions) - 1)
|
|
|
|
|
|
def maximum_rate(values: list[float], times: list[float]) -> float:
|
|
rates = [
|
|
abs((current - previous) / max(time - previous_time, 1e-3))
|
|
for previous, current, previous_time, time in zip(values, values[1:], times, times[1:], strict=False)
|
|
if 0.0 < time - previous_time < 0.5
|
|
]
|
|
return max(rates, default=0.0)
|
|
|
|
|
|
def percentile(values: list[float], quantile: float, default: float = 0.0) -> float:
|
|
return float(np.percentile(values, quantile)) if values else default
|
|
|
|
|
|
def severity_for(kind: str, score: float) -> str:
|
|
if kind == "unsafe_stop_release" and score >= 4.0:
|
|
return "critical"
|
|
if kind == "late_lead_response" and score >= 5.0:
|
|
return "high"
|
|
if score >= 7.0:
|
|
return "high"
|
|
if score >= 3.0:
|
|
return "medium"
|
|
return "low"
|
|
|
|
|
|
def analyze_samples(samples: list[LongitudinalSample], label: str, event_time_s: float) -> WindowReport:
|
|
if not samples:
|
|
raise ValueError("Cannot analyze an empty longitudinal window")
|
|
|
|
active_samples = [sample for sample in samples if sample.long_active]
|
|
relevant = active_samples if active_samples else samples
|
|
times = [sample.time_s for sample in relevant]
|
|
plans = [sample.plan_accel for sample in relevant]
|
|
commands = [sample.command_accel for sample in relevant]
|
|
actual = [sample.a_ego for sample in relevant]
|
|
integrals = [sample.i_term for sample in relevant]
|
|
sources = [sample.source for sample in relevant]
|
|
lead_statuses = [sample.lead_status for sample in relevant]
|
|
experiment_states = [sample.experimental_mode for sample in relevant]
|
|
stop_states = [sample.should_stop for sample in relevant]
|
|
tracking_states = [sample.tracking_lead for sample in relevant]
|
|
|
|
plan_sign_flips = threshold_sign_changes(plans)
|
|
command_sign_flips = threshold_sign_changes(commands)
|
|
plan_reversals = direction_reversals(plans)
|
|
command_reversals = direction_reversals(commands)
|
|
source_switches = state_changes(sources)
|
|
lead_status_flips = state_changes(lead_statuses)
|
|
experimental_flips = state_changes(experiment_states)
|
|
stop_flips = state_changes(stop_states)
|
|
tracking_flips = state_changes(tracking_states)
|
|
|
|
lead_track_ids = [sample.lead_track_id for sample in relevant if sample.lead_status and sample.lead_track_id >= 0]
|
|
lead_track_switches = state_changes(lead_track_ids)
|
|
lead_velocity_jumps = sum(
|
|
abs(current.lead_velocity - previous.lead_velocity) > 1.25
|
|
for previous, current in zip(relevant, relevant[1:], strict=False)
|
|
if previous.lead_status and current.lead_status and 0.0 < current.time_s - previous.time_s < 0.5
|
|
)
|
|
lead_distance_jumps = sum(
|
|
abs((current.lead_distance or 0.0) - (previous.lead_distance or 0.0)) > 6.0
|
|
for previous, current in zip(relevant, relevant[1:], strict=False)
|
|
if previous.lead_status and current.lead_status and 0.0 < current.time_s - previous.time_s < 0.5
|
|
)
|
|
|
|
stale_positive_i = [sample for sample in relevant if sample.plan_accel < -0.08 and sample.i_term > 0.08]
|
|
stale_negative_i = [
|
|
sample for sample in relevant
|
|
if sample.plan_accel > -0.05 and sample.i_term < -0.08 and sample.command_accel < sample.plan_accel - 0.10
|
|
]
|
|
opposite_commands = [
|
|
sample for sample in relevant
|
|
if abs(sample.plan_accel) > 0.10 and abs(sample.command_accel) > 0.10 and sample.plan_accel * sample.command_accel < 0.0
|
|
]
|
|
interface_adjustments = [abs(sample.output_accel - sample.command_accel) for sample in relevant]
|
|
response_errors = [abs(sample.a_ego - sample.output_accel) for sample in relevant]
|
|
|
|
late_response_shortfalls: list[float] = []
|
|
minimum_ttc: float | None = None
|
|
for sample in relevant:
|
|
if not sample.lead_status or sample.lead_distance is None:
|
|
continue
|
|
closing_speed = max(-sample.lead_relative_velocity, sample.v_ego - sample.lead_velocity, 0.0)
|
|
if closing_speed < 1.5:
|
|
continue
|
|
ttc = sample.lead_distance / max(closing_speed, 0.1)
|
|
minimum_ttc = ttc if minimum_ttc is None else min(minimum_ttc, ttc)
|
|
if ttc >= 4.0:
|
|
continue
|
|
usable_gap = max(sample.lead_distance - 4.0, 1.0)
|
|
required_accel = -(closing_speed ** 2) / (2.0 * usable_gap)
|
|
late_response_shortfalls.append(max(0.0, sample.command_accel - required_accel))
|
|
|
|
unsafe_stop_release = [
|
|
sample for sample in relevant
|
|
if (sample.should_stop and sample.command_accel > 0.10) or (
|
|
sample.lead_status and sample.lead_distance is not None and sample.lead_distance < 12.0 and
|
|
sample.lead_velocity < 0.5 and sample.v_ego < 5.0 and sample.command_accel > 0.15
|
|
)
|
|
]
|
|
|
|
max_plan_jerk = maximum_rate(plans, times)
|
|
max_command_jerk = maximum_rate(commands, times)
|
|
controller_gap_p95 = percentile([abs(command - plan) for command, plan in zip(commands, plans, strict=True)], 95)
|
|
response_gap_p95 = percentile(response_errors, 95)
|
|
interface_gap_p95 = percentile(interface_adjustments, 95)
|
|
stale_i_ratio = (len(stale_positive_i) + len(stale_negative_i)) / max(len(relevant), 1)
|
|
opposite_ratio = len(opposite_commands) / max(len(relevant), 1)
|
|
late_shortfall = max(late_response_shortfalls, default=0.0)
|
|
|
|
category_scores = {
|
|
"unsafe_stop_release": len(unsafe_stop_release) * 1.5,
|
|
"late_lead_response": late_shortfall * 2.0 + (1.0 if minimum_ttc is not None and minimum_ttc < 3.0 else 0.0),
|
|
"mode_arbitration": experimental_flips * 3.0 + stop_flips * 2.0 + tracking_flips,
|
|
"lead_instability": (
|
|
source_switches * SOURCE_SWITCH_SCORE + lead_status_flips + lead_track_switches * 2.0 +
|
|
lead_velocity_jumps * 0.75 + lead_distance_jumps
|
|
),
|
|
"controller_integrator": stale_i_ratio * 12.0 + opposite_ratio * 10.0 + max(0.0, controller_gap_p95 - 0.20) * 3.0,
|
|
"planner_chatter": (
|
|
plan_sign_flips * 1.5 + plan_reversals * 0.65 + command_sign_flips * 0.6 +
|
|
max(0.0, max_plan_jerk - 1.5) * 0.35
|
|
),
|
|
"vehicle_response": max(0.0, response_gap_p95 - 0.35) * 2.5 + max(0.0, interface_gap_p95 - 0.10) * 4.0,
|
|
}
|
|
|
|
evidence = {
|
|
"unsafe_stop_release": [
|
|
f"{len(unsafe_stop_release)} samples commanded positive acceleration while a stop hold or close stopped lead was active",
|
|
],
|
|
"late_lead_response": [
|
|
f"minimum TTC={minimum_ttc:.2f}s" if minimum_ttc is not None else "no closing lead TTC available",
|
|
f"maximum kinematic decel shortfall={late_shortfall:.2f} m/s^2",
|
|
],
|
|
"mode_arbitration": [
|
|
f"experimental flips={experimental_flips}, shouldStop flips={stop_flips}, trackingLead flips={tracking_flips}",
|
|
],
|
|
"lead_instability": [
|
|
f"source switches={source_switches}, lead status flips={lead_status_flips}, radar track switches={lead_track_switches}",
|
|
f"lead velocity jumps={lead_velocity_jumps}, lead distance jumps={lead_distance_jumps}",
|
|
],
|
|
"controller_integrator": [
|
|
f"stale-I samples={len(stale_positive_i) + len(stale_negative_i)}/{len(relevant)}, opposite command samples={len(opposite_commands)}",
|
|
f"planner-to-command gap p95={controller_gap_p95:.2f} m/s^2, I range={min(integrals):+.2f}..{max(integrals):+.2f}",
|
|
],
|
|
"planner_chatter": [
|
|
f"plan sign flips={plan_sign_flips}, direction reversals={plan_reversals}, max plan jerk={max_plan_jerk:.2f} m/s^3",
|
|
f"command sign flips={command_sign_flips}, direction reversals={command_reversals}, max command jerk={max_command_jerk:.2f} m/s^3",
|
|
],
|
|
"vehicle_response": [
|
|
f"command-to-applied gap p95={interface_gap_p95:.2f} m/s^2",
|
|
f"applied-to-measured acceleration gap p95={response_gap_p95:.2f} m/s^2",
|
|
],
|
|
}
|
|
|
|
findings = [
|
|
Finding(kind=kind, severity=severity_for(kind, score), score=round(score, 3), evidence=evidence[kind])
|
|
for kind, score in category_scores.items()
|
|
if score >= 1.0
|
|
]
|
|
findings.sort(key=lambda finding: finding.score, reverse=True)
|
|
|
|
lead_samples = [sample for sample in relevant if sample.lead_status and sample.lead_distance is not None]
|
|
summary = {
|
|
"activeSamples": len(active_samples),
|
|
"speedRangeMps": [round(min(sample.v_ego for sample in relevant), 3), round(max(sample.v_ego for sample in relevant), 3)],
|
|
"planAccelRange": [round(min(plans), 3), round(max(plans), 3)],
|
|
"commandAccelRange": [round(min(commands), 3), round(max(commands), 3)],
|
|
"actualAccelRange": [round(min(actual), 3), round(max(actual), 3)],
|
|
"leadDistanceRange": None if not lead_samples else [
|
|
round(min(sample.lead_distance for sample in lead_samples if sample.lead_distance is not None), 3),
|
|
round(max(sample.lead_distance for sample in lead_samples if sample.lead_distance is not None), 3),
|
|
],
|
|
"sources": sorted(set(sources)),
|
|
"personalities": sorted({sample.personality for sample in relevant}),
|
|
"minimumTtc": None if minimum_ttc is None else round(minimum_ttc, 3),
|
|
"maxPlanJerk": round(max_plan_jerk, 3),
|
|
"maxCommandJerk": round(max_command_jerk, 3),
|
|
}
|
|
return WindowReport(
|
|
label=label,
|
|
segment=samples[0].segment,
|
|
event_time_s=round(event_time_s, 3),
|
|
start_time_s=round(samples[0].time_s, 3),
|
|
end_time_s=round(samples[-1].time_s, 3),
|
|
sample_count=len(relevant),
|
|
summary=summary,
|
|
findings=findings,
|
|
)
|
|
|
|
|
|
def parse_settings(serialized: str) -> dict[str, Any]:
|
|
if not serialized:
|
|
return {}
|
|
try:
|
|
settings = json.loads(serialized)
|
|
except (TypeError, json.JSONDecodeError):
|
|
return {}
|
|
if not isinstance(settings, dict):
|
|
return {}
|
|
|
|
exact_keys = {
|
|
"acceleration_profile",
|
|
"custom_accel_profile",
|
|
"deceleration_profile",
|
|
"longitudinalActuatorDelay",
|
|
"startAccel",
|
|
"stopAccel",
|
|
"stoppingDecelRate",
|
|
"taco_tune",
|
|
"truck_tuning",
|
|
}
|
|
return {
|
|
key: value for key, value in sorted(settings.items())
|
|
if key in exact_keys or key.endswith("_follow") or (
|
|
key.startswith(("aggressive_jerk_", "relaxed_jerk_", "standard_jerk_")) and not key.endswith("_via")
|
|
)
|
|
}
|
|
|
|
|
|
def snapshot_car_params(CP: Any) -> dict[str, Any]:
|
|
tuning = safe_attr(CP, "longitudinalTuning")
|
|
return {
|
|
"brand": str(safe_attr(CP, "brand", "unknown")),
|
|
"carFingerprint": str(safe_attr(CP, "carFingerprint", "unknown")),
|
|
"openpilotLongitudinalControl": bool(safe_attr(CP, "openpilotLongitudinalControl", False)),
|
|
"longitudinalActuatorDelay": safe_float(safe_attr(CP, "longitudinalActuatorDelay", 0.0)),
|
|
"kpBP": list(safe_attr(tuning, "kpBP", [])),
|
|
"kpV": list(safe_attr(tuning, "kpV", [])),
|
|
"kiBP": list(safe_attr(tuning, "kiBP", [])),
|
|
"kiV": list(safe_attr(tuning, "kiV", [])),
|
|
"kf": safe_float(safe_attr(tuning, "kfDEPRECATED", 0.0)),
|
|
}
|
|
|
|
|
|
def snapshot_software(init_data: Any) -> dict[str, Any]:
|
|
return {
|
|
"version": str(safe_attr(init_data, "version", "unknown")),
|
|
"gitCommit": str(safe_attr(init_data, "gitCommit", "unknown")),
|
|
"gitSrcCommit": str(safe_attr(init_data, "gitSrcCommit", "")),
|
|
"gitBranch": str(safe_attr(init_data, "gitBranch", "unknown")),
|
|
"gitRemote": str(safe_attr(init_data, "gitRemote", "unknown")),
|
|
"dirty": bool(safe_attr(init_data, "dirty", False)),
|
|
}
|
|
|
|
|
|
def make_sample(segment: int, segment_start_ns: int, mono_time: int, latest: dict[str, Any]) -> LongitudinalSample | None:
|
|
required = ("carState", "carControl", "controlsState", "radarState", "starpilotPlan", "longitudinalPlan")
|
|
if not all(service in latest for service in required):
|
|
return None
|
|
|
|
car_state = latest["carState"]
|
|
car_control = latest["carControl"]
|
|
controls_state = latest["controlsState"]
|
|
radar_state = latest["radarState"]
|
|
starpilot_plan = latest["starpilotPlan"]
|
|
long_plan = latest["longitudinalPlan"]
|
|
car_output = latest.get("carOutput")
|
|
selfdrive_state = latest.get("selfdriveState")
|
|
lead = radar_state.leadOne
|
|
lead_two = radar_state.leadTwo
|
|
command_accel = safe_float(safe_attr(safe_attr(car_control, "actuators"), "accel", 0.0))
|
|
output_accel = safe_float(safe_attr(safe_attr(car_output, "actuatorsOutput"), "accel", command_accel), command_accel)
|
|
lead_status = bool(safe_attr(lead, "status", False))
|
|
lead_two_status = bool(safe_attr(lead_two, "status", False))
|
|
|
|
return LongitudinalSample(
|
|
segment=segment,
|
|
time_s=(mono_time - segment_start_ns) / 1e9,
|
|
mono_time=mono_time,
|
|
long_active=bool(safe_attr(car_control, "longActive", False)),
|
|
v_ego=safe_float(safe_attr(car_state, "vEgo", 0.0)),
|
|
a_ego=safe_float(safe_attr(car_state, "aEgo", 0.0)),
|
|
gas_pressed=bool(safe_attr(car_state, "gasPressed", False)),
|
|
brake_pressed=bool(safe_attr(car_state, "brakePressed", False)),
|
|
plan_accel=safe_float(safe_attr(long_plan, "aTarget", 0.0)),
|
|
command_accel=command_accel,
|
|
output_accel=output_accel,
|
|
p_term=safe_float(safe_attr(controls_state, "upAccelCmd", 0.0)),
|
|
i_term=safe_float(safe_attr(controls_state, "uiAccelCmd", 0.0)),
|
|
f_term=safe_float(safe_attr(controls_state, "ufAccelCmd", 0.0)),
|
|
long_state=str(safe_attr(controls_state, "longControlState", "unknown")),
|
|
source=str(safe_attr(long_plan, "longitudinalPlanSource", "unknown")),
|
|
should_stop=bool(safe_attr(long_plan, "shouldStop", False)),
|
|
allow_throttle=bool(safe_attr(long_plan, "allowThrottle", True)),
|
|
allow_brake=bool(safe_attr(long_plan, "allowBrake", True)),
|
|
has_lead=bool(safe_attr(long_plan, "hasLead", False)),
|
|
lead_status=lead_status,
|
|
lead_distance=safe_float(safe_attr(lead, "dRel", 0.0)) if lead_status else None,
|
|
lead_relative_velocity=safe_float(safe_attr(lead, "vRel", 0.0)),
|
|
lead_velocity=safe_float(safe_attr(lead, "vLead", 0.0)),
|
|
lead_acceleration=safe_float(safe_attr(lead, "aLeadK", 0.0)),
|
|
lead_probability=safe_float(safe_attr(lead, "modelProb", 0.0)),
|
|
lead_radar=bool(safe_attr(lead, "radar", False)),
|
|
lead_track_id=int(safe_attr(lead, "radarTrackId", -1)),
|
|
lead_two_status=lead_two_status,
|
|
lead_two_distance=safe_float(safe_attr(lead_two, "dRel", 0.0)) if lead_two_status else None,
|
|
experimental_mode=bool(safe_attr(starpilot_plan, "experimentalMode", False)),
|
|
forcing_stop=bool(safe_attr(starpilot_plan, "forcingStop", False)),
|
|
red_light=bool(safe_attr(starpilot_plan, "redLight", False)),
|
|
tracking_lead=bool(safe_attr(starpilot_plan, "trackingLead", False)),
|
|
curve_controller_active=bool(safe_attr(starpilot_plan, "cscControllingSpeed", False)),
|
|
cruise_target=safe_float(safe_attr(starpilot_plan, "vCruise", 0.0)),
|
|
desired_follow_distance=safe_float(safe_attr(starpilot_plan, "desiredFollowDistance", 0.0)),
|
|
personality=str(safe_attr(selfdrive_state, "personality", "unknown")),
|
|
)
|
|
|
|
|
|
def analyze_segment(identifier: str, segment: int, mode: ReadMode) -> SegmentData:
|
|
reader = LogReader(identifier, default_mode=mode, sort_by_time=True)
|
|
source = ",".join(Path(urlparse(path).path).name or path.rsplit("/", 1)[-1] for path in reader.logreader_identifiers)
|
|
latest: dict[str, Any] = {}
|
|
samples: list[LongitudinalSample] = []
|
|
bookmark_mono_times: list[int] = []
|
|
car_params = None
|
|
settings: dict[str, Any] = {}
|
|
software = None
|
|
segment_start_ns: int | None = None
|
|
|
|
for msg in reader:
|
|
mono_time = int(msg.logMonoTime)
|
|
which = msg.which()
|
|
if segment_start_ns is None and which in ("carState", "longitudinalPlan"):
|
|
segment_start_ns = mono_time
|
|
|
|
if which in ("userBookmark", "bookmarkButton"):
|
|
if not bookmark_mono_times or (mono_time - bookmark_mono_times[-1]) / 1e9 > 0.5:
|
|
bookmark_mono_times.append(mono_time)
|
|
continue
|
|
if which == "carParams" and car_params is None:
|
|
car_params = snapshot_car_params(msg.carParams)
|
|
if which == "initData" and software is None:
|
|
software = snapshot_software(msg.initData)
|
|
if which in (
|
|
"carState", "carControl", "carOutput", "controlsState", "radarState", "selfdriveState", "starpilotPlan", "longitudinalPlan",
|
|
):
|
|
latest[which] = getattr(msg, which)
|
|
if which == "starpilotPlan":
|
|
settings = parse_settings(str(safe_attr(msg.starpilotPlan, "starpilotToggles", ""))) or settings
|
|
if which == "longitudinalPlan" and segment_start_ns is not None:
|
|
sample = make_sample(segment, segment_start_ns, mono_time, latest)
|
|
if sample is not None:
|
|
samples.append(sample)
|
|
|
|
bookmarks = [] if segment_start_ns is None else [
|
|
(mono_time - segment_start_ns) / 1e9 for mono_time in bookmark_mono_times
|
|
]
|
|
|
|
return SegmentData(
|
|
segment=segment,
|
|
source=source,
|
|
samples=samples,
|
|
bookmarks=bookmarks,
|
|
car_params=car_params,
|
|
settings=settings,
|
|
software=software,
|
|
)
|
|
|
|
|
|
def point_anomaly_scores(samples: list[LongitudinalSample]) -> list[tuple[float, float]]:
|
|
scores: list[tuple[float, float]] = []
|
|
for previous, current in zip(samples, samples[1:], strict=False):
|
|
if not current.long_active:
|
|
continue
|
|
score = 0.0
|
|
score += max(0.0, abs(current.plan_accel - previous.plan_accel) - 0.12) * 4.0
|
|
score += max(0.0, abs(current.command_accel - previous.command_accel) - 0.15) * 3.0
|
|
score += SOURCE_SWITCH_SCORE if current.source != previous.source else 0.0
|
|
score += 1.0 if current.lead_status != previous.lead_status else 0.0
|
|
score += 2.0 if current.experimental_mode != previous.experimental_mode else 0.0
|
|
score += 1.5 if current.should_stop != previous.should_stop else 0.0
|
|
if current.plan_accel < -0.08 and current.i_term > 0.08:
|
|
score += 1.0
|
|
if current.plan_accel > -0.05 and current.i_term < -0.08 and current.command_accel < current.plan_accel - 0.10:
|
|
score += 1.0
|
|
if score >= 1.0:
|
|
scores.append((current.time_s, score))
|
|
return scores
|
|
|
|
|
|
def anomaly_episode_times(samples: list[LongitudinalSample], limit: int) -> list[float]:
|
|
points = point_anomaly_scores(samples)
|
|
if not points:
|
|
return []
|
|
|
|
episodes: list[list[tuple[float, float]]] = [[points[0]]]
|
|
for point in points[1:]:
|
|
if point[0] - episodes[-1][-1][0] <= 1.5:
|
|
episodes[-1].append(point)
|
|
else:
|
|
episodes.append([point])
|
|
|
|
ranked = sorted(
|
|
((max(episode, key=lambda item: item[1])[0], sum(item[1] for item in episode)) for episode in episodes),
|
|
key=lambda item: item[1],
|
|
reverse=True,
|
|
)
|
|
return [time_s for time_s, _ in ranked[:limit]]
|
|
|
|
|
|
def window_samples(samples: list[LongitudinalSample], event_time_s: float, before: float, after: float) -> list[LongitudinalSample]:
|
|
return [sample for sample in samples if event_time_s - before <= sample.time_s <= event_time_s + after]
|
|
|
|
|
|
def resolve_segments(identifier: str, mode: ReadMode) -> tuple[str, list[tuple[int, str]]]:
|
|
if parse_direct(identifier) is not None:
|
|
return identifier, [(0, identifier)]
|
|
|
|
normalized = parse_indirect(identifier)
|
|
segment_range = SegmentRange(normalized)
|
|
route = segment_range.route_name
|
|
requests = [(segment, f"{route}/{segment}") for segment in segment_range.seg_idxs]
|
|
return route, requests
|
|
|
|
|
|
def analyze_route(identifier: str, mode: ReadMode, before: float, after: float, top: int) -> dict[str, Any]:
|
|
route, segment_requests = resolve_segments(identifier, mode)
|
|
segments = [analyze_segment(request, segment, mode) for segment, request in segment_requests]
|
|
bookmark_reports: list[WindowReport] = []
|
|
anomaly_reports: list[WindowReport] = []
|
|
|
|
for segment_data in segments:
|
|
for bookmark_number, bookmark_time in enumerate(segment_data.bookmarks, start=1):
|
|
samples = window_samples(segment_data.samples, bookmark_time, before, after)
|
|
if samples:
|
|
bookmark_reports.append(analyze_samples(samples, f"bookmark {bookmark_number}", bookmark_time))
|
|
|
|
episode_times = anomaly_episode_times(segment_data.samples, top)
|
|
for episode_number, event_time in enumerate(episode_times, start=1):
|
|
if any(bookmark - before <= event_time <= bookmark + after for bookmark in segment_data.bookmarks):
|
|
continue
|
|
samples = window_samples(segment_data.samples, event_time, min(before, 5.0), min(after, 2.0))
|
|
if samples:
|
|
anomaly_reports.append(analyze_samples(samples, f"route anomaly {episode_number}", event_time))
|
|
|
|
anomaly_reports.sort(key=lambda report: report.findings[0].score if report.findings else 0.0, reverse=True)
|
|
reports = bookmark_reports + anomaly_reports[:top]
|
|
reports.sort(key=lambda report: (report.segment, report.event_time_s, report.label))
|
|
car_params = next((segment.car_params for segment in segments if segment.car_params), None)
|
|
settings = next((segment.settings for segment in reversed(segments) if segment.settings), {})
|
|
software = next((segment.software for segment in segments if segment.software), None)
|
|
return {
|
|
"route": route,
|
|
"carParams": car_params,
|
|
"settings": settings,
|
|
"software": software,
|
|
"segments": [
|
|
{
|
|
"segment": segment.segment,
|
|
"source": segment.source,
|
|
"samples": len(segment.samples),
|
|
"bookmarks": segment.bookmarks,
|
|
}
|
|
for segment in segments
|
|
],
|
|
"reports": [asdict(report) for report in reports],
|
|
}
|
|
|
|
|
|
def print_report(payload: dict[str, Any]) -> None:
|
|
print(f"route={payload['route']}")
|
|
software = payload.get("software")
|
|
if software:
|
|
source_commit = software["gitSrcCommit"] or software["gitCommit"]
|
|
print(
|
|
f"software={source_commit[:10]} branch={software['gitBranch']} version={software['version']} "
|
|
+ f"dirty={software['dirty']}"
|
|
)
|
|
car_params = payload.get("carParams")
|
|
if car_params:
|
|
vehicle_line = f"vehicle={car_params['carFingerprint']} brand={car_params['brand']}"
|
|
delay_line = f"longDelay={car_params['longitudinalActuatorDelay']:.3f}s openpilotLong={car_params['openpilotLongitudinalControl']}"
|
|
print(f"{vehicle_line} {delay_line}")
|
|
print(f"longTune kp={car_params['kpV']} ki={car_params['kiV']} kf={car_params['kf']}")
|
|
else:
|
|
print("vehicle=unknown (carParams unavailable in selected segments)")
|
|
|
|
for segment in payload["segments"]:
|
|
segment_line = f"segment={segment['segment']} source={segment['source']} samples={segment['samples']}"
|
|
print(f"{segment_line} bookmarks={len(segment['bookmarks'])}")
|
|
|
|
settings = payload.get("settings", {})
|
|
if settings:
|
|
print("settings=" + json.dumps(settings, sort_keys=True))
|
|
|
|
reports = payload["reports"]
|
|
if not reports:
|
|
print("No bookmark windows or route-wide anomalies were found.")
|
|
return
|
|
|
|
for report in reports:
|
|
primary = report["findings"][0] if report["findings"] else None
|
|
primary_text = "no deterministic fault" if primary is None else f"{primary['kind']} ({primary['severity']}, score={primary['score']:.2f})"
|
|
event_line = f"\nseg {report['segment']} {report['label']} @{report['event_time_s']:.2f}s: {primary_text}"
|
|
window_line = f" window={report['start_time_s']:.2f}..{report['end_time_s']:.2f}s samples={report['sample_count']}"
|
|
accel_line = (
|
|
f"speed={report['summary']['speedRangeMps']}m/s plan={report['summary']['planAccelRange']} "
|
|
+ f"cmd={report['summary']['commandAccelRange']} actual={report['summary']['actualAccelRange']}"
|
|
)
|
|
lead_line = (
|
|
f" leadRange={report['summary']['leadDistanceRange']}m sources={report['summary']['sources']} "
|
|
+ f"personalities={report['summary']['personalities']}"
|
|
)
|
|
print(event_line)
|
|
print(f"{window_line} {accel_line}")
|
|
minimum_ttc = report["summary"]["minimumTtc"]
|
|
print(f"{lead_line} minTTC={'n/a' if minimum_ttc is None else f'{minimum_ttc}s'}")
|
|
for finding in report["findings"][:3]:
|
|
print(f" {finding['kind']}: {finding['severity']} score={finding['score']:.2f}")
|
|
for item in finding["evidence"]:
|
|
print(f" {item}")
|
|
|
|
|
|
def parse_args() -> argparse.Namespace:
|
|
parser = argparse.ArgumentParser(description="Diagnose longitudinal behavior around route bookmarks and automatically detected anomalies.")
|
|
parser.add_argument("routes", nargs="+", help="Comma/Konik route, segment range, Connect URL, or local log file")
|
|
parser.add_argument("--mode", choices=("auto", "qlog", "rlog"), default="auto")
|
|
parser.add_argument("--before", type=float, default=DEFAULT_BOOKMARK_BEFORE, help="Seconds before each bookmark to inspect")
|
|
parser.add_argument("--after", type=float, default=DEFAULT_BOOKMARK_AFTER, help="Seconds after each bookmark to inspect")
|
|
parser.add_argument("--top", type=int, default=5, help="Maximum route-wide anomaly episodes across the route")
|
|
parser.add_argument("--json-out", type=Path, help="Optional JSON report path")
|
|
return parser.parse_args()
|
|
|
|
|
|
def main() -> None:
|
|
args = parse_args()
|
|
mode = {"auto": ReadMode.AUTO, "qlog": ReadMode.QLOG, "rlog": ReadMode.RLOG}[args.mode]
|
|
payloads = [analyze_route(route, mode, args.before, args.after, args.top) for route in args.routes]
|
|
for index, payload in enumerate(payloads):
|
|
if index:
|
|
print("\n" + "=" * 80 + "\n")
|
|
print_report(payload)
|
|
|
|
if args.json_out:
|
|
args.json_out.parent.mkdir(parents=True, exist_ok=True)
|
|
args.json_out.write_text(json.dumps(payloads, indent=2, sort_keys=True) + "\n", encoding="utf-8")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|