| |
| """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 |
| 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 |
| 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"]}) |
| |
| 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() |
|
|
