| """Bounded, best-effort timing records. No file I/O on the control thread.""" |
|
|
| from __future__ import annotations |
|
|
| import csv |
| import json |
| import os |
| from pathlib import Path |
| import threading |
| import time |
|
|
|
|
| FIELDS = ( |
| "event", "t_ns", "command_stamp_ns", "state_stamp_ns", "tt_stamp_ns", |
| "read_start_ns", "read_end_ns", "infer_start_ns", "infer_end_ns", |
| "write_start_ns", "write_end_ns", "cycle_start_ns", "cycle_end_ns", |
| "mode", "reference_id", "frozen", "generating", "tilt_rad", |
| "gyro_norm", "dq_max", "q_jump_rad", "robot_src_age_s", "ball_src_age_s", |
| "step_dt_s", "skip_frames", "wait_frames", |
| "time_to_hit_s", |
| ) |
|
|
|
|
| class TimingTrace: |
| def __init__(self, path: Path, *, capacity: int = 8192, metadata=None): |
| self.path = Path(path) |
| self.path.parent.mkdir(parents=True, exist_ok=True) |
| self._file = self.path.open("x", newline="") |
| self._writer = csv.writer(self._file) |
| self._writer.writerow(FIELDS) |
| self._file.flush() |
| self._slots = [None] * capacity |
| self._head = self._tail = 0 |
| self._lock = threading.Lock() |
| self._stop = threading.Event() |
| self.dropped = self.written = 0 |
| self.error = "" |
| self._closed = False |
| self.metadata = dict(metadata or {}) |
| try: |
| self._stats(False) |
| except OSError: |
| self._file.close() |
| raise |
| self._thread = threading.Thread(target=self._drain, name="h2-timing-writer", daemon=True) |
| self._thread.start() |
|
|
| def record(self, event: str, **values) -> None: |
| if self._stop.is_set() or not self._lock.acquire(blocking=False): |
| self.dropped += 1 |
| return |
| try: |
| if self._head - self._tail == len(self._slots): |
| self.dropped += 1 |
| return |
| values.setdefault("t_ns", time.monotonic_ns()) |
| values["event"] = event |
| self._slots[self._head % len(self._slots)] = tuple(values.get(k, "") for k in FIELDS) |
| self._head += 1 |
| finally: |
| self._lock.release() |
|
|
| def _stats(self, closed: bool) -> None: |
| stats = {**self.metadata, "clock": "CLOCK_MONOTONIC", "pid": os.getpid(), |
| "accepted": self._head, "written": self.written, "dropped": self.dropped, |
| "pending": self._head - self._tail, "closed": closed, "error": self.error} |
| target = self.path.with_suffix(".stats.json") |
| temporary = target.with_suffix(".tmp") |
| temporary.write_text(json.dumps(stats, indent=2) + "\n") |
| temporary.replace(target) |
|
|
| def _drain(self) -> None: |
| try: |
| self._stop.wait(0.25) |
| while True: |
| with self._lock: |
| end = min(self._head, self._tail + 512) |
| rows = [] |
| while self._tail < end: |
| i = self._tail % len(self._slots) |
| rows.append(self._slots[i]) |
| self._slots[i] = None |
| self._tail += 1 |
| empty = self._tail == self._head |
| self._writer.writerows(rows) |
| self.written += len(rows) |
| self._file.flush() |
| self._stats(False) |
| if self._stop.is_set(): |
| with self._lock: |
| if self._tail == self._head: |
| break |
| if empty: |
| self._stop.wait(0.25) |
| except Exception as error: |
| |
| self.error = str(error) |
| self._stop.set() |
| print(f"[TIMING WARN] recording stopped: {error}") |
| finally: |
| try: |
| self._stats(True) |
| except OSError: |
| pass |
| self._file.close() |
|
|
| def close(self) -> None: |
| if not self._closed: |
| self._closed = True |
| self._stop.set() |
| self._thread.join(timeout=2.0) |
|
|
|
|
| def trace_from_environment(**metadata) -> TimingTrace | None: |
| directory = os.environ.get("H2_TIMING_DIR") |
| if not directory: |
| return None |
| try: |
| trace = TimingTrace(Path(directory) / f"runner-{os.getpid()}.csv", metadata=metadata) |
| except OSError as error: |
| print(f"[TIMING WARN] cannot open recording: {error}") |
| return None |
| print(f"[TIMING] {trace.path} (CLOCK_MONOTONIC; diagnostic only)") |
| return trace |
|
|
