#!/usr/bin/env python3 """Join H2 timing records by command_stamp_ns; never opens robot/DDS/SHM.""" from __future__ import annotations import argparse import csv import json from pathlib import Path import time import numpy as np def number(row, key): return int(row.get(key) or 0) def distribution(values): if not values: return {"n": 0} a = np.asarray(values, dtype=float) return {"n": len(a), **{key: float(np.percentile(a, p)) for key, p in (("p50", 50), ("p95", 95), ("p99", 99), ("p99_9", 99.9))}, "max": float(np.max(a)), "negative_count": int(np.sum(a < 0))} def analyze(directory: Path): """All durations are ms. DDS publish is a host boundary, not motor receipt.""" directory = Path(directory) if (directory / "timing").is_dir(): directory = directory / "timing" metrics = {} commands = {} generation_intervals = [] event_log = [] def add(name, value, group="all"): metrics.setdefault(group, {}).setdefault(name, []).append(value) def delta(row, name, end, start, group="all"): a, b = number(row, end), number(row, start) if a and b: add(name, (a - b) / 1e6, group) for path in sorted(directory.glob("runner-*.csv")): starts = {} with path.open() as f: for row in csv.DictReader(f): if not row.get("event") or None in row or any(v is None for v in row.values()): continue # The last line may still be in flight. event = row["event"] key = row.get("reference_id") if event == "generation_start": starts[key] = number(row, "t_ns") elif event == "generation_end" and key in starts: start, end = starts.pop(key), number(row, "t_ns") generation_intervals.append((start, end)) add("generation_wall_ms", (end - start) / 1e6) elif event == "cycle": delta(row, "runner_cycle_work_ms", "cycle_end_ns", "cycle_start_ns") elif event == "command" and number(row, "command_stamp_ns"): commands[number(row, "command_stamp_ns")] = row if event in ("generation_start", "generation_end", "generation_error", "reference_adopt"): event_log.append({k: row[k] for k in ("event", "t_ns", "reference_id", "skip_frames", "wait_frames")}) for row in commands.values(): groups = ["all", row["mode"] or "unknown"] start, end = number(row, "cycle_start_ns"), number(row, "write_end_ns") overlap = any(a <= end and b >= start for a, b in generation_intervals) if row["mode"] == "policy": groups.append("policy_generating" if overlap else "policy_no_generation") row["_groups"] = groups for group in groups: for name, finish, begin in ( ("state_age_at_infer_ms", "infer_start_ns", "state_stamp_ns"), ("state_age_at_shm_write_ms", "command_stamp_ns", "state_stamp_ns"), ("shm_read_ms", "read_end_ns", "read_start_ns"), ("inference_ms", "infer_end_ns", "infer_start_ns"), ("runner_read_to_write_ms", "command_stamp_ns", "read_end_ns"), ("shm_write_call_ms", "write_end_ns", "write_start_ns")): delta(row, name, finish, begin, group) seen = set() unmatched = set() projected = published = held = 0 worst = [] for path in sorted(directory.glob("bridge-*.csv")): last_publish = last_cycle = last_new = 0 last_mode = None with path.open() as f: for row in csv.DictReader(f): if None in row or any(v is None for v in row.values()): continue tick = number(row, "cycle_start_ns") if last_cycle and tick: add("bridge_cycle_interval_ms", (tick - last_cycle) / 1e6) last_cycle = tick delta(row, "bridge_cycle_work_ms", "cycle_end_ns", "cycle_start_ns") delta(row, "bridge_shm_read_ms", "read_end_ns", "read_start_ns") pub = number(row, "publish_start_ns") if not pub: continue published += 1 projected += number(row, "projected_count") > 0 add("projection_max_rad", float(row["max_projection_rad"])) if last_publish: add("bridge_publish_interval_ms", (pub - last_publish) / 1e6) last_publish = pub delta(row, "dds_publish_call_ms", "publish_end_ns", "publish_start_ns") if number(row, "held"): held += 1 continue # A latched hold is not a newly applied policy command. stamp = number(row, "command_stamp_ns") add("command_age_at_each_publish_ms", (pub - stamp) / 1e6) if stamp in seen: continue seen.add(stamp) command = commands.get(stamp) if command is None: unmatched.add(stamp) last_new, last_mode = pub, None continue age = (pub - number(command, "state_stamp_ns")) / 1e6 worst.append({"publish_ns": pub, "command_stamp_ns": stamp, "mode": command["mode"], "reference_id": command["reference_id"], "host_state_to_publish_ms": age, "tilt_rad": command["tilt_rad"], "gyro_norm": command["gyro_norm"], "q_jump_rad": command["q_jump_rad"]}) # Keep bounded diagnostic exemplars while streaming long bridge recordings. if len(worst) > 100: worst = sorted(worst, key=lambda x: x["host_state_to_publish_ms"], reverse=True)[:20] for group in command["_groups"]: add("shm_to_first_publish_ms", (pub - stamp) / 1e6, group) add("host_state_to_first_publish_ms", age, group) if last_new and (group == "all" or last_mode == command["mode"]): gap = (pub - last_new) / 1e6 add("new_command_interval_ms", gap, group) add("new_command_interval_excess_over_20ms", max(0., gap - 20.), group) last_new, last_mode = pub, command["mode"] stats = {} for path in sorted(directory.glob("*.stats.json")): try: stats[path.name] = json.loads(path.read_text()) except (OSError, json.JSONDecodeError): stats[path.name] = {"error": "stats unavailable"} marks = [] marker_path = directory / "marks.csv" if marker_path.exists(): with marker_path.open() as f: marks = list(csv.DictReader(f)) for mark in marks: center = number(mark, "t_ns") nearby = [r for r in commands.values() if abs(number(r, "write_end_ns") - center) <= 1_000_000_000] mark["commands_within_1s"] = len(nearby) mark["inference_ms_within_1s"] = distribution([ (number(r, "infer_end_ns") - number(r, "infer_start_ns")) / 1e6 for r in nearby if number(r, "infer_start_ns") and number(r, "infer_end_ns")]) return { "directory": str(directory), "duration_units": "ms (projection_max_rad is rad)", "boundary": "PC LowState software stamp -> DDS publish call; excludes robot acquisition, wire and motor response", "commands_recorded": len(commands), "first_publishes_matched": len(seen - unmatched), "first_publishes_without_runner_record": len(unmatched), "commands_without_observed_publish": len(set(commands) - seen), "bridge_publishes": published, "latched_hold_publishes": held, "projected_publishes": projected, "recorders": stats, "metrics": {group: {k: distribution(v) for k, v in values.items()} for group, values in metrics.items()}, "worst_host_delays": sorted(worst, key=lambda x: x["host_state_to_publish_ms"], reverse=True)[:20], "reference_events": event_log, "marks": marks, "caveats": [ "Repeated 1000 Hz sends are not new 50 Hz policy commands.", "Command update excess is jitter, not a measured fixed actuator delay.", "Drops, unmatched commands, pending writes or unclosed recorders limit completeness.", "Stage-B delay thresholds must not be assumed for Stage-C or other policies.", ], } def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("run_dir", type=Path) parser.add_argument("--json", type=Path, help="Save full report.") parser.add_argument("--mark", help="Append a human event marker using this host's monotonic clock; no robot action.") args = parser.parse_args() directory = args.run_dir / "timing" if (args.run_dir / "timing").is_dir() else args.run_dir if args.mark is not None: path = directory / "marks.csv" new = not path.exists() with path.open("a", newline="") as f: writer = csv.writer(f) if new: writer.writerow(("t_ns", "label")) writer.writerow((time.monotonic_ns(), args.mark)) print(f"Recorded event: {args.mark}; no control action") return report = analyze(directory) if args.json: args.json.write_text(json.dumps(report, indent=2) + "\n") print(f"Recorded commands: {report['commands_recorded']}; matched first publishes: {report['first_publishes_matched']}") print("PC-side only; not full sensor-to-motor latency. Missing bridge data means no publish latency measurement.") for group, values in report["metrics"].items(): print(f"\n[{group}]") for name, value in values.items(): if value["n"]: print(f"{name:40s} n={value['n']:7d} p50={value['p50']:.3f} " f"p99={value['p99']:.3f} p99.9={value['p99_9']:.3f} max={value['max']:.3f}") print("\nRecorder health:", json.dumps(report["recorders"])) if __name__ == "__main__": main()