#!/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 を超えない / j0 ±15°・j1〜j5 ±10° / J4_SAFE / HOME のあと基準が HOME - 停止: L2・R2 の両方 > 0.0 で止まる・片方では止まらない / 停止中は受け付けない / OPTIONS で戻る / 接続が切れたら停止 - HOME: 1 秒に満たない長押しでは送らない / 1 秒続けたら 1 回だけ送る - 動いている間の押しは受け付けない (ためない) - daemon への送り方: 停止が立っていれば送らない / 届かなくても送り直さない - ドライランで main() がひととおり動く (偽のコントローラー・ダミーの画面・一時フォルダ) - codex R1 の指摘: 範囲外の出発点 / 目標と読んだ角度のずれ / 長押しの持ち越し / 停止と送信の順番 / 再開で古い仕事が生き返らない / 読み取り失敗 / 記録の失敗 / ボタンより先に L2+R2 を見る KIOXIA にもアームにも触らない. """ import json import os 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 HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -30.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): if not gate.run(lambda: self.sent.append([float(v) for v in target])): return {"sent": False, "why": "halted_before_send"} 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_j0_15_and_others_10(self): tp, arm, log, _ = make() tp.r1 = True for _ in range(12): press(tp, gt.BTN_RIGHT) self.assertAlmostEqual(max(s[0] for s in arm.sent), 15.0) for _ in range(20): press(tp, gt.BTN_LEFT) self.assertAlmostEqual(min(s[0] for s in arm.sent), -15.0) tp.sel = 1 # j2 for _ in range(10): press(tp, gt.BTN_UP) self.assertAlmostEqual(min(s[2] for s in arm.sent), -10.0) tp.sel = 4 # j5 (基準 -30) for _ in range(10): press(tp, gt.BTN_DOWN) self.assertAlmostEqual(max(s[5] for s in arm.sent), -20.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([0, 85, 0, 0, 55, -30]) self.assertAlmostEqual(hi[1], 90.87) self.assertEqual(hi_src[1], "J1_MAX") self.assertAlmostEqual(hi[4], 60.0) self.assertEqual(hi_src[4], "J4_SAFE") self.assertAlmostEqual(lo[0], -15.0) self.assertAlmostEqual(hi[2], 10.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, start) self.assertAlmostEqual(p["target"][1], 10.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, start) self.assertIsNone(p["target"]) self.assertEqual(p["refused"][0]["joint"], 1) def test_home_rebases(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) self.assertEqual(list(tp.base), HOME) tp.r1 = True for _ in range(10): press(tp, gt.BTN_LEFT) self.assertAlmostEqual(arm.sent[-1][0], -15.0) # 基準が HOME (j0=0) になった 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 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, start) 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, base) 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] = -20.0 # 基準から ±15 の外 → 送らない 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_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] 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]() 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", make_js), \ mock.patch.object(gt.rpc, "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 に送った")): gt.main([], on_frame=on_frame) runs = list(Path(td).iterdir()) self.assertEqual(len(runs), 1) run = runs[0] self.assertTrue(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()) 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.assertEqual(meta["controllers"][0]["name"], "PS4 Controller") self.assertEqual(meta["controllers"][0]["guid"], "fake-guid") self.assertFalse(meta["execute"]) self.assertEqual(meta["end_reason"], "window_closed") self.assertEqual(meta["guards"]["J1_MAX_DEG"], 90.87) for key in ("python", "pygame", "sdl", "start_pose", "git"): self.assertIn(key, meta) if __name__ == "__main__": unittest.main()