diff --git a/scripts/replay_speed_limit_vision.py b/scripts/replay_speed_limit_vision.py index f9fc29bdb..850137f9c 100644 --- a/scripts/replay_speed_limit_vision.py +++ b/scripts/replay_speed_limit_vision.py @@ -10,9 +10,10 @@ import starpilot.system.speed_limit_vision as slv class ReplayDaemon(slv.SpeedLimitVisionDaemon): - def __init__(self): + def __init__(self, runtime_cadence: bool): super().__init__(use_runtime=False) self.now = 0.0 + self.runtime_cadence = runtime_cadence def _write_debug_event(self, event_type, frame_bgr=None, snapshot_prefix=None, **fields): if event_type in ("candidate", "publish", "stale_clear"): @@ -30,6 +31,15 @@ class ReplayDaemon(slv.SpeedLimitVisionDaemon): slv.time.monotonic = lambda now=now: now self.current_frame_bgr = frame_bgr + if self.runtime_cadence: + inference_interval = self._inference_interval(now) + if now - self.last_inference_at < inference_interval: + if self.published_speed_limit_mph > 0 and self._published_detection_stale(now): + print(f"t={self.now:6.2f}s stale_clear {{'reason': 'inference_interval'}}") + self._clear_detection() + return + + self.last_inference_at = now detection = self._detect_sign(frame_bgr) if detection is not None: self._update_detection(detection) @@ -65,13 +75,28 @@ def main(): parser.add_argument("--frames-fps", type=float, default=5.0, help="FPS to assume when replaying an extracted frame directory.") parser.add_argument("--start", type=float, default=0.0, help="Skip frames before this timestamp in seconds.") parser.add_argument("--end", type=float, default=None, help="Stop once this timestamp in seconds is exceeded.") + parser.add_argument("--all-frames", action="store_true", help="Run inference on every decoded frame instead of the runtime cadence.") + parser.add_argument("--models-dir", type=Path, help="Directory containing speed_limit_us_detector.onnx and speed_limit_us_value_classifier.onnx.") args = parser.parse_args() path = Path(args.path) if not path.exists(): raise FileNotFoundError(path) - daemon = ReplayDaemon() + if args.models_dir: + models_dir = args.models_dir.expanduser().resolve() + detector_path = models_dir / "speed_limit_us_detector.onnx" + classifier_path = models_dir / "speed_limit_us_value_classifier.onnx" + reject_classifier_path = models_dir / "speed_limit_us_reject_classifier.onnx" + if not detector_path.is_file(): + raise FileNotFoundError(detector_path) + if not classifier_path.is_file(): + raise FileNotFoundError(classifier_path) + slv.US_DETECTOR_MODEL_PATH = detector_path + slv.US_CLASSIFIER_MODEL_PATH = classifier_path + slv.US_REJECT_CLASSIFIER_MODEL_PATH = reject_classifier_path + + daemon = ReplayDaemon(runtime_cadence=not args.all_frames) frame_iter = iter_directory_frames(path, max(args.frames_fps, 0.1)) if path.is_dir() else iter_video_frames(path) for now, frame_bgr in frame_iter: if now < args.start: diff --git a/scripts/speed_limit_vision/diagnose_runtime_manifest.py b/scripts/speed_limit_vision/diagnose_runtime_manifest.py new file mode 100644 index 000000000..4e3b4f0de --- /dev/null +++ b/scripts/speed_limit_vision/diagnose_runtime_manifest.py @@ -0,0 +1,257 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import csv + +from collections import Counter +from pathlib import Path + +import cv2 + +import starpilot.system.speed_limit_vision as slv + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Diagnose speed-limit runtime failures at detector proposal/read level.") + parser.add_argument("--models-dir", type=Path, default=Path("starpilot/assets/vision_models")) + parser.add_argument("--manifest", type=Path, required=True) + parser.add_argument("--output-rows", type=Path, required=True) + parser.add_argument("--output-proposals", type=Path, required=True) + parser.add_argument("--only-errors", action="store_true", help="Only write proposal rows for non-exact positives and false positives.") + parser.add_argument("--include-uncertain", action="store_true") + parser.add_argument("--max-proposals", type=int, default=0, help="Optional proposal row cap per image.") + return parser.parse_args() + + +def first_present(row: dict[str, str], keys: tuple[str, ...]) -> str: + for key in keys: + value = row.get(key, "").strip() + if value: + return value + return "" + + +def int_value(text: str) -> int | None: + text = text.strip() + if not text: + return None + try: + return int(float(text)) + except ValueError: + return None + + +def expected_value(row: dict[str, str]) -> int | None: + value = int_value(first_present(row, ("speed_limit_mph", "review_speed_limit_mph", "expected_speed_limit_mph", "dominant_value"))) + if value is not None: + return value + + for key in ("full_detection", "model_read", "ocr_read"): + read_text = row.get(key, "").strip() + if "@" not in read_text: + continue + value = int_value(read_text.split("@", 1)[0]) + if value is not None: + return value + return None + + +def is_negative(row: dict[str, str]) -> bool: + sample_type = row.get("sample_type", "").lower() + if "negative" in sample_type: + return True + return expected_value(row) is None + + +def load_rows(manifest_path: Path, include_uncertain: bool) -> list[dict[str, str]]: + with manifest_path.open("r", encoding="utf-8", newline="") as manifest_file: + rows = list(csv.DictReader(manifest_file)) + if include_uncertain: + return rows + return [ + row for row in rows + if row.get("sample_type", "") != "uncertain_positive" and row.get("review_status", "") != "uncertain" + ] + + +def configure_models(models_dir: Path) -> None: + models_dir = models_dir.expanduser().resolve() + slv.US_DETECTOR_MODEL_PATH = models_dir / "speed_limit_us_detector.onnx" + slv.US_CLASSIFIER_MODEL_PATH = models_dir / "speed_limit_us_value_classifier.onnx" + slv.US_REJECT_CLASSIFIER_MODEL_PATH = models_dir / "speed_limit_us_reject_classifier.onnx" + + +def classify_row(expected: int | None, negative: bool, predicted: int | None, proposal_count: int, expected_read_count: int) -> str: + if negative: + return "false_positive" if predicted is not None else "true_negative" + if predicted == expected: + return "exact" + if predicted is not None: + return "wrong_value_expected_read_seen" if expected_read_count else "wrong_value_no_expected_read" + if proposal_count <= 0: + return "miss_no_detector_proposal" + if expected_read_count > 0: + return "miss_expected_read_seen" + return "miss_no_expected_read" + + +def crop_reads(daemon: slv.SpeedLimitVisionDaemon, frame_bgr, bbox: tuple[int, int, int, int]): + frame_height, frame_width = frame_bgr.shape[:2] + x1, y1, x2, y2 = bbox + box_width = x2 - x1 + box_height = y2 - y1 + for expansion_index, (expand_left, expand_top, expand_right, expand_bottom, expansion_weight) in enumerate(slv.DETECTOR_CLASSIFIER_EXPANSIONS): + expanded_x1 = max(int(x1 - box_width * expand_left), 0) + expanded_y1 = max(int(y1 - box_height * expand_top), 0) + expanded_x2 = min(int(x2 + box_width * expand_right), frame_width) + expanded_y2 = min(int(y2 + box_height * expand_bottom), frame_height) + sign_crop = frame_bgr[expanded_y1:expanded_y2, expanded_x1:expanded_x2] + if sign_crop.size == 0: + continue + + model_read = daemon._classify_speed_limit_from_model(sign_crop) + ocr_read = daemon._read_speed_limit_from_crop(sign_crop) + yield { + "expansion_index": expansion_index, + "expansion_weight": expansion_weight, + "expanded_bbox": (expanded_x1, expanded_y1, expanded_x2, expanded_y2), + "is_regulatory": daemon._is_regulatory_speed_sign(sign_crop), + "model_speed": model_read[0] if model_read is not None else None, + "model_confidence": model_read[1] if model_read is not None else None, + "ocr_speed": ocr_read[0] if ocr_read is not None else None, + "ocr_confidence": ocr_read[1] if ocr_read is not None else None, + } + + +def main() -> int: + args = parse_args() + configure_models(args.models_dir) + daemon = slv.SpeedLimitVisionDaemon(use_runtime=False) + rows = load_rows(args.manifest.expanduser().resolve(), args.include_uncertain) + + output_rows: list[dict[str, str]] = [] + proposal_rows: list[dict[str, str]] = [] + failure_counts: Counter[str] = Counter() + + for row in rows: + image_text = first_present(row, ("dataset_image", "frame_path", "source_frame")) + image_path = Path(image_text).expanduser().resolve() if image_text else None + expected = expected_value(row) + negative = is_negative(row) + frame_bgr = cv2.imread(str(image_path)) if image_path is not None else None + if frame_bgr is None: + failure_type = "unreadable" + predicted = None + confidence = None + proposals = [] + expected_read_count = 0 + read_values: set[int] = set() + else: + detection = daemon._detect_sign(frame_bgr) + predicted = detection.speed_limit_mph if detection is not None else None + confidence = detection.confidence if detection is not None else None + proposals = daemon._collect_detector_classifier_proposals(frame_bgr) + if args.max_proposals > 0: + proposals = proposals[:args.max_proposals] + + expected_read_count = 0 + read_values = set() + should_write_proposals = not args.only_errors + if not negative and predicted != expected: + should_write_proposals = True + if negative and predicted is not None: + should_write_proposals = True + + for proposal_index, (proposal_confidence, class_id, bbox) in enumerate(proposals): + for read in crop_reads(daemon, frame_bgr, bbox): + speeds = [speed for speed in (read["model_speed"], read["ocr_speed"]) if speed is not None] + read_values.update(int(speed) for speed in speeds) + if expected is not None and expected in speeds: + expected_read_count += 1 + if should_write_proposals: + proposal_rows.append({ + "record_key": row.get("record_key", ""), + "proposal_index": str(proposal_index), + "proposal_confidence": f"{proposal_confidence:.6f}", + "class_id": str(class_id), + "bbox": ",".join(str(int(value)) for value in bbox), + "expansion_index": str(read["expansion_index"]), + "expanded_bbox": ",".join(str(int(value)) for value in read["expanded_bbox"]), + "expansion_weight": f"{read['expansion_weight']:.3f}", + "is_regulatory": str(bool(read["is_regulatory"])), + "model_speed": "" if read["model_speed"] is None else str(read["model_speed"]), + "model_confidence": "" if read["model_confidence"] is None else f"{read['model_confidence']:.6f}", + "ocr_speed": "" if read["ocr_speed"] is None else str(read["ocr_speed"]), + "ocr_confidence": "" if read["ocr_confidence"] is None else f"{read['ocr_confidence']:.6f}", + }) + + failure_type = classify_row(expected, negative, predicted, len(proposals), expected_read_count) + + failure_counts[failure_type] += 1 + output_rows.append({ + "record_key": row.get("record_key", ""), + "split": row.get("split", ""), + "sample_type": row.get("sample_type", ""), + "image_path": "" if image_path is None else str(image_path), + "expected_speed_limit_mph": "" if expected is None else str(expected), + "predicted_speed_limit_mph": "" if predicted is None else str(predicted), + "confidence": "" if confidence is None else f"{confidence:.6f}", + "negative": str(negative), + "proposal_count": str(len(proposals)), + "expected_read_count": str(expected_read_count), + "read_values": " ".join(str(value) for value in sorted(read_values)), + "failure_type": failure_type, + }) + + args.output_rows.parent.mkdir(parents=True, exist_ok=True) + with args.output_rows.open("w", encoding="utf-8", newline="") as output_file: + fieldnames = ( + "record_key", + "split", + "sample_type", + "image_path", + "expected_speed_limit_mph", + "predicted_speed_limit_mph", + "confidence", + "negative", + "proposal_count", + "expected_read_count", + "read_values", + "failure_type", + ) + writer = csv.DictWriter(output_file, fieldnames=fieldnames) + writer.writeheader() + writer.writerows(output_rows) + + args.output_proposals.parent.mkdir(parents=True, exist_ok=True) + with args.output_proposals.open("w", encoding="utf-8", newline="") as output_file: + fieldnames = ( + "record_key", + "proposal_index", + "proposal_confidence", + "class_id", + "bbox", + "expansion_index", + "expanded_bbox", + "expansion_weight", + "is_regulatory", + "model_speed", + "model_confidence", + "ocr_speed", + "ocr_confidence", + ) + writer = csv.DictWriter(output_file, fieldnames=fieldnames) + writer.writeheader() + writer.writerows(proposal_rows) + + print(f"Rows: {len(output_rows)}") + for failure_type, count in failure_counts.most_common(): + print(f"{failure_type}: {count}") + print(f"Wrote {args.output_rows}") + print(f"Wrote {args.output_proposals}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/speed_limit_vision/evaluate_bookmark_leadins.py b/scripts/speed_limit_vision/evaluate_bookmark_leadins.py index a5c9112b0..2060688ab 100644 --- a/scripts/speed_limit_vision/evaluate_bookmark_leadins.py +++ b/scripts/speed_limit_vision/evaluate_bookmark_leadins.py @@ -54,7 +54,7 @@ class LiveReplayDaemon(slv.SpeedLimitVisionDaemon): slv.time.monotonic = lambda now=now: now self.current_frame_bgr = frame_bgr - inference_interval = slv.FOLLOWUP_INFERENCE_INTERVAL if now < self.followup_until else slv.INFERENCE_INTERVAL + inference_interval = self._inference_interval(now) if now - self.last_inference_at < inference_interval: if self.published_speed_limit_mph > 0 and self._published_detection_stale(now): self._write_debug_event("stale_clear", reason="hold_timeout") @@ -78,7 +78,7 @@ def parse_args(): parser.add_argument("--session-route-map", type=Path, default=common.preferred_session_route_map_path(), help="JSON file mapping debug session ids to route log ids.") parser.add_argument("--models-dir", type=Path, help="Directory containing speed_limit_us_detector.onnx and speed_limit_us_value_classifier.onnx.") parser.add_argument("--lead-in", type=float, default=5.0, help="Seconds before each bookmark to replay.") - parser.add_argument("--sample-fps", type=float, help="Optional decode sample rate. Use 5 for faster bookmark sweeps that still match the live inference cadence.") + parser.add_argument("--sample-fps", type=float, help="Optional decode sample rate. Leave unset for the closest live-cadence replay; use 8+ for faster approximate sweeps.") parser.add_argument("--session", action="append", help="Optional session id filter. Repeat to run more than one.") parser.add_argument("--bookmark", action="append", type=int, help="Optional bookmark number filter within the selected sessions.") parser.add_argument("--json-out", type=Path, help="Optional path to write the summary JSON.") diff --git a/scripts/speed_limit_vision/replay_route_runtime.py b/scripts/speed_limit_vision/replay_route_runtime.py new file mode 100644 index 000000000..f162af0d9 --- /dev/null +++ b/scripts/speed_limit_vision/replay_route_runtime.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import bisect +import bz2 +import csv + +from dataclasses import dataclass +from pathlib import Path + +import cv2 +import zstandard as zstd +from cereal import log + +import starpilot.system.speed_limit_vision as slv + + +@dataclass(frozen=True) +class RouteSummary: + route: str + segments: int + qlog_context: bool + sampled_frames: int + inference_frames: int + candidate_events: int + publish_events: int + stale_clear_events: int + road_change_events: int + + +@dataclass +class QlogRuntimeContext: + cpu_times: list[float] + cpu_busy: list[bool] + live_pose_times: list[float] + live_pose_inputs_ok: list[bool] + road_times: list[float] + road_names: list[str] + started_times: list[float] + started: list[bool] + + def _last_index(self, times: list[float], now: float) -> int: + return bisect.bisect_right(times, now) - 1 + + def device_cpu_busy_at(self, now: float) -> bool: + index = self._last_index(self.cpu_times, now) + return index >= 0 and self.cpu_busy[index] + + def live_pose_inputs_ok_at(self, now: float) -> bool: + index = self._last_index(self.live_pose_times, now) + return index < 0 or self.live_pose_inputs_ok[index] + + def road_name_at(self, now: float) -> str: + index = self._last_index(self.road_times, now) + return self.road_names[index] if index >= 0 else "" + + def started_at(self, now: float) -> bool: + index = self._last_index(self.started_times, now) + return index < 0 or self.started[index] + + +class RouteReplayDaemon(slv.SpeedLimitVisionDaemon): + def __init__(self, runtime_context: QlogRuntimeContext | None): + super().__init__(use_runtime=False) + self.runtime_context = runtime_context + self.now = 0.0 + self.sampled_frames = 0 + self.inference_frames = 0 + self.events: list[dict[str, str]] = [] + + def _write_debug_event(self, event_type, frame_bgr=None, snapshot_prefix=None, **fields): + if event_type not in ("candidate", "publish", "stale_clear", "road_change"): + return + record = { + "time_s": f"{self.now:.3f}", + "event": event_type, + } + for key, value in fields.items(): + record[key] = str(value) + self.events.append(record) + + def _publish_status(self, status, clear_speed=False): + if clear_speed: + self._clear_detection() + + def _device_cpu_busy(self): + if self.runtime_context is None: + return False + return self.runtime_context.device_cpu_busy_at(self.now) + + def prepare_tick(self, now: float) -> bool: + self.now = now + slv.time.monotonic = lambda now=now: now + + if self.runtime_context is None: + return True + + if not self.runtime_context.started_at(now): + if self.published_speed_limit_mph > 0: + self._clear_detection() + self.last_road_name = "" + return False + + if not self.runtime_context.live_pose_inputs_ok_at(now): + self.last_live_pose_inputs_not_ok_at = now + + road_name = self.runtime_context.road_name_at(now) + if self.last_road_name and road_name and road_name != self.last_road_name: + self._write_debug_event("road_change", previousRoadName=self.last_road_name, roadName=road_name) + self._clear_detection() + self.last_road_name = road_name or self.last_road_name + return True + + def process_frame(self, now: float, frame_bgr): + self.sampled_frames += 1 + if not self.prepare_tick(now): + return + self.current_frame_bgr = frame_bgr + + inference_interval = self._inference_interval(now) + if now - self.last_inference_at < inference_interval: + if self.published_speed_limit_mph > 0 and self._published_detection_stale(now): + self._write_debug_event("stale_clear", reason="inference_interval") + self._clear_detection() + return + + self.last_inference_at = now + self.inference_frames += 1 + detection = self._detect_sign(frame_bgr) + if detection is not None: + self._update_detection(detection) + elif self.published_speed_limit_mph > 0 and self._published_detection_stale(now): + self._write_debug_event("stale_clear", reason="no_detection") + self._clear_detection() + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Replay downloaded route camera segments through the runtime speed-limit vision cadence.") + 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("--models-dir", type=Path, default=Path("starpilot/assets/vision_models"), help="Directory containing runtime ONNX models.") + parser.add_argument("--output-csv", type=Path, help="Optional CSV of candidate/publish/stale_clear events.") + parser.add_argument("--start", type=float, default=0.0, help="Skip route time before this second.") + parser.add_argument("--end", type=float, help="Stop once route time exceeds this second.") + parser.add_argument("--progress", action="store_true", help="Print a one-line progress update after each segment.") + parser.add_argument("--fast-seek", action="store_true", help="Use VideoCapture seeks when skipping frames. Faster, but less faithful for HEVC.") + parser.add_argument("--qlog-context", action="store_true", help="Replay with logged deviceState/livePose/mapdOut context for closer runtime cadence.") + 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 segment_paths(clip_root: Path, log_id: str) -> list[Path]: + return sorted( + (path for path in clip_root.glob(f"{log_id}--*/fcamera.hevc") if not path.name.startswith("._")), + key=segment_index, + ) + + +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 build_runtime_context(qlogs: list[Path]) -> QlogRuntimeContext: + cpu_times: list[float] = [] + cpu_busy: list[bool] = [] + live_pose_times: list[float] = [] + live_pose_inputs_ok: list[bool] = [] + road_times: list[float] = [] + road_names: list[str] = [] + started_times: list[float] = [] + started: list[bool] = [] + + for qlog_path in qlogs: + events = list(read_qlog(qlog_path)) + if not events: + continue + + segment_start_s = max(segment_index(qlog_path), 0) * 60.0 + segment_first_time_ns = events[0].logMonoTime + + for event in events: + now = segment_start_s + (event.logMonoTime - segment_first_time_ns) / 1e9 + event_type = event.which() + if event_type == "deviceState": + device_state = event.deviceState + usage = list(device_state.cpuUsagePercent) + busy = slv.device_cpu_usage_busy(usage) + cpu_times.append(now) + cpu_busy.append(busy) + started_times.append(now) + started.append(bool(device_state.started)) + elif event_type == "livePose": + live_pose_times.append(now) + live_pose_inputs_ok.append(bool(event.livePose.inputsOK)) + elif event_type == "mapdOut": + road_name = str(event.mapdOut.roadName or "") + if road_name: + road_times.append(now) + road_names.append(road_name) + + return QlogRuntimeContext( + cpu_times=cpu_times, + cpu_busy=cpu_busy, + live_pose_times=live_pose_times, + live_pose_inputs_ok=live_pose_inputs_ok, + road_times=road_times, + road_names=road_names, + started_times=started_times, + started=started, + ) + + +def configure_models(models_dir: Path) -> None: + models_dir = models_dir.expanduser().resolve() + slv.US_DETECTOR_MODEL_PATH = models_dir / "speed_limit_us_detector.onnx" + slv.US_CLASSIFIER_MODEL_PATH = models_dir / "speed_limit_us_value_classifier.onnx" + slv.US_REJECT_CLASSIFIER_MODEL_PATH = models_dir / "speed_limit_us_reject_classifier.onnx" + + +def skip_to_frame(capture, frame_index: int, target_index: int, fast_seek: bool) -> int: + if target_index <= frame_index: + return frame_index + if fast_seek: + capture.set(cv2.CAP_PROP_POS_FRAMES, target_index) + return target_index + while frame_index < target_index: + if not capture.grab(): + return target_index + frame_index += 1 + return frame_index + + +def replay_route( + log_id: str, + segments: list[Path], + runtime_context: QlogRuntimeContext | None, + start_s: float, + end_s: float | None, + progress: bool, + fast_seek: bool, +) -> tuple[RouteSummary, list[dict[str, str]]]: + daemon = RouteReplayDaemon(runtime_context) + for segment_path in segments: + segment = segment_index(segment_path) + capture = cv2.VideoCapture(str(segment_path)) + fps = capture.get(cv2.CAP_PROP_FPS) or 20.0 + total_frames = int(capture.get(cv2.CAP_PROP_FRAME_COUNT) or 0) + segment_start_s = segment * 60.0 + frame_index = max(int(round(max(start_s - segment_start_s, 0.0) * fps)), 0) + if frame_index > 0: + if fast_seek: + capture.set(cv2.CAP_PROP_POS_FRAMES, frame_index) + else: + frame_index = skip_to_frame(capture, 0, frame_index, fast_seek=False) + + while total_frames <= 0 or frame_index < total_frames: + now = segment_start_s + frame_index / fps + if end_s is not None and now > end_s: + capture.release() + summary = summarize(log_id, len(segments), runtime_context is not None, daemon) + return summary, daemon.events + + if not daemon.prepare_tick(now): + frame_index = skip_to_frame(capture, frame_index, frame_index + 1, fast_seek) + continue + + inference_interval = daemon._inference_interval(now) + if now - daemon.last_inference_at < inference_interval: + next_due = daemon.last_inference_at + inference_interval + target_index = max(frame_index + 1, int(round((next_due - segment_start_s) * fps))) + if total_frames > 0: + target_index = min(target_index, total_frames) + if target_index <= frame_index: + target_index = frame_index + 1 + frame_index = skip_to_frame(capture, frame_index, target_index, fast_seek) + continue + + ok, frame_bgr = capture.read() + if not ok: + break + frame_index += 1 + daemon.process_frame(now, frame_bgr) + + capture.release() + if progress: + print( + f" seg {segment:02d}: sampled={daemon.sampled_frames} inference={daemon.inference_frames} " + f"events={len(daemon.events)}", + flush=True, + ) + + return summarize(log_id, len(segments), runtime_context is not None, daemon), daemon.events + + +def summarize(route: str, segment_count: int, qlog_context: bool, daemon: RouteReplayDaemon) -> RouteSummary: + event_counts = { + event_name: sum(1 for event in daemon.events if event["event"] == event_name) + for event_name in ("candidate", "publish", "stale_clear", "road_change") + } + return RouteSummary( + route=route, + segments=segment_count, + qlog_context=qlog_context, + sampled_frames=daemon.sampled_frames, + inference_frames=daemon.inference_frames, + candidate_events=event_counts["candidate"], + publish_events=event_counts["publish"], + stale_clear_events=event_counts["stale_clear"], + road_change_events=event_counts["road_change"], + ) + + +def write_events(path: Path, route_events: list[tuple[str, dict[str, str]]]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + fieldnames = [ + "route", "time_s", "event", "candidateSpeedLimitMph", "speedLimitMph", "confidence", "reason", + "previousRoadName", "roadName", + ] + with path.open("w", encoding="utf-8", newline="") as output_file: + writer = csv.DictWriter(output_file, fieldnames=fieldnames, extrasaction="ignore") + writer.writeheader() + for route, event in route_events: + row = {"route": route} + row.update(event) + writer.writerow(row) + + +def publish_speed_changes(events: list[dict[str, str]]) -> list[tuple[float, str]]: + changes: list[tuple[float, str]] = [] + last_speed = "" + for event in events: + if event["event"] in ("stale_clear", "road_change"): + last_speed = "" + continue + if event["event"] != "publish": + continue + + speed = event.get("speedLimitMph", "") + if not speed or speed == last_speed: + continue + changes.append((float(event["time_s"]), speed)) + last_speed = speed + return changes + + +def main() -> int: + args = parse_args() + configure_models(args.models_dir) + clip_root = args.clip_root.expanduser().resolve() + all_events: list[tuple[str, dict[str, str]]] = [] + + for route_input in args.routes: + log_id = route_log_id(route_input) + paths = segment_paths(clip_root, log_id) + if not paths: + print(f"{log_id}: no fcamera.hevc segments found under {clip_root}") + continue + + runtime_context = None + if args.qlog_context: + qlogs = qlog_paths(clip_root, log_id) + if not qlogs: + print(f"{log_id}: no qlogs found under {clip_root}; replaying without qlog context") + else: + runtime_context = build_runtime_context(qlogs) + + summary, events = replay_route(log_id, paths, runtime_context, args.start, args.end, args.progress, args.fast_seek) + all_events.extend((log_id, event) for event in events) + print( + f"{summary.route}: segments={summary.segments} qlog_context={int(summary.qlog_context)} sampled={summary.sampled_frames} " + f"inference={summary.inference_frames} candidate={summary.candidate_events} " + f"publish={summary.publish_events} stale_clear={summary.stale_clear_events} road_change={summary.road_change_events}", + flush=True, + ) + publish_values = [event.get("speedLimitMph") for event in events if event["event"] == "publish"] + if publish_values: + print(f" publishes: {', '.join(publish_values)}", flush=True) + speed_changes = publish_speed_changes(events) + if speed_changes: + print(" speed changes: " + ", ".join(f"{time_s:.1f}s={speed}" for time_s, speed in speed_changes), flush=True) + + if args.output_csv: + write_events(args.output_csv.expanduser().resolve(), all_events) + print(f"Wrote {args.output_csv.expanduser().resolve()}", flush=True) + + 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 c802b52f9..c9f86afe5 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -16,21 +16,25 @@ from openpilot.common.constants import CV from openpilot.common.realtime import set_core_affinity from openpilot.system.hardware import PC -INFERENCE_INTERVAL = 0.25 -FOLLOWUP_INFERENCE_INTERVAL = 0.2 +RUNTIME_LOOP_HZ = 20 +INFERENCE_INTERVAL = 0.15 +FOLLOWUP_INFERENCE_INTERVAL = 0.10 FOLLOWUP_WINDOW_SECONDS = 2.0 BUSY_INFERENCE_INTERVAL = 1.0 LIVE_POSE_RECOVERY_THROTTLE_SECONDS = 2.0 LIVE_POSE_RECOVERY_INFERENCE_INTERVAL = 1.0 DEVICE_BUSY_AVG_CPU_USAGE_PERCENT = 78.0 DEVICE_BUSY_MAX_CPU_USAGE_PERCENT = 92.0 +DEVICE_BUSY_HOT_CORE_COUNT = 4 MIN_DETECTION_CONFIDENCE = 0.2 STRONG_DETECTION_CONFIDENCE = 0.72 OCR_MIN_CONFIDENCE = 0.35 -VALUE_TEMPLATE_MIN_CONFIDENCE = 0.62 +VALUE_TEMPLATE_MIN_CONFIDENCE = 0.55 HISTORY_SECONDS = 2.0 CONSISTENT_DETECTIONS = 2 -CHANGE_CONSISTENT_DETECTIONS = 3 +CHANGE_CONSISTENT_DETECTIONS = 10 +LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS = 12 +LOW_SPEED_CHANGE_MIN_CONFIDENCE = 0.97 MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE = 0.65 PUBLISHED_HOLD_SECONDS = 300.0 PUBLISHED_CHANGE_COOLDOWN_SECONDS = 1.4 @@ -184,6 +188,16 @@ SNAPSHOT_JPEG_QUALITY = 85 SPEED_LIMIT_VISION_AFFINITY_CORES = [0, 1, 2] +def device_cpu_usage_busy(cpu_usage): + usage = list(cpu_usage) + if not usage: + return False + return ( + sum(usage) / len(usage) >= DEVICE_BUSY_AVG_CPU_USAGE_PERCENT or + sum(core_usage >= DEVICE_BUSY_MAX_CPU_USAGE_PERCENT for core_usage in usage) >= DEVICE_BUSY_HOT_CORE_COUNT + ) + + @dataclass class Detection: speed_limit_mph: int @@ -232,9 +246,9 @@ class SpeedLimitVisionDaemon: self.classifier_net = None self.model_mode = "legacy" self.last_error = "" - self.last_inference_at = 0.0 + self.last_inference_at = -float("inf") self.last_detection_at = 0.0 - self.last_live_pose_inputs_not_ok_at = 0.0 + self.last_live_pose_inputs_not_ok_at = -float("inf") self.last_road_name = "" self.followup_until = 0.0 self.started_prev = False @@ -701,20 +715,14 @@ class SpeedLimitVisionDaemon: def _device_cpu_busy(self): if self.sm is None: return False - cpu_usage = list(self.sm["deviceState"].cpuUsagePercent) - if not cpu_usage: - return False - return ( - sum(cpu_usage) / len(cpu_usage) >= DEVICE_BUSY_AVG_CPU_USAGE_PERCENT or - max(cpu_usage) >= DEVICE_BUSY_MAX_CPU_USAGE_PERCENT - ) + return device_cpu_usage_busy(self.sm["deviceState"].cpuUsagePercent) def _inference_interval(self, now): in_followup = now < self.followup_until interval = FOLLOWUP_INFERENCE_INTERVAL if in_followup else INFERENCE_INTERVAL if now - self.last_live_pose_inputs_not_ok_at < LIVE_POSE_RECOVERY_THROTTLE_SECONDS: interval = max(interval, LIVE_POSE_RECOVERY_INFERENCE_INTERVAL) - elif not in_followup and self._device_cpu_busy(): + elif self._device_cpu_busy(): interval = max(interval, BUSY_INFERENCE_INTERVAL) return interval @@ -1696,7 +1704,12 @@ class SpeedLimitVisionDaemon: current_count = counts.get(current_speed_limit, 0) if current_speed_limit > 0 else 0 if current_speed_limit > 0 and candidate_speed_limit != current_speed_limit: - if candidate_count < CHANGE_CONSISTENT_DETECTIONS: + required_count = CHANGE_CONSISTENT_DETECTIONS + if current_speed_limit >= 30 and candidate_speed_limit < 30: + required_count = LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS + if best_confidence < LOW_SPEED_CHANGE_MIN_CONFIDENCE: + return None + if candidate_count < required_count: return None if candidate_count <= current_count: return None @@ -1816,7 +1829,7 @@ class SpeedLimitVisionDaemon: if not self.use_runtime or self.sm is None: raise RuntimeError("SpeedLimitVisionDaemon runtime loop requires use_runtime=True") - ratekeeper = self.Ratekeeper(5, None) + ratekeeper = self.Ratekeeper(RUNTIME_LOOP_HZ, None) while True: self.sm.update(0)