commit c68ba076d3f88772b68cbcb2f8a7e3876eb9f931 Author: Eiichi Shimazu Date: Sun Nov 15 09:51:18 2026 +0900 手教示の記録 (states.csv) に, 電流の記録スレッドが read_servo で読んだ角度も actual 行として書く (依頼 202・依頼 197 の案 1・Log37) policy_recorder.CurrentsRecorder に angle_sink を足し, 読めた回だけ (角度, 角度を読んだ時刻 = currents.csv の ts) を渡す. 読む命令 (read_servo)・読む時機 (本体が眠っている間だけ)・電流の記録は変えない. 渡す側の例外は loop_errors に数えて続ける. record_teach_demo.py (git の外) の _Recorder に start_current_actuals / actual_from_currents / close_current_actuals と送り手の スレッドを足した. 列は足さず note 列を source=current_thread にする (本体の読みは note が空のまま). 行を時刻順に保つため, stage・actual・gripper_cmd・電流スレッドの角度を 1 つの鍵の中で列に積み, 送り手が積んだ順に送る (鍵の中では送らない. 本体も電流スレッドも送信を待たない). 先に積んだ行より前の時刻の電流スレッドの角度は積まずに数える. 送り切れないまま閉じても trial_end を列の最後に積んで追い越さない. 試行 JSON に送った数・積まなかった数・時計が戻った数などを残す. 本体の送信・到達と静止の確かめ・爪の前後の止め方・待つ長さは変えない (偽の daemon で関節と爪の命令・get_angles の回数・ 到達と静止の確かめの結果が角度を渡さない再生と同じ). 偽の daemon で actual の間の中央値 0.333 → 0.105 秒. pi05/teach_state_rate.py: 直す前と後の本の actual の間隔 (全部・本体だけ・電流スレッドだけ・原因ごと) と試行 JSON の segments を並べる (読むだけ. --out で KIOXIA に CSV). テスト test_teach_current_actuals 22 件. test_teach_currents は cmd_run の 書き方の確かめを今の呼び方に合わせた. pi05 のテストは test_state_source (HEAD と比べる作りで依頼 196 のコミット後は必ず落ちる. 今回は触っていない) のほかは OK, test_lab_start.sh 237 OK. codex 4 回 (4 回目で必須なし) Co-Authored-By: Claude Opus 5.5 diff --git a/pi05/policy_recorder.py b/pi05/policy_recorder.py index b68ade9..02840a1 100644 --- a/pi05/policy_recorder.py +++ b/pi05/policy_recorder.py @@ -544,7 +544,9 @@ class CurrentsRecorder(_ServoCurrents): - pause() から resume() までは window があっても読まない (爪の命令と待ち・再送の前の待ち. 「爪を動かした直後にシリアルを触らない」(2026-10-19)) - states.csv・top.mp4 は今までどおり recorder.py (別のプロセス) が書く. ここは currents.csv と - currents_meta.json だけを書く (read_servo で読んだ角度は ts を決めるのに使い, どこにも書かない) + currents_meta.json だけを書く. read_servo で読んだ角度は ts を決めるのに使い, angle_sink があれば + angle_sink(角度, 読んだ時刻) で渡す (依頼 202: record_teach_demo が states.csv の actual 行にする. + 渡すのは読めた回だけ. 読み方・読む時機は変えない. 渡す側の失敗は loop_errors に数えて続ける) - daemon に read_servo がなければ (unknown cmd) 読むのをやめる (currents.csv は列名だけ) 記録の失敗で再生を止めない (例外は外へ出さない. stop()・pause() は書き込みで止まったスレッドを待ち続けない). daemon のソケットは本体と共用なので, 使う前に install_daemon_lock() を呼ぶこと. @@ -558,6 +560,7 @@ class CurrentsRecorder(_ServoCurrents): self.read_servo_fn = read_servo_fn self.read_angles_fn = lambda: None self.angles_ok = None + self.angle_sink = None # 依頼 202: 読めた角度を渡す先 (角度, 読んだ時刻). None なら渡さない self.currents_mode = "read_servo" self.n_reads = 0 self.n_currents = 0 @@ -716,7 +719,9 @@ class CurrentsRecorder(_ServoCurrents): self.next_at = t0 + ANGLE_INTERVAL n_before = len(self.read_ms) try: - self._read_once() + a, ts = self._read_once() + if a is not None and self.angle_sink is not None: + self.angle_sink(a, ts) # 依頼 202: states.csv の actual 行へ except Exception as e: # 記録の失敗で再生を止めない self.loop_errors += 1 if self.loop_errors <= 3: diff --git a/pi05/teach_state_rate.py b/pi05/teach_state_rate.py new file mode 100644 index 0000000..5020526 --- /dev/null +++ b/pi05/teach_state_rate.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python3 +"""teach_state_rate.py — 手教示の記録 (record_teach_demo.py run --execute) の実測角の間隔を, 直す前と後の本で並べる +(2026-11-15 依頼 202. 数え方は依頼 197 の KIOXIA diag/teach_state_rate_20261115/measure_gaps.py と同じ). + +依頼 202 で, 電流の記録スレッド (read_servo) が読んだ角度も states.csv の actual 行に書くようにした +(note 列が source=current_thread. 本体の読み = 到達・静止の確かめなどは note が空のまま). 実機で 1 本 → 5 本撮って, + - states.csv の actual の間隔 (全部・本体の読みだけ・電流スレッドの読みだけ) の中央値・90% 点・1 秒超 + - 間の原因ごと (poll = 到達・静止の確かめの間 / send = 送信の待ち / gripper = 爪の待ち / stage_change) + - 試行 JSON の segments: 動く区間ごとの送信の往復 (send_rtt_sec)・区間の長さ (segment_elapsed_sec)・再送・着いたか, + 爪の前の静止の確かめ (still_check の reads・elapsed_sec・still) +が直す前の本と同じ形かを見る. 合否の線は決めていない (表を出すだけ). + +入力: {runs_root}/{run}/{run}.json と {run}/recording/states.csv (runs_root の既定は KIOXIA の runs/run_pick_place) + python3 pi05/teach_state_rate.py --before run_0001_20261109_094839 run_0001_20261109_095308 --after run_0001_20261116_... + python3 pi05/teach_state_rate.py --after /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20261116_... # フォルダでも + python3 pi05/teach_state_rate.py --before ... --after ... --out # CSV も残す +--out のときだけ書く: /Volumes/KIOXIA/r2r2r-data/runs/experiments/teach_state_rate/{YYYYMMDD_HHMMSS}/ + runs.csv (本ごと)・segments.csv (区間ごと). KIOXIA がなければ止まる. 出し先がもうあれば止まる. run の記録には何も書かない. +""" +import argparse +import csv +import json +import os +import statistics +import sys +from datetime import datetime +from pathlib import Path + +KIOXIA_MOUNT = Path("/Volumes/KIOXIA") +DATA_ROOT = KIOXIA_MOUNT / "r2r2r-data" +RUNS_ROOT = DATA_ROOT / "runs" / "run_pick_place" +OUT_ROOT = DATA_ROOT / "runs" / "experiments" / "teach_state_rate" +CURRENT_THREAD_NOTE = "source=current_thread" # record_teach_demo.CURRENT_THREAD_NOTE と同じ +LONG = 1.0 + + +def ensure_ssd_mounted(): + """KIOXIA がマウントされていて書けるか. だめなら止める (ローカルへは書かない).""" + if not os.path.ismount(str(KIOXIA_MOUNT)): + sys.exit(f"[ABORT] KIOXIA がマウントされていません: {KIOXIA_MOUNT}") + if not DATA_ROOT.is_dir(): + sys.exit(f"[ABORT] {DATA_ROOT} がありません") + + +def run_dir(arg, runs_root): + p = Path(arg) + return p if p.is_dir() else Path(runs_root) / arg + + +def load_states(path): + rows = [] + with Path(path).open(newline="") as f: + for r in csv.DictReader(f): + try: + r["ts"] = float(r["ts"]) + except (KeyError, TypeError, ValueError): + continue + rows.append(r) + return rows + + +def is_actual(r): + return r.get("event_type") == "actual" and (r.get("j0") or "") != "" + + +def source_of(r): + return "current_thread" if CURRENT_THREAD_NOTE in (r.get("note") or "").split() else "main" + + +def classify(rows): + """actual の間ごとに原因を決める (依頼 197 の measure_gaps.classify と同じ決め方. 手教示の形). + rows は states.csv の行 (stage・gripper_cmd も入ったまま).""" + acts = [i for i, r in enumerate(rows) if is_actual(r)] + out = [] + for a, b in zip(acts, acts[1:]): + types = [m.get("event_type") for m in rows[a + 1:b]] + prev = rows[a - 1].get("event_type") if a > 0 else "" + if "gripper_cmd" in types or "gripper" in types: + cause = "gripper" + elif "commanded" in types or prev == "stage": + cause = "send" # record_teach_demo: stage → 送信前の読み → 送信 (wait=1.0) + elif "stage" in types: + cause = "stage_change" + else: + cause = "poll" + out.append({"t0": rows[a]["ts"], "t1": rows[b]["ts"], "gap": rows[b]["ts"] - rows[a]["ts"], + "stage": rows[a].get("stage"), "cause": cause}) + return out + + +def pct(v, p): + if not v: + return None + ys = sorted(v) + k = (len(ys) - 1) * p / 100.0 + lo = int(k) + hi = min(lo + 1, len(ys) - 1) + return ys[lo] + (ys[hi] - ys[lo]) * (k - lo) + + +def gap_stats(gaps): + v = [g["gap"] for g in gaps] + tot = sum(v) + lv = [x for x in v if x > LONG] + return {"n": len(v), "median": pct(v, 50), "p90": pct(v, 90), "max": max(v) if v else None, + "long_n": len(lv), "long_share": (sum(lv) / tot) if tot > 0 else None} + + +def order_ok(rows): + """states.csv の行が時刻順に並んでいるか (依頼 202). 逆になった所の数.""" + return sum(1 for a, b in zip(rows, rows[1:]) if b["ts"] < a["ts"]) + + +def measure(d): + """1 本の値 (dict) と区間ごとの行 (list).""" + d = Path(d) + name = d.name + rows = load_states(d / "recording" / "states.csv") + acts = [r for r in rows if is_actual(r)] + main_rows = [r for r in rows if not (is_actual(r) and source_of(r) == "current_thread")] + g_all = classify(rows) + g_main = classify(main_rows) + cur_ts = [r["ts"] for r in acts if source_of(r) == "current_thread"] + g_cur = [{"gap": b - a} for a, b in zip(cur_ts, cur_ts[1:])] + out = {"run": name, "n_actual": len(acts), "n_main": len(acts) - len(cur_ts), "n_current_thread": len(cur_ts), + "order_reversed": order_ok(rows)} + for key, g in (("all", g_all), ("main", g_main), ("cur", g_cur)): + s = gap_stats(g) + for k, v in s.items(): + out[f"{key}_{k}"] = v + for cause in ("poll", "send", "gripper"): + s = gap_stats([g for g in g_all if g["cause"] == cause]) + out[f"all_{cause}_median"], out[f"all_{cause}_max"] = s["median"], s["max"] + try: + j = json.loads((d / f"{name}.json").read_text()) + except Exception as e: + j = {} + out["json_error"] = f"{type(e).__name__}" + cur = j.get("currents") or {} + out.update({"result": j.get("result"), "overruns": cur.get("overruns"), + "sent_current_thread": j.get("recorder_actual_events_sent_current_thread"), + "late_current_thread": j.get("recorder_actual_current_thread_late"), + "actual_sampling_reads_ok": (j.get("actual_sampling") or {}).get("reads_ok")}) + segs = [] + for g in j.get("segments") or []: + sc = g.get("still_check") or {} + segs.append({"run": name, "step": g.get("step"), "kind": g.get("kind"), + "name": g.get("to_name") if g.get("kind") == "move" else g.get("action"), + "send_rtt_sec": g.get("send_rtt_sec"), "segment_elapsed_sec": g.get("segment_elapsed_sec"), + "retried": g.get("retried"), "arrival_ok": g.get("arrival_ok"), + "still_reads": sc.get("reads"), "still_elapsed_sec": sc.get("elapsed_sec"), + "still": sc.get("still")}) + out["still_reads_total"] = sum(s["still_reads"] or 0 for s in segs) + out["retried_n"] = sum(1 for s in segs if s["retried"]) + return out, segs + + +def fmt(v, nd=3): + if v is None: + return "-" + if isinstance(v, float): + return f"{v:.{nd}f}" + return str(v) + + +RUN_COLS = [("run", "run"), ("n_actual", "actual"), ("n_current_thread", "うち電流"), ("all_median", "間 中央"), + ("all_p90", "90%"), ("all_long_n", "1秒超"), ("all_poll_median", "poll中央"), ("all_send_median", "send中央"), + ("main_median", "本体だけ中央"), ("main_p90", "本体90%"), ("cur_median", "電流だけ中央"), + ("order_reversed", "逆順"), ("late_current_thread", "送らず"), ("overruns", "見込み超え"), + ("still_reads_total", "静止reads"), ("retried_n", "再送"), ("result", "結果")] + + +def print_group(label, ms): + print(f"\n## {label} ({len(ms)} 本)") + print(" " + " | ".join(h for _, h in RUN_COLS)) + for m in ms: + print(" " + " | ".join(fmt(m.get(k)) for k, _ in RUN_COLS)) + if len(ms) > 1: + def med(k): + v = [m[k] for m in ms if isinstance(m.get(k), (int, float))] + return statistics.median(v) if v else None + print(" (本ごとの中央値) " + " | ".join( + f"{h} {fmt(med(k))}" for k, h in RUN_COLS[1:] if k not in ("result",))) + + +def print_segments(groups): + """区間ごと (step 順) に, 群ごとの中央値を並べる. 区間の並びが群で違えば step の名前で分かる.""" + print("\n## 区間ごと (群ごとの中央値: 送信の往復 / 区間の長さ / 静止の確かめの reads)") + keys = [] + for _, segs in groups: + for s in segs: + k = (s["step"], s["kind"], s["name"]) + if k not in keys: + keys.append(k) + for k in sorted(keys, key=lambda x: (x[0] is None, x[0] or 0)): + cells = [] + for label, segs in groups: + ss = [s for s in segs if (s["step"], s["kind"], s["name"]) == k] + + def med(f): + v = [s[f] for s in ss if isinstance(s.get(f), (int, float)) and not isinstance(s.get(f), bool)] + return statistics.median(v) if v else None + cells.append(f"{label}: n={len(ss)} rtt {fmt(med('send_rtt_sec'))} 長さ {fmt(med('segment_elapsed_sec'))}" + f" reads {fmt(med('still_reads'))}") + print(f" step {k[0]} {k[1]} {k[2]} " + " / ".join(cells)) + + +def write_out(results, segs_all): + ensure_ssd_mounted() + out = OUT_ROOT / datetime.now().strftime("%Y%m%d_%H%M%S") + if out.exists(): + sys.exit(f"[ABORT] 出し先がもうあります: {out}") + out.mkdir(parents=True) + cols = sorted({k for r in results for k in r}) + cols = ["group"] + [c for c in cols if c != "group"] + with (out / "runs.csv").open("w", newline="") as f: + w = csv.DictWriter(f, fieldnames=cols) + w.writeheader() + w.writerows(results) + if segs_all: + with (out / "segments.csv").open("w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(segs_all[0].keys())) + w.writeheader() + w.writerows(segs_all) + print(f"\n→ {out}") + + +def main(argv=None): + ap = argparse.ArgumentParser(description="手教示の記録の実測角の間隔を直す前と後で並べる (依頼 202)") + ap.add_argument("--before", nargs="*", default=[], help="直す前の run (名前かフォルダ)") + ap.add_argument("--after", nargs="*", default=[], help="直したあとの run (名前かフォルダ)") + ap.add_argument("--runs-root", default=str(RUNS_ROOT)) + ap.add_argument("--out", action="store_true", help="KIOXIA の runs/experiments/teach_state_rate/ に CSV も残す") + a = ap.parse_args(argv) + if not a.before and not a.after: + ap.error("--before か --after に run を 1 本以上") + results, groups, segs_all = [], [], [] + for label, names in (("before", a.before), ("after", a.after)): + if not names: + continue + ms, ss = [], [] + for n in names: + d = run_dir(n, a.runs_root) + if not (d / "recording" / "states.csv").exists(): + sys.exit(f"[ABORT] states.csv がありません: {d}") + m, segs = measure(d) + m["group"] = label + for s in segs: + s["group"] = label + ms.append(m) + ss += segs + print_group(label, ms) + results += ms + groups.append((label, ss)) + segs_all += ss + print_segments(groups) + if a.out: + write_out(results, segs_all) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pi05/test_teach_current_actuals.py b/pi05/test_teach_current_actuals.py new file mode 100644 index 0000000..47540d1 --- /dev/null +++ b/pi05/test_teach_current_actuals.py @@ -0,0 +1,413 @@ +#!/usr/bin/env python3 +"""test_teach_current_actuals.py — 手教示の記録 (states.csv) に電流の記録スレッドが読んだ角度も actual 行として書く形 +(依頼 202. 依頼 197 の案 1) を偽の daemon で確かめる (アームなし・Pi なし. 2026-11-15). + + python3 pi05/test_teach_current_actuals.py # 全部 (再生を 2 回通すので 1 分ほど) + python3 pi05/test_teach_current_actuals.py -v + +偽の daemon は test_teach_currents.ArmDaemon (依頼 121) で, read_servo の角度だけ腕の今の角度にする (元は固定の値). +再生は cmd_run と同じ順 (install_daemon_lock → start_current_actuals → _start_currents(angle_sink= +recorder.actual_from_currents) → _replay → stop → close_current_actuals). recorder.py へ送る通知を集め, recorder.py の TrialWriter.write_state で states.csv にする. + +確かめること: + - 電流スレッドが読めた角度が actual 行 (note = source=current_thread) になり, 時刻は currents.csv の ts と同じ + - 本体の通知 (stage・本体の actual・gripper_cmd) と本体が daemon に送る命令は, 角度を渡さない再生と同じ + (到達・静止の確かめの結果と回数・関節と爪の命令の順と中身) + - states.csv の行は時刻順. 爪の命令から待ちの終わりまで電流スレッドの actual 行はない + - 既存の読み手が壊れない: build_demo_episodes.load_states・build_correction_episodes.gap_fill_flags + (依頼 196 の state_source)・teach_state_rate.py (数えるスクリプト) + - _Recorder: 先に積んだ行より前の時刻は積まずに数える・close_current_actuals のあとは積まない・壊れた値は数える. + 送信が止まっても本体の通知と電流スレッドは待たない (codex R1)・送り切れない分は数える・時計が戻った行は数える (R2). + 送り切れないまま閉じても積んだ行は trial_end を追い越さない (trial_end も列の最後に積む. codex 2・3 回目 R1) + start_current_actuals を呼ばなければ今までどおりその場で送る + - CurrentsRecorder: angle_sink が例外を出しても読み取りは続く (loop_errors に数える)・読めない角度は渡さない +KIOXIA にもアームにも触らない (一時フォルダと 127.0.0.1 だけ). +""" +import csv +import importlib.util +import io +import sys +import tempfile +import threading +import time +import unittest +from pathlib import Path +from unittest import mock + +_HERE = Path(__file__).resolve().parent +sys.path.insert(0, str(_HERE)) +sys.path.insert(0, str(_HERE.parent / "dynamic_tracking_limits" / "drag_teaching")) + +import test_teach_currents as ttc # noqa: E402 (ArmDaemon・WAYPOINTS. 偽の daemon と run_policy_client のスタブ) +import test_recorder_currents as trc # noqa: E402 +import policy_recorder as pr # noqa: E402 +import visual_servo_pick as vsp # noqa: E402 +import record_teach_demo as rtd # noqa: E402 +import build_demo_episodes as bde # noqa: E402 +import build_correction_episodes as bce # noqa: E402 +import teach_state_rate as tsr # noqa: E402 + +_spec = importlib.util.spec_from_file_location( + "recorder_for_test", str(_HERE.parent / "dynamic_tracking_limits" / "place" / "place_modules" / "recorder.py")) +reco = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(reco) + +NOTE = rtd.CURRENT_THREAD_NOTE + + +class PoseDaemon(ttc.ArmDaemon): + """read_servo の角度を腕の今の角度にする (states.csv に書かれる角度が動きに沿うか見るため).""" + + def _handle(self, c): + resp = super()._handle(c) + if c.get("cmd") == "read_servo" and isinstance(resp, dict) and "angles" in (resp.get("items") or {}): + resp["items"]["angles"]["value"] = self.pose() + return resp + + +def replay(daemon, run_dir, sink=True): + """cmd_run と同じ順で再生を 1 回通す. 戻り値: (result, 通知, log_record, recorder).""" + sent = [] + vsp.close_daemon_socket() + log_record = {"current_angles_before_replay": list(rtd.HOME_ANGLES)} + seq = rtd.wpsr._build_segments(ttc.WAYPOINTS) + with mock.patch.object(vsp, "DAEMON_HOST", "127.0.0.1"), mock.patch.object(vsp, "DAEMON_PORT", daemon.port), \ + mock.patch.object(rtd.rpp, "recorder_notify", lambda p: sent.append(dict(p))), \ + mock.patch("sys.stdout", new=io.StringIO()): + recorder = rtd._Recorder(True, run_dir.name) + if sink: + recorder.start_current_actuals() + recorder.currents = rtd._start_currents( + run_dir, angle_sink=recorder.actual_from_currents if sink else None) + result = rtd._replay(seq, recorder, log_record) + log_record["currents"] = recorder.currents.stop() + recorder.close_current_actuals() + vsp.close_daemon_socket() + return result, sent, log_record, recorder + + +def write_states(runs_root, run_stem, sent): + """recorder.py の TrialWriter で states.csv を書く (本物の書き手. 届いた順に書く).""" + with mock.patch.object(reco, "RUNS_ROOT", runs_root), mock.patch("sys.stdout", new=io.StringIO()): + tw = reco.TrialWriter(run_stem, 64, 48) + for p in sent: + if p["type"] in ("stage", "commanded", "gripper", "actual", "gripper_cmd"): + tw.write_state(p["type"], p) + tw.close("ok") + return runs_root / run_stem / "recording" / "states.csv" + + +def is_cur(p): + return p.get("type") == "actual" and p.get("note") == NOTE + + +def main_kinds(sent): + return [(p["type"], p.get("stage"), p.get("state"), p.get("note")) for p in sent if not is_cur(p)] + + +class TestReplayCurrentActuals(unittest.TestCase): + """再生を 2 回 (角度を渡す / 渡さない. どちらも電流は記録する) 通して比べる.""" + + @classmethod + def setUpClass(cls): + cls.td = Path(tempfile.mkdtemp()) + pr.install_daemon_lock() + cls.d_on, cls.d_off = PoseDaemon(), PoseDaemon() + try: + cls.on = replay(cls.d_on, cls.td / "run_0001_20991115_000000", sink=True) + cls.off = replay(cls.d_off, cls.td / "run_0002_20991115_000000", sink=False) + finally: + cls.d_on.close() + cls.d_off.close() + cls.rec = cls.td / "run_0001_20991115_000000" / "recording" + cls.states_root = cls.td / "states" + cls.st_on = write_states(cls.states_root, "run_0001_20991115_000000", cls.on[1]) + cls.st_off = write_states(cls.states_root, "run_0002_20991115_000000", cls.off[1]) + + def test_completed(self): + self.assertEqual((self.on[0], self.off[0]), ("completed", "completed")) + + def test_current_rows_sent(self): + cur = [p for p in self.on[1] if is_cur(p)] + rec = self.on[3] + self.assertGreater(len(cur), 30) + self.assertEqual(len(cur), rec.actual_sent_current_thread) + self.assertEqual(rec.actual_current_thread_errors, 0) + self.assertFalse(any(is_cur(p) for p in self.off[1])) + # 時刻は currents.csv の ts (角度を読んだ時刻) と同じ. 送らなかった分だけ少ない + cts = [r[0] for r in ttc.read_csv(self.rec / "currents.csv")[1:]] + got = [f"{p['ts']:.6f}" for p in cur] + self.assertTrue(set(got) <= set(cts)) + self.assertEqual(len(cts) - len(got), rec.actual_current_thread_late) + self.assertEqual(rec.actual_sent, self.off[3].actual_sent) # 本体の actual の数 (試行 JSON) は今までの意味 + + def test_main_unchanged(self): + """本体の通知・本体の daemon への命令・到達と静止の確かめは角度を渡さない再生と同じ.""" + self.assertEqual(main_kinds(self.on[1]), main_kinds(self.off[1])) + cmds = lambda d: [(s[0], s[3]) for s in d.spans if s[0] in # noqa: E731 + ("send_angles", "set_gripper", "set_gripper_value", "set_fresh_mode")] + self.assertEqual(cmds(self.d_on), cmds(self.d_off)) + n_get = lambda d: sum(1 for s in d.spans if s[0] == "get_angles") # noqa: E731 + self.assertEqual(n_get(self.d_on), n_get(self.d_off)) # 本体の読みの回数も同じ + + def summary(lr): + return [(g.get("kind"), g.get("arrival_ok"), g.get("retried"), tuple(g.get("warnings") or ()), + (g.get("still_check") or {}).get("still"), (g.get("still_check") or {}).get("reads")) + for g in lr["segments"]] + self.assertEqual(summary(self.on[2]), summary(self.off[2])) + self.assertEqual(self.on[2]["currents"]["overruns"], 0) + + def test_states_csv_order_and_note(self): + with self.st_on.open() as f: + rows = list(csv.DictReader(f)) + self.assertEqual(list(rows[0].keys()), reco.STATES_COLS) # 列は変えない + ts = [float(r["ts"]) for r in rows] + self.assertEqual(ts, sorted(ts)) # 時刻順 + acts = [r for r in rows if r["event_type"] == "actual"] + notes = {r["note"] for r in acts} + self.assertEqual(notes, {"", NOTE}) + self.assertTrue(all(r["j0"] != "" for r in acts)) + # 電流スレッドの行の stage は, その時刻に recorder.py が見ていた区間 (前の stage 行) と同じ + cur_stage = "" + for r in rows: + if r["event_type"] == "stage": + cur_stage = r["stage"] + self.assertEqual(r["stage"], cur_stage) + print("\nstates.csv の行の例 (直したあと):") + for r in rows[:12]: + print(" " + ",".join(r[c] for c in reco.STATES_COLS)) + + def test_no_current_rows_during_gripper(self): + cur_ts = [p["ts"] for p in self.on[1] if is_cur(p)] + for cmd, g0, nxt in ttc.gripper_windows(self.d_on.spans): + inside = [t for t in cur_ts if g0 <= t < nxt - 0.03] + self.assertEqual(inside, [], f"{cmd} の待ちの間に電流スレッドの actual 行") + + def test_angles_follow_arm(self): + """電流スレッドの角度は腕の動きに沿う (固定の値ではない).""" + cur = [p["angles"] for p in self.on[1] if is_cur(p)] + self.assertGreater(len({tuple(round(x, 1) for x in a) for a in cur}), 3) + + def test_readers(self): + at_on, aq_on, gt_on, _ = bde.load_states(self.st_on) + at_off, _, gt_off, _ = bde.load_states(self.st_off) + n_cur = sum(1 for p in self.on[1] if is_cur(p)) + self.assertEqual(len(at_on), len(at_off) + n_cur) + self.assertEqual(list(gt_on) == sorted(gt_on), True) + self.assertEqual(len(gt_on), len(gt_off)) + # 依頼 196 の state_source: 実測の間が狭くなるので, 埋めたコマ (0.1 秒おきのコマで数える) の割合は増えない + def filled_share(st): + at, aq, _, _ = bde.load_states(st) + t = bde.np.arange(at[0], at[-1], 0.1) + return float((bce.gap_fill_flags(t, at, aq) != 0).mean()) + self.assertLessEqual(filled_share(self.st_on), filled_share(self.st_off)) + + def test_rate_table(self): + """数えるスクリプト (teach_state_rate.py) で直す前・後を数える. ポーリングの間が狭くなる.""" + m_on, _ = tsr.measure(self.st_on.parent.parent) + m_off, _ = tsr.measure(self.st_off.parent.parent) + self.assertEqual(m_on["order_reversed"], 0) + self.assertEqual(m_on["n_main"], m_off["n_actual"]) + self.assertAlmostEqual(m_on["main_median"], m_off["all_median"], delta=0.03) # 本体の読みだけなら今までと同じ形 + self.assertLess(m_on["all_poll_median"], m_off["all_poll_median"]) + print(f"\n偽の daemon での actual の間 (send_angles {ttc.SEND_SEC} 秒): 直す前 中央値 {m_off['all_median']:.3f}" + f" 90% {m_off['all_p90']:.3f} poll {m_off['all_poll_median']:.3f} / 直したあと 中央値 {m_on['all_median']:.3f}" + f" 90% {m_on['all_p90']:.3f} poll {m_on['all_poll_median']:.3f} (電流スレッドの行 {m_on['n_current_thread']})") + + +class TestRecorderUnits(unittest.TestCase): + def setUp(self): + self.sent = [] + self.p = mock.patch.object(rtd.rpp, "recorder_notify", lambda p: self.sent.append(dict(p))) + self.p.start() + self.rec = rtd._Recorder(True, "run_x") + self.rec.start_current_actuals() + + def tearDown(self): + self.rec.close_current_actuals() + self.p.stop() + + def test_late_dropped(self): + self.rec.actual([0] * 6) + self.rec.actual_from_currents([1] * 6, self.rec._last_state_ts - 0.01) # 本体の行より前 → 積まない + self.rec.actual_from_currents([2] * 6, self.rec._last_state_ts + 0.01) + self.rec.close_current_actuals() + self.assertEqual([p.get("note") for p in self.sent], [None, NOTE]) + self.assertEqual((self.rec.actual_current_thread_late, self.rec.actual_sent_current_thread), (1, 1)) + self.assertEqual(self.rec.actual_sent, 1) + + def test_stage_counts_as_previous_row(self): + self.rec.stage("PICK") + self.rec.actual_from_currents([1] * 6, self.rec._last_state_ts - 0.001) + self.rec.close_current_actuals() + self.assertEqual([p["type"] for p in self.sent], ["stage"]) + + def test_gripper_ts_kept(self): + self.rec.gripper_cmd(1, "close", ts=123.0) + self.rec.close_current_actuals() + self.assertEqual(self.sent[-1]["ts"], 123.0) # 渡した時刻のまま (今までどおり) + + def test_closed(self): + self.rec.close_current_actuals() + self.rec.actual_from_currents([1] * 6, time.time()) + self.assertEqual(self.sent, []) + self.assertEqual(self.rec.actual_sent_current_thread, 0) + self.rec.stage("X") # 閉じたあとは今までどおりその場で送る + self.assertEqual([p["type"] for p in self.sent], ["stage"]) + + def test_bad_values(self): + self.rec.actual_from_currents(["x"] * 6, time.time()) + self.rec.actual_from_currents([float("nan")] * 6, time.time()) + self.rec.actual_from_currents([1] * 5, time.time()) + self.rec.close_current_actuals() + self.assertEqual((self.sent, self.rec.actual_current_thread_errors), ([], 3)) + + def test_disabled(self): + rec = rtd._Recorder(False, "run_x") + rec.start_current_actuals() + rec.actual_from_currents([1] * 6, time.time()) + rec.actual([1] * 6) + rec.close_current_actuals() + self.assertEqual(self.sent, []) + + def test_not_started_sends_inline(self): + """start_current_actuals を呼ばなければ今までどおりその場で送り, 電流スレッドの角度は受け付けない.""" + rec = rtd._Recorder(True, "run_y") + rec.actual([1] * 6) + rec.actual_from_currents([2] * 6, time.time() + 1) + self.assertEqual([p.get("note") for p in self.sent], [None]) + self.assertEqual(rec.actual_sent_current_thread, 0) + + def test_order_across_threads(self): + """本体と電流スレッドが同時に積んでも, 届く順は時刻順.""" + stop = time.time() + 0.5 + + def cur(): + while time.time() < stop: + self.rec.actual_from_currents([1] * 6, time.time()) + th = threading.Thread(target=cur) + th.start() + while time.time() < stop: + self.rec.actual([0] * 6) + self.rec.stage("S") + th.join() + self.rec.close_current_actuals() + ts = [p["ts"] for p in self.sent] + self.assertGreater(self.rec.actual_sent_current_thread, 10) + self.assertEqual(ts, sorted(ts)) + + def test_clock_backwards_counted(self): + self.rec.gripper_cmd(1, "close", ts=time.time() + 100) + self.rec.actual([0] * 6) # 前の行より前の時刻 → 送るが数える + self.rec.close_current_actuals() + self.assertEqual(len(self.sent), 2) + self.assertEqual(self.rec.state_ts_backwards, 1) + + +class TestSlowNotify(unittest.TestCase): + """codex R1 必須: 通知の送信が止まっても本体と電流スレッドは待たない. 閉じるのも上限まで. + codex 2・3 回目 R1 必須: 送り切れないまま閉じても, 積んだ行は trial_end を追い越さない (trial_end も列の最後に積む).""" + + def _stuck(self): + self.gate = threading.Event() + self.sent = [] + + def slow(p): + if p.get("type") != "trial_end": + self.gate.wait(5.0) + self.sent.append(dict(p)) + return mock.patch.object(rtd.rpp, "recorder_notify", slow) + + def test_stuck_send(self): + with self._stuck(): + rec = rtd._Recorder(True, "run_z") + rec.start_current_actuals() + t0 = time.time() + for _ in range(20): + rec.actual([0] * 6) + rec.stage("S") + rec.actual_from_currents([1] * 6, time.time()) + self.assertLess(time.time() - t0, 0.2) # 送信が止まっていても積むだけで戻る + t1 = time.time() + rec.close_current_actuals(timeout=0.3) + rec.trial_end() + self.assertLess(time.time() - t1, 1.0) # 閉じるのも trial_end も上限まで (本体は待たない) + self.assertTrue(rec.current_actuals_flush_timeout) + self.assertEqual(rec.current_actuals_unsent, 60) # 期限の時点でまだの数 (途中の 1 件を含む) + self.assertTrue(rec.trial_end_via_sender) + self.gate.set() # 送信が戻ると, 積んだ順に送り切る + for _ in range(100): + if self.sent and self.sent[-1]["type"] == "trial_end": + break + time.sleep(0.02) + types = [p["type"] for p in self.sent] + self.assertEqual(len(types), 61) + self.assertEqual(types[-1], "trial_end") # trial_end を追い越さない + ts = [p["ts"] for p in self.sent[:-1]] + self.assertEqual(ts, sorted(ts)) + self.assertEqual(rec.state_rows_sent_by_sender, 61) + + def test_normal_counts(self): + sent = [] + with mock.patch.object(rtd.rpp, "recorder_notify", lambda p: sent.append(dict(p))): + rec = rtd._Recorder(True, "run_v") + rec.start_current_actuals() + rec.stage("S") + rec.actual_from_currents([1] * 6, time.time()) + rec.close_current_actuals() + rec.trial_end() + self.assertEqual([p["type"] for p in sent], ["stage", "actual", "trial_end"]) + self.assertEqual((rec.state_rows_sent_by_sender, rec.current_actuals_unsent, + rec.current_actuals_flush_timeout, rec.trial_end_via_sender), (2, 0, False, False)) + + +class TestCurrentsRecorderSink(unittest.TestCase): + def _run(self, sink, daemon): + td = Path(tempfile.mkdtemp()) + with mock.patch("sys.stdout", new=io.StringIO()): + cur = pr.CurrentsRecorder(td, lambda c: daemon._handle(c)) + cur.angle_sink = sink + cur.start() + t_end = time.time() + 1.0 + while time.time() < t_end: + cur.window(0.3) + time.sleep(0.3) + meta = cur.stop() + return cur, meta + + def test_sink_error_keeps_reading(self): + d = trc.FakeDaemon() + calls = [] + + def bad(a, ts): + calls.append(ts) + raise RuntimeError("x") + cur, meta = self._run(bad, d) + try: + self.assertGreaterEqual(len(calls), 2) + self.assertEqual(meta["loop_errors"], len(calls)) + self.assertEqual(meta["currents_rows"], len(calls)) # 電流の行は今までどおり書く + finally: + d.close() + + def test_no_angles_not_sent(self): + d = trc.FakeDaemon() + orig = d._handle + + def no_angles(c): + r = orig(c) + if c.get("cmd") == "read_servo": + r["items"] = {} + return r + d._handle = no_angles + calls = [] + cur, meta = self._run(lambda a, ts: calls.append(ts), d) + try: + self.assertEqual(calls, []) + self.assertGreaterEqual(meta["currents_rows"], 2) + finally: + d.close() + + +if __name__ == "__main__": + unittest.main() diff --git a/pi05/test_teach_currents.py b/pi05/test_teach_currents.py index f639383..2793fd9 100644 --- a/pi05/test_teach_currents.py +++ b/pi05/test_teach_currents.py @@ -366,10 +366,16 @@ class TestPauseAndOldDaemon(unittest.TestCase): """cmd_run: settle のあとに始め, trial_end より先に止めて試行 JSON に残す (記録するときだけ).""" src = Path(rtd.__file__).read_text() i_settle = src.index("time.sleep(RECORDER_START_SETTLE_SEC)") - i_start = src.index("recorder.currents = _start_currents(run_dir)") + i_start = src.index("recorder.currents = _start_currents(\n run_dir, angle_sink=") # 依頼 202 i_replay = src.index("result = _replay(seq, recorder, log_record)") i_stop = src.index('log_record["currents"] = recorder.currents.stop()') i_end = src.index("recorder.trial_end()\n", i_stop) + i_open = src.index("recorder.start_current_actuals()") # 依頼 202: 角度を受け付けてから始める + i_close = src.index("recorder.close_current_actuals()", i_stop) # 依頼 202: 止めてから閉じ, trial_end の前 + self.assertLess(i_settle, i_open) + self.assertLess(i_open, i_start) + self.assertLess(i_stop, i_close) + self.assertLess(i_close, i_end) self.assertLess(i_settle, i_start) self.assertLess(i_start, i_replay) self.assertLess(i_replay, i_stop) @@ -525,8 +531,9 @@ class TestFailurePaths(unittest.TestCase): def test_no_currents_option(self): src = Path(rtd.__file__).read_text() self.assertIn('ap_r.add_argument("--no-currents"', src) - self.assertIn('if not getattr(args, "no_currents", False):\n recorder.currents = ' - '_start_currents(run_dir)', src) + self.assertIn('if not getattr(args, "no_currents", False):\n' + ' recorder.start_current_actuals() # 依頼 202\n' + ' recorder.currents = _start_currents(', src) def test_stop_flush_hang(self): """スレッドが終わっていても, ファイルの flush が止まると stop() は CLOSE_LIMIT_SEC ほどで戻る (codex R2 必須 1)."""