commit 4a017f3dd03178805c307ac6cf72ec54cac773e7 Author: Eiichi Shimazu Date: Sun Nov 15 18:47:59 2026 +0900 手教示の再生で送信のあとの 1.0 秒を Mac 側で待つ形 (依頼 213・依頼 197 の案 2・Log37) のテストと集計の列を足す record_teach_demo.py (git の外) は daemon へ wait=0・期限 4.0 秒で送り, 返ってから Mac 側で SEND_WAIT_SEC (1.0 秒) 眠る (--send-wait-on mac が既定. daemon は依頼 202 までと同じ). 待つ長さは変えない. その間は電流の記録スレッドが read_servo で読む. - test_send_wait_mac.py: wait を本物の daemon どおり扱う偽の daemon で mac / daemon を比べる (25 件) - teach_state_rate.py: 本ごとの「待ち」(send_wait_on) と区間ごとの daemon だけの往復を出す - test_teach_currents.py / test_teach_current_actuals.py: 偽の daemon が wait を見ないので daemon 側の待ちに固定 pi05 のテスト 27 本 OK (test_state_source を除く)・test_lab_start.sh OK 237. codex 3 回で必須なし. 実機は動かしていない. Co-Authored-By: Claude Opus 5.5 diff --git a/pi05/teach_state_rate.py b/pi05/teach_state_rate.py index 5020526..1e02685 100644 --- a/pi05/teach_state_rate.py +++ b/pi05/teach_state_rate.py @@ -138,6 +138,8 @@ def measure(d): out["json_error"] = f"{type(e).__name__}" cur = j.get("currents") or {} out.update({"result": j.get("result"), "overruns": cur.get("overruns"), + # 依頼 213: 送信のあとの 1.0 秒をどこで待ったか (mac / daemon. 依頼 213 より前の本は項目がないので -) + "send_wait_on": j.get("send_wait_on"), "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")}) @@ -147,6 +149,7 @@ def measure(d): 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"), + "send_daemon_rtt_sec": g.get("send_daemon_rtt_sec"), # 依頼 213: daemon とのやりとりだけ "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")}) @@ -167,7 +170,7 @@ RUN_COLS = [("run", "run"), ("n_actual", "actual"), ("n_current_thread", "うち ("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", "結果")] + ("still_reads_total", "静止reads"), ("retried_n", "再送"), ("send_wait_on", "待ち"), ("result", "結果")] def print_group(label, ms): @@ -180,12 +183,12 @@ def print_group(label, ms): 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",))) + f"{h} {fmt(med(k))}" for k, h in RUN_COLS[1:] if k not in ("result", "send_wait_on"))) def print_segments(groups): """区間ごと (step 順) に, 群ごとの中央値を並べる. 区間の並びが群で違えば step の名前で分かる.""" - print("\n## 区間ごと (群ごとの中央値: 送信の往復 / 区間の長さ / 静止の確かめの reads)") + print("\n## 区間ごと (群ごとの中央値: 送信の往復 (待ちを含む) / daemon だけの往復 / 区間の長さ / 静止の確かめの reads)") keys = [] for _, segs in groups: for s in segs: @@ -200,7 +203,8 @@ def print_segments(groups): 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'))}" + cells.append(f"{label}: n={len(ss)} rtt {fmt(med('send_rtt_sec'))} daemon {fmt(med('send_daemon_rtt_sec'))}" + f" 長さ {fmt(med('segment_elapsed_sec'))}" f" reads {fmt(med('still_reads'))}") print(f" step {k[0]} {k[1]} {k[2]} " + " / ".join(cells)) diff --git a/pi05/test_send_wait_mac.py b/pi05/test_send_wait_mac.py new file mode 100644 index 0000000..5ea86dd --- /dev/null +++ b/pi05/test_send_wait_mac.py @@ -0,0 +1,464 @@ +#!/usr/bin/env python3 +"""test_send_wait_mac.py — 手教示の再生で, 送信のあとの 1.0 秒の待ちを daemon から Mac 側へ移し, その間も +電流の記録スレッドが角度を読む形 (依頼 213. 依頼 197 の案 2) を偽の daemon で確かめる (アームなし・Pi なし. 2026-11-15). + + python3 pi05/test_send_wait_mac.py # 全部 (再生を 2 回通すので 1 分半ほど) + python3 pi05/test_send_wait_mac.py -v + +偽の daemon は test_teach_current_actuals.PoseDaemon (read_servo の角度が腕の今の角度) で, send_angles だけ +本物の tools/pi/mecharm_daemon.py と同じ形にする: 書き込み → wait 秒眠る → 角度を 1 回読む → 返事. + +確かめること: + - --send-wait-on mac (既定): daemon へは wait=0・ソケットの期限 4.0 秒 (wait+3.0 と同じ) で送り, 返ってから + Mac 側で 1.0 秒待つ. この間に電流の記録スレッドが read_servo で読み, states.csv の actual 行になる + - 到達の確かめ (本体の最初の get_angles) が始まる時刻は送信の始めから 1.0 秒 + 書き込みと読み 1 回 のあと + (daemon で待つときと同じ. 縮んでいない) + - 本体が daemon に送る命令 (関節の目標・速さ・爪) と順, 本体の読みの回数, 到達・再送・静止の確かめは daemon で + 待つときと同じ (違うのは send_angles の wait だけ) + - 送信と読みは同じ鍵で順に出る (送信の途中に read_servo が割り込まない. 鍵の中の時刻で重ならない) + - 爪の命令から待ちの終わりまでは今までどおり読まない + - --send-wait-on daemon は依頼 202 までと同じ (wait=1.0 を daemon へ. Mac 側で待たない. その間は読まない) + - 区間と試行 JSON に send_wait_on・send_daemon_rtt_sec が残る. teach_state_rate.py が待ちの列を出す + - _send_angles_guarded: 送信が例外でも 1.0 秒待ってから例外を返す・Ctrl-C は待たない・姿勢検査で弾いたら送らず待たない + - コマンドの --send-wait-on の既定は mac・ほかの値は受け付けない + - (codex R1) 窓の終わりぎわの read_servo が長引くと本体の次の読みはその分遅れる (早まらない・overruns に数える) + - (codex R3) 送信が 4.0 秒で返らなければ切れて, 1.0 秒待ってから例外. 次の命令はつなぎ直して通る +KIOXIA にもアームにも触らない (一時フォルダと 127.0.0.1 だけ). +""" +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 +import test_recorder_currents as trc # noqa: E402 +import test_teach_current_actuals as tca # noqa: E402 (PoseDaemon・write_states) +import policy_recorder as pr # noqa: E402 +import visual_servo_pick as vsp # noqa: E402 +import record_teach_demo as rtd # noqa: E402 +import teach_state_rate as tsr # noqa: E402 + +WRITE_SEC = 0.010 # 本物の _mc.send_angles() の書き込み (実測 約 10 ms) +READ_ONCE_SEC = trc.OVERHEAD_SEC + trc.ANGLES_SEC # 送ったあとの read_angles_once_for_log 1 回 + + +class WaitDaemon(tca.PoseDaemon): + """send_angles を本物の daemon と同じ形にする: 書き込み → wait 秒眠る → 角度を 1 回読む → 返事.""" + + def _handle(self, c): + if c.get("cmd") != "send_angles": + return super()._handle(c) + t0 = time.time() + self.cmds.append(c) + n = self.n_send + self.n_send += 1 + time.sleep(WRITE_SEC) + if n not in self.ignore_sends: + start, goal = self.pose(), [float(x) for x in c["angles"]] + self.cur = goal + self.move = (start, goal, time.time(), + max(abs(x - y) for x, y in zip(start, goal)) / ttc.ARM_DEG_PER_SEC) + wait = float(c.get("wait", 0)) + if wait > 0: + time.sleep(wait) + time.sleep(READ_ONCE_SEC) + with self.span_lock: + self.spans.append(("send_angles", t0, time.time(), dict(c))) + return {"ok": True, "result": 1, "elapsed_ms": WRITE_SEC * 1000.0} + + +def replay(mode, run_dir, ignore_sends=()): + """cmd_run と同じ順で再生を 1 回通す. 戻り値: dict (result・通知・log_record・daemon・鍵の中の時刻).""" + d = WaitDaemon(ignore_sends=ignore_sends) + rpcs = [] + + def obs(cmd_obj, resp, t0, t1, err): + rpcs.append(((cmd_obj or {}).get("cmd"), t0, t1, threading.current_thread().name)) + pr.add_rpc_observer(obs) + try: + result, sent, log_record, recorder = tca.replay(d, run_dir, sink=True, send_wait_on=mode) + finally: + pr.remove_rpc_observer(obs) + d.close() + return {"result": result, "sent": sent, "lr": log_record, "rec": recorder, "d": d, "rpcs": rpcs} + + +def sends(d): + return [s for s in d.spans if s[0] == "send_angles"] + + +def next_main_get(d, after): + """after より後で最初の本体の get_angles の始め (記録スレッドは get_angles を使わない).""" + return min(s[1] for s in d.spans if s[0] == "get_angles" and s[1] >= after) + + +class TestReplaySendWait(unittest.TestCase): + """再生を 2 回 (mac / daemon. どちらも電流を記録し角度を states.csv に渡す) 通して比べる.""" + + @classmethod + def setUpClass(cls): + cls.td = Path(tempfile.mkdtemp()) + pr.install_daemon_lock() + cls.mac = replay("mac", cls.td / "run_0001_20991115_000001") + cls.dmn = replay("daemon", cls.td / "run_0002_20991115_000002") + root = cls.td / "states" + cls.st_mac = tca.write_states(root, "run_0001_20991115_000001", cls.mac["sent"]) + cls.st_dmn = tca.write_states(root, "run_0002_20991115_000002", cls.dmn["sent"]) + + def test_completed(self): + self.assertEqual((self.mac["result"], self.dmn["result"]), ("completed", "completed")) + + def test_wait_sent_to_daemon(self): + """mac は wait=0, daemon は wait=1.0 を送る. ほかの中身 (目標・速さ) と順は同じ.""" + wm = {s[3].get("wait") for s in sends(self.mac["d"])} + wd = {s[3].get("wait") for s in sends(self.dmn["d"])} + self.assertEqual((wm, wd), ({0}, {rtd.SEND_WAIT_SEC})) + strip = lambda d: [(s[0], {k: v for k, v in s[3].items() if k != "wait"}) # noqa: E731 + for s in d.spans if s[0] in ("send_angles", "set_gripper", "set_gripper_value", + "set_fresh_mode")] + self.assertEqual(strip(self.mac["d"]), strip(self.dmn["d"])) + + def test_socket_timeout_kept(self): + """mac の wait=0 でもソケットの期限は daemon で待つときと同じ 4.0 秒 (既定の 35 秒にしない).""" + seen = [] + orig = vsp._daemon_request + + def spy(cmd_obj, timeout_override=None, **k): + seen.append((cmd_obj.get("wait"), timeout_override)) + return {"ok": True, "elapsed_ms": 1.0} + with mock.patch.object(vsp, "_daemon_request", spy), mock.patch.object(rtd, "SEND_WAIT_ON", "mac"), \ + mock.patch.object(rtd.time, "sleep", lambda s: None): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t") + self.assertIs(vsp._daemon_request, orig) + self.assertEqual(seen, [(0, rtd.SEND_WAIT_SEC + 3.0)]) + + def test_arrival_start_not_earlier(self): + """本体の到達の確かめが始まるのは, どちらも送信の始めから 1.0 秒 + 書き込み + 読み 1 回 のあと.""" + lo = rtd.SEND_WAIT_SEC + WRITE_SEC + READ_ONCE_SEC + delays = {} + for name, r in (("mac", self.mac), ("daemon", self.dmn)): + ds = [] + for s in sends(r["d"]): + ds.append(next_main_get(r["d"], s[2]) - s[1]) + delays[name] = ds + self.assertGreaterEqual(min(ds), lo - 0.002, name) + # mac は daemon と同じ (送信の返事を待つ時間と, 眠りの終わりから起きるまでの時間の分だけずれる) + for m, d in zip(delays["mac"], delays["daemon"]): + self.assertLess(abs(m - d), 0.08) + print(f"\n送信の始め → 到達の確かめの始め (秒): mac {[round(x, 3) for x in delays['mac']]}" + f" / daemon {[round(x, 3) for x in delays['daemon']]}") + + def test_mac_reads_during_wait(self): + """mac: 送信の返事のあとの 1.0 秒に read_servo が入る. daemon: 送信の間に read_servo は入らない.""" + per = [] + for s in sends(self.mac["d"]): + end = s[2] + n = sum(1 for x in self.mac["d"].spans + if x[0] == "read_servo" and end <= x[1] and x[2] <= end + rtd.SEND_WAIT_SEC + 0.05) + per.append(n) + self.assertTrue(all(n >= 5 for n in per), per) + for s in sends(self.dmn["d"]): + inside = [x for x in self.dmn["d"].spans if x[0] == "read_servo" and s[1] <= x[1] < s[2]] + self.assertEqual(inside, []) + rs = sorted(x[2] - x[1] for x in self.mac["d"].spans if x[0] == "read_servo") + print(f"\n送信のあとの 1.0 秒に読めた read_servo の回数 (偽の daemon. 1 回 中央値 {rs[len(rs) // 2] * 1000:.0f} ms):" + f" {per} 中央値 {sorted(per)[len(per) // 2]}") + + def test_lock_serializes_send_and_read(self): + """送信と read_servo は同じ鍵の中で 1 件ずつ (鍵の中の時刻が重ならない). daemon 側の時刻でも重ならない.""" + for r in (self.mac, self.dmn): + ivs = sorted((t0, t1, c) for c, t0, t1, _ in r["rpcs"] if c in ("send_angles", "read_servo", "get_angles")) + self.assertGreater(len(ivs), 50) + for (a0, a1, ca), (b0, b1, cb) in zip(ivs, ivs[1:]): + self.assertLessEqual(a1, b0 + 1e-6, f"{ca} と {cb} が重なる") + threads = {(c, th) for c, _, _, th in r["rpcs"] if c == "read_servo"} + self.assertTrue(threads and all(th == "record_currents" for _, th in threads)) + sp = sorted(r["d"].spans, key=lambda s: s[1]) + for a, b in zip(sp, sp[1:]): + self.assertLessEqual(a[2], b[1] + 1e-6) + + def test_main_unchanged(self): + """本体の読みの回数・到達と静止の確かめの結果・本体の通知の並びは daemon で待つときと同じ.""" + n_get = lambda r: sum(1 for s in r["d"].spans if s[0] == "get_angles") # noqa: E731 + # 到達の確かめの回数は腕が着くまでの実時間で決まるので, 別々に流した 2 回の再生では混んだ Mac で 1 回ずれる + # ことがある (test_teach_current_actuals の daemon どうしでも起きた). 読み方は同じなので差は 1 回までとする + self.assertLessEqual(abs(n_get(self.mac) - n_get(self.dmn)), 1) + + 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.mac["lr"]), summary(self.dmn["lr"])) + no_act = lambda r: [k for k in tca.main_kinds(r["sent"]) if k[0] != "actual"] # noqa: E731 + self.assertEqual(no_act(self.mac), no_act(self.dmn)) # stage・爪の通知の並びと中身は同じ + n_act = lambda r: sum(1 for k in tca.main_kinds(r["sent"]) if k[0] == "actual") # noqa: E731 + self.assertLessEqual(abs(n_act(self.mac) - n_act(self.dmn)), 1) # 本体の actual (= 本体の読み) は上と同じ扱い + self.assertEqual(self.mac["lr"]["currents"]["overruns"], 0) + + def test_no_reads_during_gripper(self): + for r in (self.mac, self.dmn): + for cmd, g0, nxt in ttc.gripper_windows(r["d"].spans): + inside = [x for x in r["d"].spans if x[0] == "read_servo" and g0 <= x[1] < nxt - 0.03] + self.assertEqual(inside, [], f"{cmd} の待ちの間に read_servo") + + def test_segment_records(self): + for r, mode in ((self.mac, "mac"), (self.dmn, "daemon")): + moves = [g for g in r["lr"]["segments"] if g.get("kind") == "move" or "send_rtt_sec" in g] + self.assertGreater(len(moves), 3) + for g in moves: + self.assertEqual(g["send_wait_on"], mode) + self.assertIsNotNone(g["send_daemon_rtt_sec"]) + # send_rtt_sec はどちらでも 1.0 秒の待ちを含む + self.assertGreaterEqual(g["send_rtt_sec"], rtd.SEND_WAIT_SEC) + if mode == "mac": + self.assertLess(g["send_daemon_rtt_sec"], 0.5) + else: + self.assertGreaterEqual(g["send_daemon_rtt_sec"], rtd.SEND_WAIT_SEC) + + def test_rate_table(self): + """teach_state_rate.py: mac は送信の間の抜けが埋まり 1 秒超の抜けが減る. 待ちの列は試行 JSON から読む.""" + m_mac, segs = tsr.measure(self.st_mac.parent.parent) + m_dmn, _ = tsr.measure(self.st_dmn.parent.parent) + self.assertEqual(m_mac["order_reversed"], 0) + self.assertLess(m_mac["all_long_n"], m_dmn["all_long_n"]) + self.assertLess(m_mac["all_p90"], m_dmn["all_p90"]) + self.assertGreater(m_mac["n_current_thread"], m_dmn["n_current_thread"]) + print(f"\n偽の daemon での actual の間: daemon で待つ 中央値 {m_dmn['all_median']:.3f} 90% {m_dmn['all_p90']:.3f}" + f" 1秒超 {m_dmn['all_long_n']} / mac で待つ 中央値 {m_mac['all_median']:.3f} 90% {m_mac['all_p90']:.3f}" + f" 1秒超 {m_mac['all_long_n']} (電流スレッドの行 {m_dmn['n_current_thread']} → {m_mac['n_current_thread']})") + + def test_rate_table_send_wait_column(self): + td = Path(tempfile.mkdtemp()) + run = td / "run_0009_20991115_000009" + (run / "recording").mkdir(parents=True) + (run / "recording" / "states.csv").write_text(self.st_mac.read_text()) + import json + (run / f"{run.name}.json").write_text(json.dumps({"send_wait_on": "mac", "segments": [ + {"step": 1, "kind": "move", "to_name": "HOME", "send_rtt_sec": 1.05, "send_daemon_rtt_sec": 0.03}]})) + m, segs = tsr.measure(run) + self.assertEqual(m["send_wait_on"], "mac") + self.assertEqual(segs[0]["send_daemon_rtt_sec"], 0.03) + with mock.patch("sys.stdout", new=io.StringIO()) as out: + tsr.print_group("後", [m]) + tsr.print_segments([("後", segs)]) + self.assertIn("待ち", out.getvalue()) + self.assertIn("daemon 0.030", out.getvalue()) + + +class TestSendRetry(unittest.TestCase): + """送信無視 → 5 秒待って再送の経路も mac で通る (再送も Mac 側で 1.0 秒待つ).""" + + def test_retry_path(self): + pr.install_daemon_lock() + r = replay("mac", Path(tempfile.mkdtemp()) / "run_0003_20991115_000003", ignore_sends=(1,)) + self.assertEqual(r["result"], "completed") + seg = [g for g in r["lr"]["segments"] if g.get("retried")] + self.assertEqual(len(seg), 1) + self.assertIsNotNone(seg[0]["send_daemon_rtt_retry_sec"]) + self.assertGreaterEqual(seg[0]["send_rtt_retry_sec"], rtd.SEND_WAIT_SEC) + self.assertEqual({s[3].get("wait") for s in sends(r["d"])}, {0}) + + +class SlowTailDaemon(WaitDaemon): + """送信の返事から slow_after 秒より後に始まる read_servo を slow_sec 秒遅らせる (窓の終わりぎわの読みが長引く場合).""" + + def __init__(self, slow_after=0.7, slow_sec=0.5, send_extra=0.0): + self.slow_after, self.slow_sec, self.send_extra = slow_after, slow_sec, send_extra + self.send_end = None + super().__init__() + + def _handle(self, c): + if c.get("cmd") == "send_angles": + time.sleep(self.send_extra) + resp = super()._handle(c) + self.send_end = time.time() + return resp + if c.get("cmd") == "read_servo" and self.send_end and time.time() - self.send_end > self.slow_after: + time.sleep(self.slow_sec) + return super()._handle(c) + + +class TestSlowAndTimeout(unittest.TestCase): + """codex R1・R3: 窓の終わりで読みが長引いた場合と, 送信のソケットの期限切れを本物の通信 (127.0.0.1) で通す.""" + + def _with_daemon(self, d, fn): + vsp.close_daemon_socket() + try: + with mock.patch.object(vsp, "DAEMON_HOST", "127.0.0.1"), mock.patch.object(vsp, "DAEMON_PORT", d.port), \ + mock.patch.object(rtd, "SEND_WAIT_ON", "mac"), mock.patch("sys.stdout", new=io.StringIO()): + return fn() + finally: + vsp.close_daemon_socket() + d.close() + + def test_slow_read_at_window_end(self): + """窓の終わりぎわに始めた read_servo が 0.5 秒長引くと, 本体の次の読みはその分だけ遅れる (早まらない). + 遅れは read_servo 1 回の上限 (READ_SERVO_TIMEOUT_SEC) までで, overruns に数える. 1.0 秒の待ちそのものは縮まない. + 読み方 (依頼 121 の窓の決まり) は変えていないので, 0.3 秒の到達の確かめの合間でも前から同じことが起こりうる.""" + pr.install_daemon_lock() + d = SlowTailDaemon(slow_after=0.7, slow_sec=0.5) + td = Path(tempfile.mkdtemp()) + + def go(): + rec = rtd._Recorder(False, "run_s") + rec.currents = rtd._start_currents(td) + time.sleep(0.2) + t0 = time.time() + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t", recorder=rec) + t_back = time.time() + vsp.daemon_get_angles() + meta = rec.currents.stop() + rtd._ACTIVE_CURRENTS.remove(rec.currents) + return t0, t_back, meta + t0, t_back, meta = self._with_daemon(d, go) + get0 = next_main_get(d, t0) + self.assertGreaterEqual(t_back - t0, rtd.SEND_WAIT_SEC) # Mac 側の待ちは縮まない + self.assertGreaterEqual(get0 - t0, rtd.SEND_WAIT_SEC + WRITE_SEC) # 次の読みは早まらない + self.assertGreaterEqual(meta["overruns"], 1) # 見込み超えとして数える + self.assertLess(get0 - t0, rtd.SEND_WAIT_SEC + 0.1 + pr.READ_SERVO_TIMEOUT_SEC) + print(f"\n窓の終わりで read_servo が 0.5 秒長引いた場合: 送信の始め → 本体の次の読み {get0 - t0:.3f} 秒" + f" (長引かなければ 約 {rtd.SEND_WAIT_SEC + WRITE_SEC + READ_ONCE_SEC:.3f} 秒) overruns {meta['overruns']}") + + def test_send_socket_timeout(self): + """daemon が 4.0 秒より長く返さなければ, 4.0 秒で切れて例外. そのあとも 1.0 秒待ってから例外を返す. + 次の命令はつなぎ直して通る.""" + d = SlowTailDaemon(send_extra=rtd.SEND_SOCKET_TIMEOUT_SEC + 0.6) + + def go(): + t0 = time.monotonic() + with self.assertRaises(Exception): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t") + el = time.monotonic() - t0 + rtt = rtd._LAST_SEND["daemon_rtt_sec"] + time.sleep(1.0) # 偽の daemon が遅れた返事を書き終えるまで + a = vsp.daemon_get_angles() + return el, rtt, a + el, rtt, a = self._with_daemon(d, go) + self.assertAlmostEqual(rtt, rtd.SEND_SOCKET_TIMEOUT_SEC, delta=0.3) + self.assertGreaterEqual(el, rtd.SEND_SOCKET_TIMEOUT_SEC + rtd.SEND_WAIT_SEC - 0.05) + self.assertLess(el, rtd.SEND_SOCKET_TIMEOUT_SEC + rtd.SEND_WAIT_SEC + 0.5) + self.assertEqual(len(a), 6) + + +class _Rec(object): + def __init__(self): + self.windows = [] + + def currents_window(self, sec): + self.windows.append(sec) + + +class TestGuardedUnits(unittest.TestCase): + def setUp(self): + self.calls = [] + self.slept = [] + self.p = [mock.patch.object(rtd.time, "sleep", lambda s: self.slept.append(s)), + mock.patch.object(rtd, "daemon_send_angles", self._send)] + for p in self.p: + p.start() + self.exc = None + + def tearDown(self): + for p in self.p: + p.stop() + + def _send(self, pose, speed, wait=0, timeout_override=None): + self.calls.append((list(pose), speed, wait, timeout_override, list(self.slept))) + if self.exc is not None: + raise self.exc + return {"ok": True} + + def test_mac_waits_after_send(self): + rec = _Rec() + with mock.patch.object(rtd, "SEND_WAIT_ON", "mac"): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t", recorder=rec) + self.assertEqual(self.calls, [(list(rtd.HOME_ANGLES), 20, 0, rtd.SEND_SOCKET_TIMEOUT_SEC, [])]) # 送る前は眠らない + self.assertEqual(self.slept, [rtd.SEND_WAIT_SEC]) # 送ったあとに 1.0 秒 (縮めない) + self.assertEqual(rec.windows, [rtd.SEND_WAIT_SEC]) # その間を電流の記録に知らせる + self.assertIsNotNone(rtd._LAST_SEND["daemon_rtt_sec"]) + + def test_mac_without_recorder(self): + with mock.patch.object(rtd, "SEND_WAIT_ON", "mac"): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t") + self.assertEqual(self.slept, [rtd.SEND_WAIT_SEC]) + + def test_daemon_mode_unchanged(self): + rec = _Rec() + with mock.patch.object(rtd, "SEND_WAIT_ON", "daemon"): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t", recorder=rec) + self.assertEqual(self.calls, [(list(rtd.HOME_ANGLES), 20, rtd.SEND_WAIT_SEC, None, [])]) + self.assertEqual((self.slept, rec.windows), ([], [])) + + def test_mac_send_error_still_waits(self): + self.exc = RuntimeError("socket timeout") + rec = _Rec() + with mock.patch.object(rtd, "SEND_WAIT_ON", "mac"), self.assertRaises(RuntimeError): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t", recorder=rec) + self.assertEqual(self.slept, [rtd.SEND_WAIT_SEC]) + self.assertIsNotNone(rtd._LAST_SEND["daemon_rtt_sec"]) + + def test_daemon_send_error_keeps_rtt(self): + self.exc = RuntimeError("socket timeout") + with mock.patch.object(rtd, "SEND_WAIT_ON", "daemon"), self.assertRaises(RuntimeError): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t", recorder=_Rec()) + self.assertEqual(self.slept, []) + self.assertIsNotNone(rtd._LAST_SEND["daemon_rtt_sec"]) + + def test_mac_ctrl_c_not_waited(self): + self.exc = KeyboardInterrupt() + with mock.patch.object(rtd, "SEND_WAIT_ON", "mac"), self.assertRaises(KeyboardInterrupt): + rtd._send_angles_guarded(list(rtd.HOME_ANGLES), 20, "t", recorder=_Rec()) + self.assertEqual(self.slept, []) + + def test_backstop_no_send_no_wait(self): + bad = list(rtd.HOME_ANGLES) + bad[0] = float("nan") + with mock.patch.object(rtd, "SEND_WAIT_ON", "mac"), self.assertRaises(rtd._Aborted): + rtd._send_angles_guarded(bad, 20, "t", recorder=_Rec()) + self.assertEqual((self.calls, self.slept), ([], [])) + self.assertIsNone(rtd._LAST_SEND["daemon_rtt_sec"]) + + +class TestCli(unittest.TestCase): + def _parse(self, argv): + got = {} + with mock.patch.object(rtd, "cmd_run", lambda a: got.setdefault("a", a) and 0), \ + mock.patch.object(sys, "argv", ["record_teach_demo.py"] + argv): + rtd.main() + return got["a"] + + def test_default_mac(self): + self.assertEqual(self._parse(["run", "--teach", "x.json"]).send_wait_on, "mac") + self.assertEqual(self._parse(["run", "--teach", "x.json", "--send-wait-on", "daemon"]).send_wait_on, "daemon") + + def test_bad_value(self): + with mock.patch("sys.stderr", new=io.StringIO()), self.assertRaises(SystemExit): + self._parse(["run", "--teach", "x.json", "--send-wait-on", "pi"]) + + def test_trial_json_has_send_wait_on(self): + """cmd_run の試行 JSON (log_record) と params に send_wait_on を入れている (cmd_run を最後まで通すには + カメラと Pi が要るので, ソースで確かめる).""" + import inspect + src = inspect.getsource(rtd.cmd_run) + head = src[src.index("log_record = {"):] + self.assertIn('"send_wait_on": SEND_WAIT_ON,', head[:head.index('"current_angles_before_replay"')]) + self.assertEqual(src.count('"send_wait_on": SEND_WAIT_ON'), 2) # いちばん上と params + + def test_cmd_run_rejects_bad_value(self): + """cmd_run を直接呼ぶ形でも mac / daemon 以外は止める (グローバルは変えない).""" + a = mock.Mock(grip_value=None, power=None, send_wait_on="pi", no_currents=False, no_record=False) + with mock.patch("sys.stdout", new=io.StringIO()), self.assertRaises(SystemExit): + rtd.cmd_run(a) + self.assertEqual(rtd.SEND_WAIT_ON, "mac") + + +if __name__ == "__main__": + unittest.main() diff --git a/pi05/test_teach_current_actuals.py b/pi05/test_teach_current_actuals.py index 47540d1..d3f0e5c 100644 --- a/pi05/test_teach_current_actuals.py +++ b/pi05/test_teach_current_actuals.py @@ -65,14 +65,17 @@ class PoseDaemon(ttc.ArmDaemon): return resp -def replay(daemon, run_dir, sink=True): - """cmd_run と同じ順で再生を 1 回通す. 戻り値: (result, 通知, log_record, recorder).""" +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: diff --git a/pi05/test_teach_currents.py b/pi05/test_teach_currents.py index 2793fd9..d1af2eb 100644 --- a/pi05/test_teach_currents.py +++ b/pi05/test_teach_currents.py @@ -131,13 +131,16 @@ WAYPOINTS = { # HOME [0, 0, 0, 0, 0, -30] の近く. 関節の範 def replay(daemon, run_dir, currents=True): """cmd_run と同じ順 (install_daemon_lock → _start_currents → _replay → stop) で再生を 1 回通す. - recorder.py への通知は送らずに集める. 戻り値: (result, 通知, log_record, 再生にかかった秒).""" + recorder.py への通知は送らずに集める. 戻り値: (result, 通知, log_record, 再生にかかった秒). + 依頼 213: この偽の daemon は send_angles の wait を見ずに SEND_SEC (待ちを含む) で返すので, + 送信のあとの待ちは daemon 側 (--send-wait-on daemon) に固定する. Mac 側で待つ形は test_send_wait_mac.py.""" sent = [] vsp.close_daemon_socket() log_record = {"current_angles_before_replay": list(rtd.HOME_ANGLES)} seq = rtd.wpsr._build_segments(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", "daemon"), \ mock.patch("sys.stdout", new=io.StringIO()): recorder = rtd._Recorder(True, run_dir.name) if currents: