diff --git a/scripts/speed_limit_vision/analyze_route_speed_limit_plan.py b/scripts/speed_limit_vision/analyze_route_speed_limit_plan.py new file mode 100644 index 000000000..4dac32798 --- /dev/null +++ b/scripts/speed_limit_vision/analyze_route_speed_limit_plan.py @@ -0,0 +1,237 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import bz2 +import csv + +from collections import Counter +from dataclasses import dataclass +from pathlib import Path + +import zstandard as zstd +from cereal import log + + +MS_TO_MPH = 2.2369362920544 +MS_TO_KPH = 3.6 + + +@dataclass(frozen=True) +class PlanState: + time_s: float + segment: int + source: str + speed: int + pending: int + map_speed: int + mapbox_speed: int + next_speed: int + + +@dataclass(frozen=True) +class VisionSpan: + start_s: float + end_s: float + speed: int + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Summarize actual speed-limit planner source changes from downloaded route qlogs.") + parser.add_argument("routes", nargs="+", help="Route log ids like 00000004--0da2db69c7 or dongle/logid.") + parser.add_argument("--clip-root", type=Path, default=Path("/Volumes/T5/starpilot_speed_limit/realdata"), help="Downloaded segment root.") + parser.add_argument("--metric", action="store_true", help="Print km/h instead of mph.") + parser.add_argument("--output-csv", type=Path, help="Optional CSV of all planner state changes.") + parser.add_argument("--observed-hold-seconds", type=float, default=12.0, help="Hold duration used by the logged build.") + parser.add_argument("--project-hold-seconds", type=float, default=0.0, help="Project Vision spans using this stale-hold duration.") + return parser.parse_args() + + +def route_log_id(route: str) -> str: + text = route.strip().strip("'\"") + if "/" in text: + text = text.rsplit("/", 1)[1] + return text + + +def segment_index(path: Path) -> int: + try: + return int(path.parent.name.rsplit("--", 1)[1]) + except (IndexError, ValueError): + return -1 + + +def qlog_paths(clip_root: Path, log_id: str) -> list[Path]: + paths: list[Path] = [] + for name in ("qlog.zst", "qlog.bz2", "qlog"): + paths.extend(clip_root.glob(f"{log_id}--*/{name}")) + return sorted((path for path in paths if not path.name.startswith("._")), key=segment_index) + + +def read_qlog(path: Path): + if path.suffix == ".zst": + with path.open("rb") as qlog_file, zstd.ZstdDecompressor().stream_reader(qlog_file) as reader: + return log.Event.read_multiple_bytes(reader.read()) + if path.suffix == ".bz2": + return log.Event.read_multiple_bytes(bz2.decompress(path.read_bytes())) + return log.Event.read_multiple_bytes(path.read_bytes()) + + +def convert_speed(speed_ms: float, metric: bool) -> int: + return round(speed_ms * (MS_TO_KPH if metric else MS_TO_MPH)) + + +def extract_changes(qlogs: list[Path], metric: bool) -> tuple[list[PlanState], Counter[str], Counter[tuple[str, int]]]: + first_time_ns: int | None = None + last_state: tuple[str, int, int, int, int, int] | None = None + changes: list[PlanState] = [] + source_counts: Counter[str] = Counter() + speed_counts: Counter[tuple[str, int]] = Counter() + + for qlog_path in qlogs: + segment = segment_index(qlog_path) + for event in read_qlog(qlog_path): + if event.which() != "starpilotPlan": + continue + if first_time_ns is None: + first_time_ns = event.logMonoTime + + plan = event.starpilotPlan + source = str(plan.slcSpeedLimitSource) + speed = convert_speed(float(plan.slcSpeedLimit), metric) + pending = convert_speed(float(plan.unconfirmedSlcSpeedLimit), metric) + map_speed = convert_speed(float(plan.slcMapSpeedLimit), metric) + mapbox_speed = convert_speed(float(plan.slcMapboxSpeedLimit), metric) + next_speed = convert_speed(float(plan.slcNextSpeedLimit), metric) + state = (source, speed, pending, map_speed, mapbox_speed, next_speed) + source_counts[source] += 1 + speed_counts[(source, speed)] += 1 + if state == last_state: + continue + + changes.append(PlanState( + time_s=(event.logMonoTime - first_time_ns) / 1e9, + segment=segment, + source=source, + speed=speed, + pending=pending, + map_speed=map_speed, + mapbox_speed=mapbox_speed, + next_speed=next_speed, + )) + last_state = state + + return changes, source_counts, speed_counts + + +def vision_spans(changes: list[PlanState]) -> list[VisionSpan]: + spans: list[VisionSpan] = [] + active: PlanState | None = None + for state in changes: + if active is not None and active.source == "Vision": + spans.append(VisionSpan(active.time_s, state.time_s, active.speed)) + active = state if state.source == "Vision" else None + return spans + + +def project_spans(spans: list[VisionSpan], observed_hold_s: float, project_hold_s: float) -> list[VisionSpan]: + if project_hold_s <= 0: + return [] + + projected: list[VisionSpan] = [] + for index, span in enumerate(spans): + support_end = max(span.start_s, span.end_s - observed_hold_s) + projected_end = support_end + project_hold_s + if index + 1 < len(spans): + projected_end = min(projected_end, spans[index + 1].start_s) + if projected and projected[-1].speed == span.speed and projected[-1].end_s >= span.start_s: + previous = projected[-1] + projected[-1] = VisionSpan(previous.start_s, max(previous.end_s, projected_end), previous.speed) + else: + projected.append(VisionSpan(span.start_s, projected_end, span.speed)) + return projected + + +def print_route_summary( + route: str, + qlogs: list[Path], + changes: list[PlanState], + source_counts: Counter[str], + speed_counts: Counter[tuple[str, int]], + spans: list[VisionSpan], + projected: list[VisionSpan], + unit: str, +) -> None: + print(f"\nRoute {route}") + print(f" qlogs: {len(qlogs)}") + print(f" source counts: {dict(source_counts)}") + print(" top source/speed states:") + for (source, speed), count in speed_counts.most_common(8): + print(f" {source or 'None':8s} {speed:3d} {unit}: {count}") + print(" planner changes:") + for state in changes: + print( + f" t={state.time_s:7.1f}s seg={state.segment:02d} " + f"source={state.source:8s} speed={state.speed:3d} pending={state.pending:3d} " + f"map={state.map_speed:3d} mapbox={state.mapbox_speed:3d} next={state.next_speed:3d}" + ) + print(" actual Vision spans:") + if not spans: + print(" none") + for span in spans: + print(f" {span.speed:3d} {unit} from {span.start_s:7.1f}s to {span.end_s:7.1f}s ({span.end_s - span.start_s:5.1f}s)") + if projected: + print(" projected Vision spans:") + for span in projected: + print(f" {span.speed:3d} {unit} from {span.start_s:7.1f}s to {span.end_s:7.1f}s ({span.end_s - span.start_s:5.1f}s)") + + +def write_csv(output_csv: Path, rows: list[tuple[str, PlanState]]) -> None: + output_csv.parent.mkdir(parents=True, exist_ok=True) + with output_csv.open("w", encoding="utf-8", newline="") as csv_file: + writer = csv.DictWriter(csv_file, fieldnames=[ + "route", "time_s", "segment", "source", "speed", "pending", "map_speed", "mapbox_speed", "next_speed", + ]) + writer.writeheader() + for route, state in rows: + writer.writerow({ + "route": route, + "time_s": f"{state.time_s:.3f}", + "segment": state.segment, + "source": state.source, + "speed": state.speed, + "pending": state.pending, + "map_speed": state.map_speed, + "mapbox_speed": state.mapbox_speed, + "next_speed": state.next_speed, + }) + + +def main() -> int: + args = parse_args() + clip_root = args.clip_root.expanduser().resolve() + unit = "km/h" if args.metric else "mph" + csv_rows: list[tuple[str, PlanState]] = [] + + for route_input in args.routes: + log_id = route_log_id(route_input) + qlogs = qlog_paths(clip_root, log_id) + if not qlogs: + print(f"\nRoute {log_id}") + print(f" no qlogs found under {clip_root}") + continue + + changes, source_counts, speed_counts = extract_changes(qlogs, args.metric) + spans = vision_spans(changes) + projected = project_spans(spans, args.observed_hold_seconds, args.project_hold_seconds) + print_route_summary(log_id, qlogs, changes, source_counts, speed_counts, spans, projected, unit) + csv_rows.extend((log_id, state) for state in changes) + + if args.output_csv and csv_rows: + write_csv(args.output_csv.expanduser().resolve(), csv_rows) + print(f"\nWrote {args.output_csv.expanduser().resolve()}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index eb1999805..c802b52f9 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -32,7 +32,7 @@ HISTORY_SECONDS = 2.0 CONSISTENT_DETECTIONS = 2 CHANGE_CONSISTENT_DETECTIONS = 3 MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE = 0.65 -PUBLISHED_HOLD_SECONDS = 12.0 +PUBLISHED_HOLD_SECONDS = 300.0 PUBLISHED_CHANGE_COOLDOWN_SECONDS = 1.4 PUBLISHED_REVERT_CONFIDENCE = 0.97 AUTO_BOOKMARK_CONFIRM_DELAY_SECONDS = 0.9