#!/usr/bin/env python3 """test_gamepad_teleop.py — gamepad_teleop.py の確かめ (アームなし・コントローラーなし. 依頼 68). python3 tools/test_gamepad_teleop.py -v 中身: - ボタン番号と操作の対応 (依頼 68 の表) / 十字キー上下の向き / △ で j1 → j5 → j1 - 守り: j1 は 90.87 を超えない / 物理可動域まで (基準の姿勢からの枠はない. 2026-11-03) / J4_SAFE - 長押し (2026-11-03): 短い押しは 1 刻み / 押している間は送り続ける / 離す・停止・△・範囲の端で止まる / 指令が読んだ角度より 3° 先なら送らずに待つ / 追いつかなければ止める - 停止: L2・R2 の両方 > 0.0 で止まる・片方では止まらない / 停止中は受け付けない / OPTIONS で戻る / 接続が切れたら停止 - HOME: 1 秒に満たない長押しでは送らない / 1 秒続けたら 1 回だけ送る - 動いている間の押しは受け付けない (ためない) - daemon への送り方: 停止が立っていれば送らない / 届かなくても送り直さない - ドライランで main() がひととおり動く (偽のコントローラー・ダミーの画面・一時フォルダ) - 依頼 69: ドライランは KIOXIA に書かない (フォルダも作らない・KIOXIA がなくても動く) / --execute は今までどおり書く / ドライランでは cv2 (2 つ目の libSDL2) を読まない・objc の重複の警告が出ない (別のプロセスで確かめる) - codex R1 の指摘: 範囲外の出発点 / 目標と読んだ角度のずれ / 長押しの持ち越し / 停止と送信の順番 / 再開で古い仕事が生き返らない / 読み取り失敗 / 記録の失敗 / ボタンより先に L2+R2 を見る KIOXIA にもアームにも触らない. """ import json import os import subprocess import sys import tempfile import threading import unittest from pathlib import Path from unittest import mock os.environ.setdefault("SDL_VIDEODRIVER", "dummy") os.environ.setdefault("SDL_AUDIODRIVER", "dummy") import numpy as np # noqa: E402 sys.path.insert(0, str(Path(__file__).resolve().parent)) import gamepad_teleop as gt # noqa: E402 # DaemonArm のテストで daemon の送り口 (の差し替え) を使うので読み込んでおく. # ドライランで読まないことは TestNoCv2InDryRun で別のプロセスで確かめる gt.load_daemon_modules() HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0] # テストは 0.5° の刻みで書いてある. 実際の値 (1°. 2026-11-03) は TestStepValue で確かめる STEP_FINE_REAL = gt.STEP_FINE_DEG gt.STEP_FINE_DEG = 0.5 class TestStepValue(unittest.TestCase): def test_real_steps(self): self.assertEqual(STEP_FINE_REAL, 1.0) self.assertEqual(gt.STEP_COARSE_DEG, 2.0) class ListLog: def __init__(self): self.rows = [] def write(self, rec): json.dumps(rec, ensure_ascii=False) # JSON にできること self.rows.append(rec) class FakeArm: execute = False def __init__(self, start=HOME, reach=True): self.q = np.asarray(start, dtype=float).copy() self.sent = [] self.grips = [] self.reach = reach def read(self): return self.q.copy() def send_and_check(self, target, gate, quitting, check=True): if not gate.run(lambda: self.sent.append([float(v) for v in target])): return {"sent": False, "why": "halted_before_send"} if not check: self.checks = getattr(self, "checks", []) + [False] self.q = np.asarray(target, dtype=float).copy() if self.reach else self.q return {"sent": True, "reached": None, "no_check": True} if self.reach: self.q = np.asarray(target, dtype=float).copy() return {"sent": True, "reached": self.reach, "reached_angles": gt._r(self.q), "reach_err": 0.0} def gripper(self, state, gate): return gate.run(lambda: self.grips.append(state)) def make(start=HOME, reach=True, sync=True): arm = FakeArm(start, reach) log = ListLog() spawned = [] spawn = (lambda fn: fn()) if sync else spawned.append tp = gt.Teleop(arm, start, log, out=lambda *a: None, spawn=spawn) return tp, arm, log, spawned def press(tp, b, now=0.0): tp.press(b, now) tp.release(b, now) tp.poll() class TestButtonMap(unittest.TestCase): def test_numbers(self): self.assertEqual((gt.BTN_CROSS, gt.BTN_CIRCLE, gt.BTN_SQUARE, gt.BTN_TRIANGLE), (0, 1, 2, 3)) self.assertEqual((gt.BTN_SHARE, gt.BTN_HOME, gt.BTN_OPTIONS, gt.BTN_L3, gt.BTN_R3), (4, 5, 6, 7, 8)) self.assertEqual((gt.BTN_L1, gt.BTN_R1), (9, 10)) self.assertEqual((gt.BTN_UP, gt.BTN_DOWN, gt.BTN_LEFT, gt.BTN_RIGHT, gt.BTN_TOUCHPAD), (11, 12, 13, 14, 15)) self.assertEqual((gt.AXIS_L2, gt.AXIS_R2), (4, 5)) def test_dpad_left_right_always_j0(self): for sel in range(5): tp, arm, log, _ = make() tp.sel = sel press(tp, gt.BTN_RIGHT) self.assertEqual(log.rows[-1]["joint"], 0) self.assertAlmostEqual(arm.sent[-1][0], 0.5) press(tp, gt.BTN_LEFT) press(tp, gt.BTN_LEFT) self.assertAlmostEqual(arm.sent[-1][0], -0.5) self.assertEqual(arm.sent[-1][1:], HOME[1:]) def test_up_minus_down_plus_starts_j1(self): tp, arm, log, _ = make() self.assertEqual(tp.joint, 1) press(tp, gt.BTN_DOWN) self.assertAlmostEqual(arm.sent[-1][1], 0.5) press(tp, gt.BTN_UP) press(tp, gt.BTN_UP) self.assertAlmostEqual(arm.sent[-1][1], -0.5) def test_triangle_cycles(self): tp, arm, log, _ = make() seen = [tp.joint] for _ in range(5): press(tp, gt.BTN_TRIANGLE) seen.append(tp.joint) self.assertEqual(seen, [1, 2, 3, 4, 5, 1]) self.assertEqual(arm.sent, []) # 切り替えだけでは送らない tp.sel = 3 # j4 press(tp, gt.BTN_DOWN) self.assertAlmostEqual(arm.sent[-1][4], 0.5) def test_r1_coarse_step(self): tp, arm, log, _ = make() tp.press(gt.BTN_R1, 0.0) press(tp, gt.BTN_RIGHT) self.assertAlmostEqual(arm.sent[-1][0], 2.0) press(tp, gt.BTN_DOWN) self.assertAlmostEqual(arm.sent[-1][1], 2.0) tp.release(gt.BTN_R1, 0.0) press(tp, gt.BTN_RIGHT) self.assertAlmostEqual(arm.sent[-1][0], 2.5) def test_gripper(self): tp, arm, log, _ = make() press(tp, gt.BTN_CROSS) self.assertEqual(arm.grips, [1]) self.assertEqual(tp.grip, "closed") press(tp, gt.BTN_CIRCLE) self.assertEqual(arm.grips, [1, 0]) self.assertEqual(tp.grip, "open") def test_unassigned_do_nothing(self): tp, arm, log, _ = make() for b in (gt.BTN_SQUARE, gt.BTN_SHARE, gt.BTN_L1, gt.BTN_L3, gt.BTN_R3, gt.BTN_TOUCHPAD): press(tp, b) self.assertEqual(arm.sent, []) self.assertEqual(arm.grips, []) self.assertTrue(all(r["kind"] == "ignored" for r in log.rows)) self.assertEqual(tp.joint, 1) class TestGuards(unittest.TestCase): def test_j1_never_above_9087(self): start = [0.0, 85.0, 0.0, 0.0, 0.0, -30.0] tp, arm, log, _ = make(start) tp.r1 = True for _ in range(6): press(tp, gt.BTN_DOWN) self.assertTrue(all(s[1] <= 90.87 + 1e-9 for s in arm.sent)) self.assertAlmostEqual(arm.sent[-1][1], 90.87) clipped = [r for r in log.rows if r.get("clip")] self.assertEqual(clipped[0]["clip"][0]["limit"], "J1_MAX") refused = [r for r in log.rows if r.get("refused")] self.assertTrue(refused and refused[-1]["refused"][0]["why"] == "at_limit") self.assertIn("J1_MAX", tp.last_limit) def test_home_to_pick_height(self): # HOME から j1 を PICK の高さ 90.87 まで下ろせる (基準の姿勢からの ±10° の枠はない. 2026-11-03) tp, arm, log, _ = make() tp.r1 = True for _ in range(50): press(tp, gt.BTN_DOWN) self.assertAlmostEqual(max(s[1] for s in arm.sent), 90.87) self.assertFalse(any(r.get("clip") and r["clip"][0]["limit"] == "BASE" for r in log.rows)) # ほかの関節も ±10° を超えて動く for _ in range(12): press(tp, gt.BTN_RIGHT) self.assertAlmostEqual(arm.sent[-1][0], 24.0) tp.sel = 1 # j2 for _ in range(15): press(tp, gt.BTN_UP) self.assertAlmostEqual(arm.sent[-1][2], -30.0) tp.sel = 4 # j5 for _ in range(10): press(tp, gt.BTN_DOWN) self.assertAlmostEqual(arm.sent[-1][5], -10.0) def test_j4_safe(self): start = [0.0, 0.0, 0.0, 0.0, 55.0, -30.0] tp, arm, log, _ = make(start) tp.sel = 3 tp.r1 = True for _ in range(5): press(tp, gt.BTN_DOWN) self.assertAlmostEqual(max(s[4] for s in arm.sent), 60.0) self.assertEqual([r for r in log.rows if r.get("clip")][0]["clip"][0]["limit"], "J4_SAFE") def test_window_function(self): lo, hi, lo_src, hi_src = gt.joint_window() self.assertAlmostEqual(hi[1], 90.87) self.assertEqual(hi_src[1], "J1_MAX") self.assertAlmostEqual(lo[4], -90.0) self.assertAlmostEqual(hi[4], 60.0) self.assertEqual((lo_src[4], hi_src[4]), ("J4_SAFE", "J4_SAFE")) for k in (0, 2, 3, 5): # 残りは物理可動域 self.assertEqual((lo[k], hi[k]), gt.JOINT_LIMITS[k]) self.assertEqual((lo_src[k], hi_src[k]), ("PHYS", "PHYS")) self.assertEqual(lo[1], gt.JOINT_LIMITS[1][0]) def test_no_compensation_other_joints(self): start = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0] p = gt.plan_step(start, 1, 20.0) self.assertAlmostEqual(p["target"][1], 20.0) self.assertEqual(list(p["target"][[0, 2, 3, 4, 5]]), [0.0, 0.0, 0.0, 0.0, -30.0]) def test_out_of_window_start_refuses(self): # 起動したときに j1 が 91 (守りの外): j1 を下げる (−) 押しも, 他の関節の押しも送らない start = [0.0, 91.0, 0.0, 0.0, 0.0, -30.0] p = gt.plan_step(start, 0, 0.5) self.assertIsNone(p["target"]) self.assertEqual(p["refused"][0]["joint"], 1) def test_home_then_continue(self): start = [10.0, 5.0, 0.0, 0.0, 0.0, -30.0] tp, arm, log, _ = make(start) tp.press(gt.BTN_HOME, 0.0) tp.tick(1.01) tp.poll() self.assertEqual(arm.sent[-1], HOME) tp.r1 = True for _ in range(10): press(tp, gt.BTN_LEFT) self.assertAlmostEqual(arm.sent[-1][0], -20.0) # HOME から続けて動く (枠はない) def test_home_bypasses_range(self): # 範囲の外 (j1=95) にいても HOME は送れる start = [0.0, 95.0, 0.0, 0.0, 0.0, -30.0] tp, arm, log, _ = make(start) press(tp, gt.BTN_DOWN) self.assertEqual(arm.sent, []) tp.press(gt.BTN_HOME, 0.0) tp.tick(1.0) tp.poll() self.assertEqual(arm.sent, [HOME]) def test_start_from_measured_when_not_reached(self): tp, arm, log, _ = make(reach=False) press(tp, gt.BTN_RIGHT) # 届かない (仮の姿勢は HOME のまま) self.assertFalse(tp.cmd_reached) press(tp, gt.BTN_RIGHT) # 次は読んだ角度 (0) から self.assertAlmostEqual(arm.sent[-1][0], 0.5) self.assertEqual(log.rows[-1]["start_source"], "measured") class LagArm(FakeArm): """送ると「届いた」と返すが, 角度は読むたびに lag_step ずつしか追いつかない (読みが古いのと同じ).""" def __init__(self, start=HOME, lag_step=0.0): super().__init__(start) self.target = np.asarray(start, dtype=float).copy() self.lag_step = lag_step self.reads = 0 def read(self): self.reads += 1 d = self.target - self.q self.q = self.q + np.clip(d, -self.lag_step, self.lag_step) return self.q.copy() def send_and_check(self, target, gate, quitting, check=True): if not gate.run(lambda: self.sent.append([float(v) for v in target])): return {"sent": False, "why": "halted_before_send"} self.target = np.asarray(target, dtype=float).copy() if not check: return {"sent": True, "reached": None, "no_check": True} return {"sent": True, "reached": True, "reached_angles": gt._r(self.q), "reach_err": 0.0} def make_arm(arm, sync=True): log = ListLog() out = [] tp = gt.Teleop(arm, arm.q.copy(), log, out=out.append, spawn=lambda fn: fn()) return tp, log, out class TestHold(unittest.TestCase): """長押し (2026-11-03): 0.4 秒から 0.1 秒おきに「読んだ角度 + 3° 先」を送り, 届くのを待たない.""" def test_short_press_is_one_step(self): tp, arm, log, _ = make() tp.press(gt.BTN_RIGHT, 0.0) tp.poll() tp.release(gt.BTN_RIGHT, 0.1) for t in (0.4, 0.6, 1.0): tp.tick(t) tp.poll() self.assertEqual(len(arm.sent), 1) def test_hold_sends_ahead_of_measured_without_waiting(self): arm = LagArm(lag_step=1.0) # 読むたびに 1° ずつ目標へ寄る tp, log, out = make_arm(arm) tp.press(gt.BTN_RIGHT, 0.0); tp.poll() # 1 刻み (0.5°. テストの刻み) for t in (0.1, 0.3, 0.39): tp.tick(t); tp.poll() self.assertEqual(len(arm.sent), 1) t = 0.4 for _ in range(6): tp.tick(t); tp.poll() t += 0.1 self.assertEqual(len(arm.sent), 7) rows = [r for r in log.rows if r["kind"] == "move" and r.get("hold_n", 0) >= 1] for r in rows: # 動かす関節は 読んだ角度 + 3°, ほかは そのまま self.assertAlmostEqual(r["command"][0], r["measured_before"][0] + gt.HOLD_AHEAD_DEG, places=2) self.assertEqual(r["command"][1:], HOME[1:]) self.assertTrue(r["no_check"]) self.assertEqual([r["hold_n"] for r in rows], [1, 2, 3, 4, 5, 6]) tp.release(gt.BTN_RIGHT, 1.05) for t in (1.2, 1.5): tp.tick(t); tp.poll() self.assertEqual(len(arm.sent), 7) self.assertTrue(any("[長押し] j0" in o for o in out)) # 離したときに 1 行 def test_hold_keeps_other_joints_at_command(self): # j1 が重さで 1° 沈んでいても, 長押しの送信で j1 を読んだ値で送り直さない (ずり下がらない) arm = LagArm(start=[0, 30.0, 0, 0, 0, -30], lag_step=1.0) tp, log, out = make_arm(arm) arm.q[1] = 31.0 tp.press(gt.BTN_RIGHT, 0.0); tp.poll() t = 0.4 for _ in range(5): tp.tick(t); tp.poll() t += 0.1 self.assertTrue(all(abs(s[1] - 30.0) < 1e-9 for s in arm.sent)) def test_hold_direction_and_selected_joint(self): tp, arm, log, _ = make() tp.sel = 1 # j2 tp.press(gt.BTN_UP, 0.0); tp.poll() tp.tick(0.4); tp.poll() self.assertAlmostEqual(arm.sent[-1][2], -0.5 - gt.HOLD_AHEAD_DEG) def test_triangle_and_other_buttons_end_hold(self): tp, arm, log, _ = make() tp.press(gt.BTN_DOWN, 0.0); tp.poll() tp.press(gt.BTN_TRIANGLE, 0.1) tp.tick(0.5); tp.poll() self.assertEqual(len(arm.sent), 1) tp.press(gt.BTN_RIGHT, 1.0); tp.poll() tp.press(gt.BTN_CIRCLE, 1.1); tp.poll() # 爪で止まる tp.tick(1.5); tp.poll() self.assertEqual(len(arm.sent), 2) def test_stop_ends_hold_and_resume_does_not_revive(self): tp, arm, log, _ = make() tp.press(gt.BTN_RIGHT, 0.0); tp.poll() tp.tick(0.4); tp.poll() tp.set_triggers(0.5, 0.5) tp.tick(0.6); tp.poll() self.assertEqual(len(arm.sent), 2) self.assertIsNone(tp.hold) tp.set_triggers(-1.0, -1.0) press(tp, gt.BTN_OPTIONS, 0.7) tp.tick(0.9); tp.tick(1.2); tp.poll() self.assertEqual(len(arm.sent), 2) def test_disconnect_ends_hold(self): tp, arm, log, _ = make() tp.press(gt.BTN_RIGHT, 0.0); tp.poll() tp.controller_lost("controller_disconnected") tp.tick(0.5); tp.poll() self.assertEqual(len(arm.sent), 1) self.assertIsNone(tp.hold) def test_limit_ends_hold_without_repeating_lines(self): start = [0.0, 89.0, 0.0, 0.0, 0.0, -30.0] arm = LagArm(start=start, lag_step=0.2) # 端の手前で読みが追いつかない tp, log, out = make_arm(arm) tp.press(gt.BTN_DOWN, 0.0); tp.poll() # 89 → 89.5 tp.tick(0.4); tp.poll() # 読んだ角度 + 3 → 90.87 に切る self.assertAlmostEqual(arm.sent[-1][1], 90.87) tp.tick(0.5); tp.poll() # 同じ端の目標はもう送らない → 長押しを止める n_rows = len(log.rows) for t in (0.6, 0.8, 1.0, 2.0): tp.tick(t); tp.poll() self.assertEqual(len(arm.sent), 2) self.assertEqual(len(log.rows), n_rows) self.assertIsNone(tp.hold) self.assertEqual(log.rows[-1]["refused"][0]["why"], "at_limit") def test_hold_target_never_goes_back(self): # 読みが古く, 前の目標より後ろの「読んだ角度 + 3°」になるときは, 前の目標を送る (引き戻さない) arm = LagArm(lag_step=2.0) tp, log, out = make_arm(arm) tp.press(gt.BTN_RIGHT, 0.0); tp.poll() tp.tick(0.4); tp.poll() arm.lag_step = 0.0 # 読みが進まない (古い) arm.q[0] = 0.0 tp.tick(0.5); tp.poll() js = [s[0] for s in arm.sent] self.assertEqual(js, sorted(js)) # 右へ押している間, 目標は戻らない self.assertTrue(log.rows[-1].get("held_previous_target")) def test_stall_ends_hold(self): arm = LagArm(lag_step=0.0) # まったく動かない (当たっている) tp, log, out = make_arm(arm) tp.press(gt.BTN_RIGHT, 0.0); tp.poll() t = 0.4 for _ in range(15): tp.tick(t); tp.poll() t += 0.1 self.assertIsNone(tp.hold) self.assertLessEqual(len(arm.sent), 1 + 12) self.assertTrue(any("進まないので止めました" in o for o in out)) self.assertTrue(all(s[0] <= gt.HOLD_AHEAD_DEG + 1e-9 for s in arm.sent)) # 先へ出るのは 3° まで def test_short_press_after_hold_starts_from_command(self): tp, arm, log, _ = make() # FakeArm は目標へすぐ着く tp.press(gt.BTN_RIGHT, 0.0); tp.poll() tp.tick(0.4); tp.poll() tp.release(gt.BTN_RIGHT, 0.45) last = arm.sent[-1][0] press(tp, gt.BTN_RIGHT, 1.0) self.assertAlmostEqual(arm.sent[-1][0], last + 0.5) self.assertEqual(log.rows[-1]["start_source"], "command") class TestHoldReview(unittest.TestCase): """2026-11-03 のレビューの指摘 (A: 受け付けられなかった押し / B: 離したあとの 1 刻み / D: 離した合図の取りこぼし).""" def test_press_rejected_while_busy_does_not_become_hold(self): tp, arm, log, spawned = make(sync=False) press(tp, gt.BTN_CROSS) # 爪の仕事で busy self.assertTrue(tp.busy) tp.press(gt.BTN_DOWN, 0.0) # 受付なし self.assertIsNone(tp.hold) spawned.pop(0)() # 爪の仕事が終わる tp.poll() for t in (0.4, 0.6, 0.8, 1.0): tp.tick(t) while spawned: spawned.pop(0)() tp.poll() self.assertEqual(arm.sent, []) def test_press_rejected_while_stopped_does_not_become_hold(self): tp, arm, log, _ = make() tp.set_triggers(0.5, 0.5) tp.press(gt.BTN_RIGHT, 0.0) self.assertIsNone(tp.hold) def test_release_cancels_uncommitted_repeat(self): tp, arm, log, spawned = make(sync=False) out = [] tp.out = out.append tp.press(gt.BTN_RIGHT, 0.0) spawned.pop(0)(); tp.poll() # 1 回目 (短い押しの分) は送る tp.tick(0.4) # 2 回目の仕事ができた (まだ走っていない) tp.release(gt.BTN_RIGHT, 0.45) # 走る前に離した spawned.pop(0)(); tp.poll() self.assertEqual(len(arm.sent), 1) last = [r for r in log.rows if r["kind"] == "move"][-1] self.assertEqual(last["why"], "halted_before_send") self.assertTrue(last["released_during_job"]) self.assertFalse(tp.stopped) self.assertFalse(any("停止が入った" in o for o in out)) tp.press(gt.BTN_RIGHT, 1.0) # 次の押しは普通に動く tp.release(gt.BTN_RIGHT, 1.05) spawned.pop(0)(); tp.poll() self.assertEqual(len(arm.sent), 2) def test_release_does_not_cancel_first_step(self): tp, arm, log, spawned = make(sync=False) tp.press(gt.BTN_RIGHT, 0.0) tp.release(gt.BTN_RIGHT, 0.05) # 短い押し: 仕事が走る前に離しても 1 刻みは送る spawned.pop(0)(); tp.poll() self.assertEqual(len(arm.sent), 1) def test_loop_releases_hold_when_button_state_is_up(self): import pygame pygame.init() screen = pygame.display.set_mode((200, 100)) fonts = {k: pygame.font.SysFont("menlo", 12) for k in ("big", "mono", "small")} js = FakeJoystick(0) js.held = set() tp, arm, log, _ = make() frames = { 0: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)), 2: lambda: (js.held.add(gt.BTN_RIGHT), pygame.event.post(pygame.event.Event(pygame.JOYBUTTONDOWN, instance_id=7, button=gt.BTN_RIGHT, joy=0))), 4: lambda: js.held.discard(gt.BTN_RIGHT), # 離した合図 (JOYBUTTONUP) は来ない 60: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)), } seen = {} def on_frame(n, t): frames.get(n, lambda: None)() if n == 3: seen["hold_at_3"] = t.hold is not None with mock.patch.object(gt.pygame.joystick, "Joystick", lambda i: js): gt.run_loop(tp, screen, fonts, "TEST", on_frame=on_frame) pygame.quit() self.assertTrue(seen["hold_at_3"]) # 押している間は長押しのまま self.assertIsNone(tp.hold) # 状態が離れたら止まる self.assertEqual(len(arm.sent), 1) # 1 秒近くたっても 1 刻みだけ class TestCodexR1(unittest.TestCase): def test_selected_joint_out_of_window_not_clamped(self): # 基準 j1=100 (守りの外) から −0.5 を押しても 90.87 へ 1 回で動かさない start = [0.0, 100.0, 0.0, 0.0, 0.0, -30.0] p = gt.plan_step(start, 1, -0.5) self.assertIsNone(p["target"]) self.assertEqual(p["refused"][0]["why"], "start_out_of_window") tp, arm, log, _ = make(start) press(tp, gt.BTN_UP) self.assertEqual(arm.sent, []) def test_physical_limit_ends(self): base = [0.0, 0.0, 170.0, 0.0, 0.0, -30.0] # j2 の物理可動域は ±177 p = gt.plan_step(base, 2, 20.0) self.assertAlmostEqual(p["target"][2], 177.0) self.assertEqual(p["clip"][0]["limit"], "PHYS") def test_gap_between_command_and_measured(self): tp, arm, log, _ = make() press(tp, gt.BTN_RIGHT) # 目標 j0=0.5 に届いた arm.q[0] = 10.0 # 手で動かされた press(tp, gt.BTN_RIGHT) self.assertAlmostEqual(arm.sent[-1][0], 10.5) # 今読んだ角度から 0.5 self.assertEqual(log.rows[-1]["start_source"], "measured") arm.q[0] = -170.0 # 物理可動域 (-156) の外 → 送らない press(tp, gt.BTN_RIGHT) self.assertEqual(len(arm.sent), 2) def test_home_hold_across_busy_is_dropped(self): tp, arm, log, spawned = make(sync=False) press(tp, gt.BTN_RIGHT) # 動いている tp.press(gt.BTN_HOME, 0.0) # busy 中に押し始め spawned.pop()() tp.poll() # 0.5 秒で空く tp.tick(1.5) self.assertEqual(spawned, []) self.assertIn(("home", False, "busy"), [(r["kind"], r.get("accepted"), r.get("reason")) for r in log.rows]) def test_home_hold_dropped_when_other_op_starts(self): tp, arm, log, spawned = make(sync=False) tp.press(gt.BTN_HOME, 0.0) press(tp, gt.BTN_RIGHT) # 長押しの途中で j0 を動かした tp.tick(0.2) spawned.pop()() tp.poll() tp.tick(1.5) self.assertEqual(spawned, []) self.assertIn("hold_cancelled", [r.get("reason") for r in log.rows]) def test_home_hold_dropped_by_stop_and_disconnect(self): tp, arm, log, _ = make() tp.press(gt.BTN_HOME, 0.0) tp.set_triggers(1, 1) tp.set_triggers(-1, -1) press(tp, gt.BTN_OPTIONS) tp.tick(1.5) self.assertEqual(arm.sent, []) tp.r1 = True tp.press(gt.BTN_HOME, 2.0) tp.controller_lost("controller_disconnected") self.assertFalse(tp.r1) press(tp, gt.BTN_OPTIONS) tp.tick(5.0) self.assertEqual(arm.sent, []) def test_resume_does_not_revive_old_job(self): tp, arm, log, spawned = make(sync=False) press(tp, gt.BTN_RIGHT) tp.set_triggers(1, 1) tp.set_triggers(-1, -1) press(tp, gt.BTN_OPTIONS) # すぐ再開 self.assertFalse(tp.stopped) spawned.pop()() # 止める前の仕事が走る tp.poll() self.assertEqual(arm.sent, []) def test_home_read_failure_does_not_send(self): tp, arm, log, _ = make([5.0, 0, 0, 0, 0, -30]) reads = iter([None, np.array(HOME)]) arm.read = lambda: next(reads) tp.press(gt.BTN_HOME, 0.0) tp.tick(1.0) tp.poll() self.assertEqual(arm.sent, []) self.assertTrue(tp.stopped) def test_log_failure_blocks_resume(self): tp, arm, log, _ = make() def bad(rec): raise OSError("disk gone") log.write = bad press(tp, gt.BTN_TRIANGLE) self.assertTrue(tp.stopped) press(tp, gt.BTN_OPTIONS) self.assertTrue(tp.stopped) press(tp, gt.BTN_RIGHT) self.assertEqual(arm.sent, []) def test_loop_reads_triggers_before_buttons(self): import pygame pygame.init() screen = pygame.display.set_mode((200, 100)) fonts = {k: pygame.font.SysFont("menlo", 12) for k in ("big", "mono", "small")} js = FakeJoystick(0) tp, arm, log, _ = make() frames = { 0: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)), 2: lambda: (js.axes.__setitem__(slice(4, 6), [1.0, 1.0]), pygame.event.post(pygame.event.Event(pygame.JOYBUTTONDOWN, instance_id=7, button=gt.BTN_RIGHT, joy=0))), 4: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)), } with mock.patch.object(gt.pygame.joystick, "Joystick", lambda i: js): gt.run_loop(tp, screen, fonts, "TEST", on_frame=lambda n, t: frames.get(n, lambda: None)()) self.assertEqual(arm.sent, []) self.assertTrue(tp.stopped) # 読み取りに失敗したコマのボタンも処理しない js2 = FakeJoystick(0) js2.get_axis = lambda a: (_ for _ in ()).throw(pygame.error("gone")) tp2, arm2, log2, _ = make() frames2 = { 0: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)), 2: lambda: pygame.event.post(pygame.event.Event(pygame.JOYBUTTONDOWN, instance_id=7, button=gt.BTN_RIGHT, joy=0)), 4: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)), } with mock.patch.object(gt.pygame.joystick, "Joystick", lambda i: js2): gt.run_loop(tp2, screen, fonts, "TEST", on_frame=lambda n, t: frames2.get(n, lambda: None)()) self.assertEqual(arm2.sent, []) self.assertTrue(tp2.stopped) pygame.quit() class TestShutdown(unittest.TestCase): def test_uncommitted_job_is_not_sent_at_exit(self): tp, arm, log, spawned = make(sync=False) press(tp, gt.BTN_RIGHT) tp.shutdown() # 仕事はまだ走っていない spawned.pop()() # 終了のあとに走っても送らない self.assertEqual(arm.sent, []) last = [r for r in log.rows if r["kind"] == "move"][0] self.assertIs(last["send_committed_before_exit"], False) self.assertTrue(last["interrupted_at_exit"]) def test_committed_send_is_waited_and_recorded(self): tp, arm, log, _ = make(sync=False) release = threading.Event() entered = threading.Event() orig = arm.send_and_check def slow(target, gate, quitting): def fn(): entered.set() release.wait(2.0) arm.sent.append([float(v) for v in target]) if not gate.run(fn): return {"sent": False, "why": "halted_before_send"} return {"sent": True, "reached": None, "interrupted": True} arm.send_and_check = slow tp.spawn = tp._spawn_thread press(tp, gt.BTN_RIGHT) entered.wait(1.0) threading.Timer(0.2, release.set).start() done = tp.shutdown() # 送ると決めた送信が戻るまで待つ self.assertTrue(done) self.assertEqual(len(arm.sent), 1) move = [r for r in log.rows if r["kind"] == "move"][0] self.assertIs(move["send_committed_before_exit"], True) def test_worker_not_done_returns_false(self): tp, arm, log, _ = make(sync=False) entered = threading.Event() def stuck(target, gate, quitting): gate.run(lambda: (entered.set(), threading.Event().wait(1.0))) return {"sent": True} arm.send_and_check = stuck tp.spawn = tp._spawn_thread press(tp, gt.BTN_RIGHT) entered.wait(1.0) with mock.patch.object(gt, "WORKER_JOIN_SEC", 0.1): self.assertFalse(tp.shutdown()) # 戻らない → main はソケットを閉じない class TestStop(unittest.TestCase): def test_both_triggers(self): tp, arm, log, _ = make() tp.set_triggers(0.5, -1.0) self.assertFalse(tp.stopped) tp.set_triggers(-1.0, 0.9) self.assertFalse(tp.stopped) tp.set_triggers(0.0, 0.0) # ちょうど 0.0 は超えていない self.assertFalse(tp.stopped) tp.set_triggers(0.01, 0.01) self.assertTrue(tp.stopped) self.assertEqual(tp.state, gt.STATE_STOPPED) def test_stopped_rejects_and_options_resumes(self): tp, arm, log, _ = make() tp.set_triggers(1.0, 1.0) for b in (gt.BTN_RIGHT, gt.BTN_DOWN, gt.BTN_CROSS, gt.BTN_CIRCLE): press(tp, b) tp.press(gt.BTN_HOME, 0.0) tp.tick(2.0) tp.release(gt.BTN_HOME, 2.0) self.assertEqual(arm.sent, []) self.assertEqual(arm.grips, []) self.assertTrue(all(r.get("reason") == "stopped" for r in log.rows if r["kind"] in ("move", "grip_close", "grip_open", "home"))) press(tp, gt.BTN_OPTIONS) # まだ L2+R2 を押している → 戻らない self.assertTrue(tp.stopped) tp.set_triggers(-1.0, -1.0) press(tp, gt.BTN_OPTIONS) self.assertFalse(tp.stopped) press(tp, gt.BTN_RIGHT) self.assertEqual(len(arm.sent), 1) def test_disconnect_and_read_error(self): tp, arm, log, _ = make() tp.controller_lost("controller_disconnected") self.assertTrue(tp.stopped) self.assertEqual(log.rows[-1]["kind"], "controller_lost") tp2, arm2, log2, _ = make() tp2.controller_read_failed("x") tp2.controller_read_failed("x") self.assertTrue(tp2.stopped) self.assertEqual(sum(r["kind"] == "read_error" for r in log2.rows), 1) press(tp2, gt.BTN_OPTIONS) # 読めないうちは戻らない self.assertTrue(tp2.stopped) tp2.controller_read_ok() press(tp2, gt.BTN_OPTIONS) self.assertFalse(tp2.stopped) def test_stop_while_moving_blocks_send(self): tp, arm, log, spawned = make(sync=False) press(tp, gt.BTN_RIGHT) # 仕事を作ったが, まだ走らせていない self.assertTrue(tp.busy) tp.set_triggers(1.0, 1.0) # 動いている間でも停止は受け付ける self.assertTrue(tp.stopped) spawned.pop()() # 仕事が走る: 送る直前に停止を見て送らない tp.poll() self.assertEqual(arm.sent, []) self.assertFalse(tp.busy) move = [r for r in log.rows if r["kind"] == "move"][0] self.assertEqual(move["why"], "halted_before_send") class TestHomeHold(unittest.TestCase): def test_short_press_does_not_send(self): tp, arm, log, _ = make() tp.press(gt.BTN_HOME, 10.0) tp.tick(10.5) tp.tick(10.99) tp.release(gt.BTN_HOME, 10.99) tp.poll() self.assertEqual(arm.sent, []) self.assertEqual(log.rows[-1]["kind"], "home_short") def test_hold_sends_once(self): tp, arm, log, _ = make([5.0, 0, 0, 0, 0, -30]) tp.press(gt.BTN_HOME, 0.0) for t in (0.5, 1.0, 1.5, 3.0, 5.0): tp.tick(t) tp.poll() tp.release(gt.BTN_HOME, 5.0) self.assertEqual(arm.sent, [HOME]) self.assertEqual(sum(r["kind"] == "home" for r in log.rows), 1) self.assertFalse(any(r["kind"] == "home_short" for r in log.rows)) tp.press(gt.BTN_HOME, 6.0) # 押し直せば, また 1 回 tp.tick(7.0) tp.poll() self.assertEqual(len(arm.sent), 2) class TestBusy(unittest.TestCase): def test_presses_while_moving_are_dropped(self): tp, arm, log, spawned = make(sync=False) press(tp, gt.BTN_RIGHT) for b in (gt.BTN_RIGHT, gt.BTN_DOWN, gt.BTN_CROSS): press(tp, b) tp.press(gt.BTN_HOME, 0.0) tp.tick(1.5) self.assertEqual(len(spawned), 1) # 仕事は 1 つだけ spawned.pop()() tp.poll() tp.tick(3.0) # 長押しのまま空いても, ためた HOME は出ない self.assertEqual(spawned, []) self.assertEqual(arm.sent, [[0.5, 0, 0, 0, 0, -30]]) busy = [r for r in log.rows if r.get("reason") == "busy"] self.assertEqual(len(busy), 4) class TestDaemonArm(unittest.TestCase): def _arm(self, reads): it = iter(reads) last = [None] def read(): try: last[0] = next(it) except StopIteration: pass return None if last[0] is None else np.asarray(last[0], dtype=float) return read def test_halt_blocks_send(self): sends = [] with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \ mock.patch.object(gt, "daemon_send_angles", lambda *a, **k: sends.append(a)): gate, quit_ = gt.SendGate(), threading.Event() gate.cancel_now() r = gt.DaemonArm().send_and_check(np.array(HOME), gate, quit_) self.assertEqual(sends, []) self.assertFalse(r["sent"]) def test_single_send_no_retry(self): sends = [] with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: sends.append((a, k))), \ mock.patch.object(gt, "REACH_TIMEOUT_SEC", 0.3), \ mock.patch.object(gt.wr, "_compute_arrival_timeout", lambda d: 0.1), \ mock.patch.object(gt, "POLL_SLEEP", 0.05): r = gt.DaemonArm().send_and_check(np.array([5.0, 0, 0, 0, 0, -30]), gt.SendGate(), threading.Event()) self.assertEqual(len(sends), 1) self.assertEqual(sends[0][1]["speed"], 20) self.assertFalse(r["reached"]) self.assertTrue(r["sent"]) def test_ignored_send_is_resent(self): # 送っても動かない (無視された) → 0.5 秒後に同じ目標を送り直す → 動いて届く (2026-11-03) sends = [] state = {"q": np.array(HOME, dtype=float)} def send(a, **k): sends.append(list(a)) if len(sends) >= 2: # 2 回目で通る state["q"] = np.array(a, dtype=float) target = [5.0, 0, 0, 0, 0, -30] with mock.patch.object(gt.rpc, "read_angles", lambda: state["q"].copy()), \ mock.patch.object(gt, "daemon_send_angles", send), \ mock.patch.object(gt, "IGNORED_CHECK_SEC", 0.05), \ mock.patch.object(gt, "POLL_SLEEP", 0.02): r = gt.DaemonArm().send_and_check(np.array(target), gt.SendGate(), threading.Event()) self.assertEqual(len(sends), 2) self.assertEqual(sends[1], target) self.assertTrue(r["reached"]) self.assertEqual(len(r["resends"]), 1) def test_resend_limited_and_not_when_moving(self): sends = [] with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: sends.append(a)), \ mock.patch.object(gt, "IGNORED_CHECK_SEC", 0.02), \ mock.patch.object(gt, "REACH_TIMEOUT_SEC", 0.5), \ mock.patch.object(gt.wr, "_compute_arrival_timeout", lambda d: 0.1), \ mock.patch.object(gt, "POLL_SLEEP", 0.01): r = gt.DaemonArm().send_and_check(np.array([5.0, 0, 0, 0, 0, -30]), gt.SendGate(), threading.Event()) self.assertEqual(len(sends), 1 + gt.IGNORED_MAX_RESENDS) self.assertFalse(r["reached"]) # 動いている (届いていないが 0.3° 以上動いた) なら送り直さない sends.clear() moving = [HOME] + [[0.5 * (i + 1), 0, 0, 0, 0, -30] for i in range(6)] with mock.patch.object(gt.rpc, "read_angles", self._arm(moving)), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: sends.append(a)), \ mock.patch.object(gt, "IGNORED_CHECK_SEC", 0.0), \ mock.patch.object(gt, "REACH_TIMEOUT_SEC", 0.2), \ mock.patch.object(gt.wr, "_compute_arrival_timeout", lambda d: 0.1), \ mock.patch.object(gt, "POLL_SLEEP", 0.01): gt.DaemonArm().send_and_check(np.array([9.0, 0, 0, 0, 0, -30]), gt.SendGate(), threading.Event()) self.assertEqual(len(sends), 1) def test_no_resend_after_stop(self): sends = [] gate = gt.SendGate() reads = {"n": 0} def read(): reads["n"] += 1 if reads["n"] == 2: gate.cancel_now() # 確かめる途中で L2+R2 return np.array(HOME, dtype=float) with mock.patch.object(gt.rpc, "read_angles", read), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: sends.append(a)), \ mock.patch.object(gt, "IGNORED_CHECK_SEC", 0.0), \ mock.patch.object(gt, "POLL_SLEEP", 0.01): r = gt.DaemonArm().send_and_check(np.array([5.0, 0, 0, 0, 0, -30]), gate, threading.Event()) self.assertEqual(len(sends), 1) self.assertTrue(r["halted_during_check"]) def test_reached(self): target = [1.0, 0, 0, 0, 0, -30] with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME, [0.5, 0, 0, 0, 0, -30]])), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: None): r = gt.DaemonArm().send_and_check(np.array(target), gt.SendGate(), threading.Event()) self.assertTrue(r["reached"]) self.assertEqual(r["reach_err"], 0.5) def test_reach_uses_unrounded_error(self): target = [3.004, 0, 0, 0, 0, -30] with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: None), \ mock.patch.object(gt, "REACH_TIMEOUT_SEC", 0.2), \ mock.patch.object(gt.wr, "_compute_arrival_timeout", lambda d: 0.1), \ mock.patch.object(gt, "POLL_SLEEP", 0.05): r = gt.DaemonArm().send_and_check(np.array(target), gt.SendGate(), threading.Event()) self.assertFalse(r["reached"]) def test_read_none_during_check_is_error(self): with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME, None])), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: None): r = gt.DaemonArm().send_and_check(np.array([5.0, 0, 0, 0, 0, -30]), gt.SendGate(), threading.Event()) self.assertTrue(r["sent"]) self.assertEqual(r["error"], "read_failed_during_check") self.assertIsNone(r["reached"]) def test_stop_does_not_wait_and_reports_commit(self): # 送ると決めて送信 (通信) の最中に停止が来ても, 停止は待たない. 決めていたことを返す. # 停止のあとに「送ると決める」ことはない gate = gt.SendGate() entered, release = threading.Event(), threading.Event() def slow_send(): entered.set() release.wait(2.0) t = threading.Thread(target=lambda: gate.run(slow_send)) t.start() entered.wait(1.0) got = [] stopper = threading.Thread(target=lambda: got.append(gate.cancel_now())) stopper.start() stopper.join(0.5) self.assertFalse(stopper.is_alive()) # 待たされない self.assertEqual(got, [True]) # 停止より前に送ると決めていた release.set() t.join(2.0) self.assertFalse(gate.run(lambda: self.fail("取り消しのあとに送った"))) g2 = gt.SendGate() self.assertFalse(g2.cancel_now()) # まだ決めていない仕事は送らない self.assertFalse(g2.run(lambda: self.fail("取り消しのあとに送った"))) def test_send_uses_rpc_timeout(self): seen = [] with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \ mock.patch.object(gt, "daemon_send_angles", lambda a, **k: seen.append(k)): gt.DaemonArm().send_and_check(np.array(HOME), gt.SendGate(), threading.Event()) self.assertEqual(seen[0]["timeout_override"], gt.SEND_RPC_TIMEOUT_SEC) def test_stop_log_says_whether_send_was_committed(self): tp, arm, log, spawned = make(sync=False) press(tp, gt.BTN_RIGHT) tp.set_triggers(1, 1) # まだ送ると決めていない stop = [r for r in log.rows if r["kind"] == "stop"][0] self.assertIs(stop["send_committed_before_stop"], False) def test_gripper_gate(self): calls = [] with mock.patch.object(gt, "daemon_set_gripper_value", lambda v: calls.append(("close", v))), \ mock.patch.object(gt, "daemon_set_gripper", lambda s, wait: calls.append(("open", s, wait))), \ mock.patch.object(gt.rpc, "GRIPPER_SLEEP", 0.0): arm = gt.DaemonArm() self.assertTrue(arm.gripper(1, gt.SendGate())) self.assertTrue(arm.gripper(0, gt.SendGate())) g = gt.SendGate() g.cancel_now() self.assertFalse(arm.gripper(1, g)) self.assertEqual(calls, [("close", 20), ("open", 0, gt.rpc.GRIP_OPEN_DAEMON_WAIT)]) class FakeJoystick: def __init__(self, idx): self.axes = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0] def get_instance_id(self): return 7 def get_name(self): return "PS4 Controller" def get_guid(self): return "fake-guid" def get_numaxes(self): return 6 def get_numbuttons(self): return 16 def get_numhats(self): return 0 def get_axis(self, a): return self.axes[a] def get_button(self, b): return 1 if b in getattr(self, "held", set()) else 0 class TestDryRunMain(unittest.TestCase): def test_flow(self): import pygame js_holder = {} def make_js(idx): js_holder["js"] = FakeJoystick(idx) return js_holder["js"] def btn(b, down=True): t = pygame.JOYBUTTONDOWN if down else pygame.JOYBUTTONUP pygame.event.post(pygame.event.Event(t, instance_id=7, button=b, joy=0)) # フレームごとの台本 (ドライランは 1 回 0.3 秒動いている扱い = 60fps で 18 コマ程度) script = { 1: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)), 3: lambda: (btn(gt.BTN_RIGHT), btn(gt.BTN_RIGHT, False)), 5: lambda: (btn(gt.BTN_RIGHT), btn(gt.BTN_RIGHT, False)), # 動いている間 → 受け付けない 40: lambda: btn(gt.BTN_TRIANGLE), 42: lambda: (btn(gt.BTN_DOWN), btn(gt.BTN_DOWN, False)), 80: lambda: btn(gt.BTN_CROSS), 82: lambda: js_holder["js"].axes.__setitem__(slice(4, 6), [0.5, 0.5]), # 停止 85: lambda: (btn(gt.BTN_LEFT), btn(gt.BTN_LEFT, False)), 86: lambda: js_holder["js"].axes.__setitem__(slice(4, 6), [-1.0, -1.0]), 88: lambda: btn(gt.BTN_OPTIONS), 150: lambda: btn(gt.BTN_HOME), 230: lambda: btn(gt.BTN_HOME, False), 300: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)), } def on_frame(n, tp): if n in script: script[n]() logs = [] class SpyNullLog(gt.NullLog): # 書かないことは変えずに, 渡された行だけ見る def __init__(self): self.rows = [] logs.append(self) def write(self, rec): json.dumps(rec, ensure_ascii=False) self.rows.append(rec) out = [] with tempfile.TemporaryDirectory() as td, \ mock.patch.object(gt, "LOG_ROOT", Path(td)), \ mock.patch.object(gt, "ensure_ssd_mounted", side_effect=AssertionError("KIOXIA を確かめた")), \ mock.patch.object(gt, "load_daemon_modules", side_effect=AssertionError("daemon の送り口を読んだ")), \ mock.patch.object(gt, "JsonlLog", side_effect=AssertionError("log.jsonl を開いた")), \ mock.patch.object(gt, "write_json_atomic", side_effect=AssertionError("meta.json を書いた")), \ mock.patch.object(gt.shutil, "copy2", side_effect=AssertionError("スクリプトを写した")), \ mock.patch.object(gt, "NullLog", SpyNullLog), \ mock.patch.object(gt.pygame.joystick, "Joystick", make_js), \ mock.patch.object(gt, "GRIPPER_SLEEP", 0.2), \ mock.patch.object(gt, "get_daemon_socket", side_effect=AssertionError("daemon に触った")), \ mock.patch.object(gt, "daemon_send_angles", side_effect=AssertionError("daemon に送った")), \ mock.patch("builtins.print", lambda *a, **k: out.append(" ".join(str(x) for x in a))): gt.main([], on_frame=on_frame) self.assertEqual(list(Path(td).iterdir()), []) # フォルダも作らない self.assertEqual(len(logs), 1) rows = logs[0].rows self.assertIn("[記録] ドライランなので記録しません", out) self.assertIn("[終了] アームは動かしていません。記録なし（ドライラン）", out) kinds = [(r["kind"], r.get("accepted"), r.get("reason")) for r in rows] # 受け付けた 1 回目の行は届いてから書くので, 動いている間に断った行 (busy) が先に来る self.assertEqual(kinds[0], ("move", False, "busy")) self.assertEqual(kinds[1][:2], ("move", True)) self.assertIn(("move", False, "busy"), kinds) self.assertIn(("select_joint", True, None), kinds) self.assertIn(("stop", True, "L2+R2"), kinds) self.assertIn(("move", False, "stopped"), kinds) self.assertIn(("resume", True, None), kinds) home = [r for r in rows if r["kind"] == "home"] self.assertEqual(len(home), 1) self.assertEqual(home[0]["command"], HOME) moves = [r for r in rows if r["kind"] == "move" and r.get("sent") is True] self.assertEqual(moves[0]["command"], [0.5, 0, 0, 0, 0, -30]) self.assertEqual(moves[1]["command"], [0.5, 0.0, 0.5, 0, 0, -30]) # △ で j2 にしてから下 for r in moves: for key in ("t", "button", "kind", "selected_joint", "step_deg", "measured_before", "command", "reached_angles", "reached", "grip_state", "clip", "stopped"): self.assertIn(key, r) self.assertTrue(all(r["execute"] is False for r in rows)) class TestExecuteMainRecords(unittest.TestCase): """--execute は今までどおり KIOXIA (ここでは一時フォルダ) に log.jsonl・meta.json・写しを書く.""" def test_execute_writes(self): import pygame script = { 1: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)), 3: lambda: (pygame.event.post(pygame.event.Event(pygame.JOYBUTTONDOWN, instance_id=7, button=gt.BTN_RIGHT, joy=0)), pygame.event.post(pygame.event.Event(pygame.JOYBUTTONUP, instance_id=7, button=gt.BTN_RIGHT, joy=0))), 60: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)), } sends = [] q = {"now": list(HOME)} def send(a, **k): sends.append(list(a)) q["now"] = list(a) with tempfile.TemporaryDirectory() as td, \ mock.patch.object(gt, "LOG_ROOT", Path(td)), \ mock.patch.object(gt, "ensure_ssd_mounted", lambda: None), \ mock.patch.object(gt.pygame.joystick, "Joystick", FakeJoystick), \ mock.patch.object(gt, "get_daemon_socket", lambda: None), \ mock.patch.object(gt, "close_daemon_socket", lambda: None), \ mock.patch.object(gt.rpc, "install_daemon_lock", lambda: None), \ mock.patch.object(gt.rpc, "read_angles", lambda: np.array(q["now"], dtype=float)), \ mock.patch.object(gt, "daemon_send_angles", send), \ mock.patch("builtins.print", lambda *a, **k: None): gt.main(["--execute"], on_frame=lambda n, tp: script[n]() if n in script else None) runs = list(Path(td).iterdir()) self.assertEqual(len(runs), 1) run = runs[0] self.assertFalse(run.name.endswith("_dry")) rows = [json.loads(x) for x in (run / "log.jsonl").read_text().splitlines()] meta = json.loads((run / "meta.json").read_text()) self.assertTrue((run / "gamepad_teleop.py").exists()) self.assertEqual(sends, [[0.5, 0.0, 0.0, 0.0, 0.0, -30.0]]) self.assertEqual([(r["kind"], r.get("sent")) for r in rows], [("move", True)]) self.assertTrue(rows[0]["execute"]) self.assertEqual(meta["controllers"][0]["name"], "PS4 Controller") self.assertEqual(meta["controllers"][0]["guid"], "fake-guid") self.assertTrue(meta["execute"]) self.assertEqual(meta["end_reason"], "window_closed") self.assertEqual(meta["n_sent"], 1) self.assertEqual(meta["guards"]["J1_MAX_DEG"], 90.87) self.assertEqual(meta["motion"]["speed"], 20) for key in ("python", "pygame", "sdl", "start_pose", "git"): self.assertIn(key, meta) class TestNoCv2InDryRun(unittest.TestCase): """依頼 69: ドライランは cv2 (cv2/.dylibs/libSDL2) を読まず, objc の重複の警告が出ない. このプロセスはもう load_daemon_modules() を呼んでいるので, 別のプロセスで main([]) を回す.""" CHILD = r''' import os, sys, tempfile os.environ["SDL_VIDEODRIVER"] = "dummy"; os.environ["SDL_AUDIODRIVER"] = "dummy" sys.path.insert(0, sys.argv[1]) from pathlib import Path import gamepad_teleop as gt import pygame td = tempfile.mkdtemp() gt.LOG_ROOT = Path(td) gt.SSD_MOUNT = "/nonexistent_kioxia_for_test" # KIOXIA がなくても動く gt.main([], on_frame=lambda n, tp: pygame.event.post(pygame.event.Event(pygame.QUIT)) if n == 5 else None) assert os.listdir(td) == [], os.listdir(td) bad = [m for m in ("cv2", "run_policy_client", "visual_servo_pick", "j0_sweep_scan") if m in sys.modules] assert not bad, bad assert gt.rpc is None and gt.daemon_send_angles is None print("CHILD_OK") ''' def test_dry_run_in_fresh_process(self): r = subprocess.run([sys.executable, "-c", self.CHILD, str(Path(gt.__file__).parent)], capture_output=True, text=True, timeout=60) self.assertEqual(r.returncode, 0, r.stdout + r.stderr) self.assertIn("CHILD_OK", r.stdout) self.assertNotIn("is implemented in both", r.stderr) class TestConstantsMatchModules(unittest.TestCase): """読み込まずに読んだ値が, 読み込んだモジュールの値と同じ.""" def test_same(self): self.assertEqual(gt.HOME, [float(v) for v in gt.rpc.HOME]) self.assertEqual(gt.J4_SAFE, tuple(float(v) for v in gt.rpc.J4_SAFE)) self.assertEqual(gt.GRIPPER_SLEEP, gt.rpc.GRIPPER_SLEEP) self.assertEqual(gt.JOINT_LIMITS, [tuple(float(v) for v in x) for x in gt.wr.JOINT_LIMITS]) self.assertEqual((gt.SPEED, gt.POLL_SLEEP, gt.REACH_TIMEOUT_SEC), (20, 0.3, 8.0)) def test_read_constants_rejects_double_assignment(self): with tempfile.TemporaryDirectory() as td: f = Path(td) / "m.py" f.write_text("A = 1\nA = 2\n") with self.assertRaises(RuntimeError): gt.read_constants(f, ("A",)) with self.assertRaises(RuntimeError): gt.read_constants(f, ("B",)) for src in ("A = 1\nA += 1\n", "A = 1\nA: int = 2\n", "A = 1\nB = A = 2\n", "A = 1\nfor A in []:\n pass\n", "A = 1\ndef f():\n global A\n A = 2\n"): f.write_text(src) with self.assertRaises(RuntimeError, msg=src): gt.read_constants(f, ("A",)) f.write_text("A = 1\ndef f():\n return A\n") self.assertEqual(gt.read_constants(f, ("A",)), {"A": 1}) class TestExecuteLoadGuards(unittest.TestCase): def test_no_load_when_ssd_missing(self): with mock.patch.object(gt, "ensure_ssd_mounted", side_effect=SystemExit("no ssd")), \ mock.patch.object(gt, "load_daemon_modules") as load: with self.assertRaises(SystemExit) as cm: gt.main(["--execute"]) self.assertEqual(str(cm.exception), "no ssd") load.assert_not_called() def test_load_failure_aborts_before_record(self): with tempfile.TemporaryDirectory() as td, \ mock.patch.object(gt, "LOG_ROOT", Path(td)), \ mock.patch.object(gt, "ensure_ssd_mounted", lambda: None), \ mock.patch.object(gt, "load_daemon_modules", side_effect=ImportError("x")), \ mock.patch.object(gt, "get_daemon_socket", side_effect=AssertionError("daemon に触った")): with self.assertRaises(SystemExit) as cm: gt.main(["--execute"]) self.assertIn("[ABORT]", str(cm.exception)) self.assertEqual(list(Path(td).iterdir()), []) def test_mismatch_aborts_in_fresh_process(self): """ファイルから読んだ値と読み込んだ値が違えば, daemon の送り口を入れずに止める (別のプロセス).""" child = r''' import os, sys, tempfile os.environ["SDL_VIDEODRIVER"] = "dummy" sys.path.insert(0, sys.argv[1]) from pathlib import Path import gamepad_teleop as gt gt.HOME = [1.0, 0.0, 0.0, 0.0, 0.0, -30.0] td = tempfile.mkdtemp() gt.LOG_ROOT = Path(td) gt.ensure_ssd_mounted = lambda: None try: gt.main(["--execute"]) except SystemExit as e: assert "HOME" in str(e), e else: raise AssertionError("止まらなかった") assert os.listdir(td) == [], os.listdir(td) assert gt.rpc is None and gt.daemon_send_angles is None and gt.get_daemon_socket is None print("CHILD_OK") ''' r = subprocess.run([sys.executable, "-c", child, str(Path(gt.__file__).parent)], capture_output=True, text=True, timeout=60) self.assertEqual(r.returncode, 0, r.stdout + r.stderr) self.assertIn("CHILD_OK", r.stdout) if __name__ == "__main__": unittest.main()