File size: 4,525 Bytes
d8290eb | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 | """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:
# A broken diagnostic disk must never stop robot control.
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
|