#!/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, send_wait_on="daemon"): """cmd_run と同じ順で再生を 1 回通す. 戻り値: (result, 通知, log_record, recorder). 依頼 213: ttc.ArmDaemon は send_angles の wait を見ずに SEND_SEC (待ちを含む) で返すので, 既定は daemon 側で待つ形. Mac 側で待つ形は test_send_wait_mac.py (wait を本物どおりに扱う偽の daemon) が send_wait_on="mac" で呼ぶ.""" 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.object(rtd, "SEND_WAIT_ON", send_wait_on), \ 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()