commit 32a42803fea52e2b745d1fb51aa69232fe06a4f7 Author: Eiichi Shimazu Date: Sat Nov 7 14:21:21 2026 +0900 daemon に電流などを読むだけの read_servo を足し (tools/pi. Pi へは tools/daemon_add_read_servo.sh で置く. まだ置いていない), 関節と爪の電流と角度を読める限り速く読んで KIOXIA の diag/servo_current_{日付}/ に残す tools/servo_current_probe.py を作る (印のキー・--with-pad で gamepad_teleop を同じプロセスで動かしパッドの命令の鍵の待ちを残す). 学習のテストの偽物のチェックポイントの assets 名を作るモデルの学習データに合わせる (lab_train_fakes.sh・test_lab_train.sh. 本番の流れは変えない) (依頼 109. codex 3 回) Co-Authored-By: Claude Opus 5.5 diff --git a/docs/review_request_20261107_read_servo.md b/docs/review_request_20261107_read_servo.md new file mode 100644 index 0000000..81e4d34 --- /dev/null +++ b/docs/review_request_20261107_read_servo.md @@ -0,0 +1,30 @@ +# コードレビュー依頼：daemon に電流を読むだけの命令 read_servo を足す（2026-11-07 依頼 109） + +mechArm 270-Pi の関節と爪の電流が読めるか・何 Hz で読めるかを測るための準備。Pi の daemon（tools/pi/mecharm_daemon.py。Pi の Python 3.6・pymycobot 2.9.6 の MyCobot で動く。TCP 5555 で 1 本ずつ受ける）に、読むだけの命令を 1 つ足した。まだ Pi には置いていない（置くのは tools/daemon_add_read_servo.sh。エイイチが流す）。 + +## 守りたいこと（ここを重く見てください） +- アームと爪に何も送らないこと（pymycobot の読む関数だけを呼ぶ） +- 今までの命令（ping・send_angles・send_angle・get_angles・get_angles_once・send_angles_fast・set_gripper*・get_gripper_value・is_gripper_moving・set_fresh_mode・stop）の動きを 1 文字も変えないこと +- 読めないときに daemon が落ちない・止まらないこと（1 件ずつ処理するので、長くかかるとゲームパッドの指令が遅れる） +- Python 3.6 で動くこと + +## したこと +- `tools/pi/mecharm_daemon.py`: `handle_servo_read()` と `_servo_read_one()` を足し、`handle_command` の unknown cmd の else の直前に `elif cmd == "read_servo": return handle_servo_read(cmd_obj)` を足した + - 頼まれた項目（angles・currents は既定で読む / status・gripper・regs は頼んだときだけ）を 1 回ずつ呼ぶ。読み直さない・待たない + - 例外や関数がないときは value None と error（ok=True のまま。get_angles_once と同じ考え） + - regs は get_servo_data(servo_id, addr)。servo_id 1〜7（7 = 爪）・addr 0〜100・32 個まで。外れていれば何も読まずに ok=False + - raw=True のとき、インスタンスの `_read` を一時的に包んで返事のバイト列を写し、finally で `del _mc._read` で外す（2.9.6 の MyCobot は `_read = read` をクラスに持つ） +- `tools/daemon_add_read_servo.sh`: Pi の今の daemon を持ち帰り KIOXIA に写しを残し、11-05 の版（sha256 806336b6…。git HEAD の tools/pi/mecharm_daemon.py）と同じときだけ新しい版を置いて systemd で起動し直し、ping・get_angles・read_servo を確かめる。違えば何も変えずに止まる +- `tools/test_daemon_read_servo.py`（新）: 偽物の MyCobot で 10 件（送る関数が呼ばれない・順番・読めないとき・範囲外は何も読まない・raw のあと `_read` が戻る・今までの命令の返事が同じ） +- 参考（daemon ではない Mac 側。見てもらえれば助かるが主ではない）: `tools/servo_current_probe.py`（read_servo を繰り返し呼んで CSV に残す。--with-pad で gamepad_teleop を同じプロセスで動かし、policy_recorder.install_daemon_lock の鍵を gamepad_teleop の送り口を読む前にかける）、`tools/test_servo_current_probe.py` + +差分: `git diff -- tools/pi/mecharm_daemon.py` と、新しいファイル `tools/daemon_add_read_servo.sh`・`tools/test_daemon_read_servo.py`・`tools/servo_current_probe.py`・`tools/test_servo_current_probe.py`。pymycobot 2.9.6 のソースは /tmp/pmc_x/pymycobot/（generate.py の get_servo_currents・get_servo_status・get_servo_data、mycobot.py の _mesg、common.py の read・_process_received）。 + +## 見てほしいこと +1. 読むだけになっているか（get_servo_currents / get_servo_status / get_gripper_value / get_servo_data が 2.9.6 で何かを動かす命令にならないか） +2. 今までの命令に影響がないか +3. `_read` の包みと外し方が安全か（例外のとき・2.9.6 の read の中身との組み合わせ） +4. 読めない項目があるとき 1 回の read_servo が最長どれだけかかるか（2.9.6 の read は返事がないと 0.1 秒待つ）。ゲームパッドと一緒に回すときの問題 +5. daemon_add_read_servo.sh の確かめ方・戻し方に抜けがないか + +指摘は「必須 / 推奨 / 据え置きでよい」に分けてください。ファイルは変えないでください。 diff --git a/docs/review_request_20261107_read_servo_r2.md b/docs/review_request_20261107_read_servo_r2.md new file mode 100644 index 0000000..d78eea7 --- /dev/null +++ b/docs/review_request_20261107_read_servo_r2.md @@ -0,0 +1,19 @@ +# コードレビュー依頼 R2：daemon の read_servo（2026-11-07 依頼 109） + +R1（docs/review_result_20261107_read_servo_codex.md）の指摘を反映した。前提・守りたいことは docs/review_request_20261107_read_servo.md と同じ（アームと爪に駆動の命令を送らない・今までの命令を変えない・daemon が止まらない・Python 3.6）。 + +## R1 の指摘への対応 +| # | 分類 | 指摘 | 反映 | 対応 | 次に見てほしい点 | +| --- | --- | --- | --- | --- | --- | +| 1 | 必須 | 始めの一括読み取り (32 個) が 3 秒を超えうる・その間ゲームパッドの stop も待つ | 反映 | servo_current_probe.py: 始めの 1 回を FIRST_CHUNK=4 個ずつの read_servo に分けた (1 回目 = 角度・電流・状態・爪、あとは番地 4 個ずつ × 7)。--with-pad のときは毎回読む数 (角度・電流・状態・爪・番地の合計) を PAD_MAX_READS=4 までにし、超えたら記録を作る前に止める。daemon の説明に待ちの目安を書いた | 分けた 1 回の最長 (4 × 約 0.2 秒) とゲームパッドの命令の待ちの見込みが妥当か | +| 2 | 必須 | 返事なしの [] / -1 を成功値として返す | 反映 | daemon: リストを返す関数 (angles・currents・status) は []・None、1 つの値の関数 (gripper・regs) は -1 なら value None・error "no reply"・returned に元の値。probe: やりとりの失敗 (n_error) と項目が読めなかった回 (n_item_error) を分けて数え、往復の時間は項目が読めなくても数える | -1 を「返事なし」とみなすこと (番地の値として -1 は来ない前提。2.9.6 の _process_single) | +| 3 | 推奨 | regs の int() 丸め | 反映 | 長さ 2 の list/tuple で、要素が int (bool は除く) のときだけ受ける。1.9・True・"1"・dict は ok=False で何も読まない | 抜けがないか | +| 4 | 推奨 | get_servo_data がないときの理由 | 反映 | regs_detail に error "get_servo_data is not available ..." | — | +| 5 | 推奨 | 導入の確かめで返事の形も見る | 反映 | daemon_add_read_servo.sh: items が 4 つそろうこと・角度が 6 個読めること (空なら 0.2 秒あけて 5 回まで頼み直す) を確かめ、電流・状態・爪の値は出すだけ (読めなくてもよい) | — | +| 6 | 推奨 | mv のあとに ssh が切れたときの戻し方・すでに新版のときの確かめ | 反映 | 構文の確かめと mv を分け、mv から先は失敗したら戻し方を出す。すでに新版なら置き直さず起動し直さず、返事の確かめだけ流す | — | +| 7 | 据え置き | 読むだけ・今までの命令・_read の戻し方・導入の基本手順・3.6 | — | R1 で問題なしとされた | — | + +テスト: tools/test_daemon_read_servo.py 12 件・tools/test_servo_current_probe.py 10 件 (どちらも通る)。 +差分: `git diff -- tools/pi/mecharm_daemon.py` と新しいファイル tools/daemon_add_read_servo.sh・tools/test_daemon_read_servo.py・tools/servo_current_probe.py・tools/test_servo_current_probe.py。 + +指摘は「必須 / 推奨 / 据え置きでよい」に分けてください。ファイルは変えないでください。 diff --git a/docs/review_request_20261107_read_servo_r3.md b/docs/review_request_20261107_read_servo_r3.md new file mode 100644 index 0000000..3a5683f --- /dev/null +++ b/docs/review_request_20261107_read_servo_r3.md @@ -0,0 +1,17 @@ +# コードレビュー依頼 R3：daemon の read_servo（2026-11-07 依頼 109） + +R2（docs/review_result_20261107_read_servo_codex_r2.md）の指摘を反映した。前提は docs/review_request_20261107_read_servo.md と同じ。 + +## R2 の指摘への対応 +| # | 分類 | 指摘 | 反映 | 対応 | 次に見てほしい点 | +| --- | --- | --- | --- | --- | --- | +| 1 | 必須 | 始めの 1 回を on_frame の中で読むと、その間 gamepad_teleop が停止 (L2+R2)・切断・窓を閉じるを見ない | 反映 | servo_current_probe.py run_with_pad: 始めの 1 回は gamepad_teleop.main を呼ぶ前に、vsp.get_daemon_socket() で作ったつながりで読む (鍵の中・パッドの命令には数えない)。gamepad_teleop.main は同じつながりをそのまま使う (get_daemon_socket はつながりがあればそれを返す)。読めなければつながりを閉じて終わる。on_frame は読み取りのスレッドを始めるだけ。テストで「偽物の main が呼ばれた時点で first_read.json があり、read_servo が 8 回済んでいる」を確かめる | 窓を開く前に読むことで抜けがないか (つながりの持ち主・終わるときの閉じ方) | +| 2 | 推奨 | regs={}・""・0・False が空として通る | 反映 | regs がない / None のときだけ空。それ以外は list/tuple でなければ ok=False (テストに足した) | — | +| 3 | 推奨 | -1 を返事なしと断定する前提 | 反映 | daemon の説明に「番地の値は 1 バイトで返る前提・この機体で未確認・ffff だと見分けられない・raw で確かめる」と書いた。始めの 1 回は raw=True でバイト列を残す | — | +| 4 | 推奨 | 「1 秒以内」は目安 | 反映 | 説明を「目安 0.4〜0.8 秒 (シリアルの待ちだけの見込み。上限の保証ではない)」にした | — | +| 5 | 据え置き | 正規化・集計・PAD_MAX_READS・導入スクリプト・読むだけ・既存命令・_read・3.6 | — | R2 で妥当とされた | — | + +テスト: tools/test_daemon_read_servo.py 12 件・tools/test_servo_current_probe.py 10 件 (通る)。 +差分: `git diff -- tools/pi/mecharm_daemon.py` と新しいファイル tools/daemon_add_read_servo.sh・tools/test_daemon_read_servo.py・tools/servo_current_probe.py・tools/test_servo_current_probe.py。gamepad_teleop.py の main・run_loop は tools/gamepad_teleop.py。 + +指摘は「必須 / 推奨 / 据え置きでよい」に分けてください。ファイルは変えないでください。 diff --git a/docs/review_result_20261107_read_servo_codex.md b/docs/review_result_20261107_read_servo_codex.md new file mode 100644 index 0000000..5a7d38d --- /dev/null +++ b/docs/review_result_20261107_read_servo_codex.md @@ -0,0 +1,100 @@ +# codex レビュー結果 R1：daemon の read_servo（2026-11-07 依頼 109） + +依頼文: docs/review_request_20261107_read_servo.md（codex exec -s read-only） + +**現状は、ゲームパッドとの併用前に修正が必要です。** 読み取り専用であること、既存コマンドの差分がないことは確認できました。ファイルは変更していません。 + +**必須** + +1. **一括読み取りが3秒タイムアウトを超えます。** + [servo_current_probe.py:380](/Users/shimazueiichi/r2r2r-scripts/tools/servo_current_probe.py:380) の初回読み取りは、通常項目4件＋レジスタ28件＝32回です。全件無応答なら約3.2秒＋処理時間となり、設定の3秒を超えます。初回記録前に終了する可能性があります。 + + また、[mecharm_daemon.py:144](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:144) は要求全体を中断せず処理するため、その間はゲームパッドの `stop` も待たされます。初回診断は操作開始前に行い、併用中は読み取り件数を制限するか、複数RPCに分割して操作指令を通せるようにしてください。タイムアウト延長だけでは操作遅延は解消しません。 + + 無応答時の目安は以下です。 + + | 要求 | 読み取り回数 | 待ち時間の目安 | + |---|---:|---:| + | 既定 | 2 | 約0.2秒 | + | 初回診断 | 32 | 約3.2秒 | + | 最大要求 | 36 | 約3.6秒 | + + **これは厳密な最長時間ではありません。** 2.9.6 の `read()` は期限確認後に最大0.1秒のシリアル読み取りへ入るため、部分受信では1呼び出しが約0.2秒近くまで伸び得ます。さらに `_write()` の `write/flush` には、この追加実装による時間制限がありません。 + +2. **「読めない」を成功値として返し、計測結果が誤解を招きます。** + [mecharm_daemon.py:97](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:97) は、ライブラリの戻り値をそのまま採用します。2.9.6 では無応答時に currents/status は `[]`、gripper/regs は `-1` になり得ますが、`error` が付きません。計測側もこれを正常な往復として集計します。 + + 項目ごとの無応答値を `value=None`＋`error` に正規化し、必要なら元の値を別フィールドに残してください。`ok=True` は維持できます。空電流 `[]` がエラーなしで返ることは再現しました。 + +**推奨** + +- **regsの入力を厳密に検証する。** + [mecharm_daemon.py:123](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:123) の `int()` により、`[[1.9, 60.9]]` が `[1, 60]` に変換され、実際に読み取りへ進みます。各要素を「長さ2の配列・整数・範囲内」として検証してください。 + +- **get_servo_data が存在しない場合も理由を返す。** + [mecharm_daemon.py:155](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:155) は `None` だけを返し、ほかの項目と違って関数未対応の `error` が残りません。 + +- **導入確認は `ok` に加えて返事の構造を確認する。** + [daemon_add_read_servo.sh:71](/Users/shimazueiichi/r2r2r-scripts/tools/daemon_add_read_servo.sh:71) は、全項目が例外でも成功扱いです。電流未対応は許容しつつ、期待した `items` が存在し、角度取得が成立していることなどを確認すると確実です。 + +- **入れ替え時の通信断にも復旧案内を出す。** + [daemon_add_read_servo.sh:53](/Users/shimazueiichi/r2r2r-scripts/tools/daemon_add_read_servo.sh:53) は、Piで `mv` 成功後にSSHが切れても「変えていません」と表示し得ます。この経路でも復旧コマンドを案内してください。既に新版だった場合の早期終了も、稼働確認は省略されています。 + +**据え置きでよい** + +- **読み取り専用性**：指定された2.9.6のソースでは、各関数は取得用プロトコル命令を送るだけで、駆動・設定関数は呼びません。「何も送らない」は厳密には「駆動命令を送らない」です。 +- **既存命令**：git差分は関数・定数と `read_servo` 分岐の追加だけで、既存分岐は変更されていません。 +- **`_read` の復元**：指定環境ではクラスに `_read = read` があるため、一時ラップ後に `del _mc._read` で戻す方式は成立します。取得関数内の例外でも `finally` が動きます。 +- **導入の基本手順**：旧版SHAはHEADと一致しました。バックアップ、旧版照合、Piでの構文確認、一時名からの入れ替えは妥当です。復旧は自動ではなく手動です。 +- **Python 3.6**：追加コードの構文互換性を確認しました。 + +検証はdaemonの偽物テスト10件が全件成功、シェル構文チェックも成功しました。Piへの接続・転送・実機操作は行っていません。計測側テストは一時ファイルを書き込むため、読み取り専用環境では実行していません。 +tokens used +56,597 +**現状は、ゲームパッドとの併用前に修正が必要です。** 読み取り専用であること、既存コマンドの差分がないことは確認できました。ファイルは変更していません。 + +**必須** + +1. **一括読み取りが3秒タイムアウトを超えます。** + [servo_current_probe.py:380](/Users/shimazueiichi/r2r2r-scripts/tools/servo_current_probe.py:380) の初回読み取りは、通常項目4件＋レジスタ28件＝32回です。全件無応答なら約3.2秒＋処理時間となり、設定の3秒を超えます。初回記録前に終了する可能性があります。 + + また、[mecharm_daemon.py:144](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:144) は要求全体を中断せず処理するため、その間はゲームパッドの `stop` も待たされます。初回診断は操作開始前に行い、併用中は読み取り件数を制限するか、複数RPCに分割して操作指令を通せるようにしてください。タイムアウト延長だけでは操作遅延は解消しません。 + + 無応答時の目安は以下です。 + + | 要求 | 読み取り回数 | 待ち時間の目安 | + |---|---:|---:| + | 既定 | 2 | 約0.2秒 | + | 初回診断 | 32 | 約3.2秒 | + | 最大要求 | 36 | 約3.6秒 | + + **これは厳密な最長時間ではありません。** 2.9.6 の `read()` は期限確認後に最大0.1秒のシリアル読み取りへ入るため、部分受信では1呼び出しが約0.2秒近くまで伸び得ます。さらに `_write()` の `write/flush` には、この追加実装による時間制限がありません。 + +2. **「読めない」を成功値として返し、計測結果が誤解を招きます。** + [mecharm_daemon.py:97](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:97) は、ライブラリの戻り値をそのまま採用します。2.9.6 では無応答時に currents/status は `[]`、gripper/regs は `-1` になり得ますが、`error` が付きません。計測側もこれを正常な往復として集計します。 + + 項目ごとの無応答値を `value=None`＋`error` に正規化し、必要なら元の値を別フィールドに残してください。`ok=True` は維持できます。空電流 `[]` がエラーなしで返ることは再現しました。 + +**推奨** + +- **regsの入力を厳密に検証する。** + [mecharm_daemon.py:123](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:123) の `int()` により、`[[1.9, 60.9]]` が `[1, 60]` に変換され、実際に読み取りへ進みます。各要素を「長さ2の配列・整数・範囲内」として検証してください。 + +- **get_servo_data が存在しない場合も理由を返す。** + [mecharm_daemon.py:155](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:155) は `None` だけを返し、ほかの項目と違って関数未対応の `error` が残りません。 + +- **導入確認は `ok` に加えて返事の構造を確認する。** + [daemon_add_read_servo.sh:71](/Users/shimazueiichi/r2r2r-scripts/tools/daemon_add_read_servo.sh:71) は、全項目が例外でも成功扱いです。電流未対応は許容しつつ、期待した `items` が存在し、角度取得が成立していることなどを確認すると確実です。 + +- **入れ替え時の通信断にも復旧案内を出す。** + [daemon_add_read_servo.sh:53](/Users/shimazueiichi/r2r2r-scripts/tools/daemon_add_read_servo.sh:53) は、Piで `mv` 成功後にSSHが切れても「変えていません」と表示し得ます。この経路でも復旧コマンドを案内してください。既に新版だった場合の早期終了も、稼働確認は省略されています。 + +**据え置きでよい** + +- **読み取り専用性**：指定された2.9.6のソースでは、各関数は取得用プロトコル命令を送るだけで、駆動・設定関数は呼びません。「何も送らない」は厳密には「駆動命令を送らない」です。 +- **既存命令**：git差分は関数・定数と `read_servo` 分岐の追加だけで、既存分岐は変更されていません。 +- **`_read` の復元**：指定環境ではクラスに `_read = read` があるため、一時ラップ後に `del _mc._read` で戻す方式は成立します。取得関数内の例外でも `finally` が動きます。 +- **導入の基本手順**：旧版SHAはHEADと一致しました。バックアップ、旧版照合、Piでの構文確認、一時名からの入れ替えは妥当です。復旧は自動ではなく手動です。 +- **Python 3.6**：追加コードの構文互換性を確認しました。 + +検証はdaemonの偽物テスト10件が全件成功、シェル構文チェックも成功しました。Piへの接続・転送・実機操作は行っていません。計測側テストは一時ファイルを書き込むため、読み取り専用環境では実行していません。 diff --git a/docs/review_result_20261107_read_servo_codex_r2.md b/docs/review_result_20261107_read_servo_codex_r2.md new file mode 100644 index 0000000..2e7787a --- /dev/null +++ b/docs/review_result_20261107_read_servo_codex_r2.md @@ -0,0 +1,43 @@ +# codex レビュー結果 R2：daemon の read_servo（2026-11-07 依頼 109） + +依頼文: docs/review_request_20261107_read_servo_r2.md（codex exec -s read-only） + +**R1の修正は概ね反映されていますが、初回診断中の停止入力について1点、修正が必要です。** ファイルは変更していません。 + +**必須** + +1. **初回読み取りの分割だけでは、ゲームパッドの停止入力を処理できません。** + + [servo_current_probe.py:530](/Users/shimazueiichi/r2r2r-scripts/tools/servo_current_probe.py:530) は、`on_frame()` の中で初回診断8回分を同期実行しています。[gamepad_teleop.py:1398](/Users/shimazueiichi/r2r2r-scripts/tools/gamepad_teleop.py:1398) は、そのコールバックが戻ってからイベントと停止トリガーを読みます。 + + このため、RPC単位では4項目に分かれていても、初回診断全体が終わるまで停止入力・切断・ウィンドウ終了を処理できません。全32項目が無応答なら約3.2秒、部分受信では約6.4秒近く＋処理時間になる見込みです。 + + 初回診断を操作受付開始前の準備として明確に分離するか、別スレッドまたはフレームごとの分割実行にしてください。現在のテストはRPCの件数だけを確認しており、この入力停止を検出できません。 + +**推奨** + +1. **空の不正な`regs`も検証してください。** + + [mecharm_daemon.py:127](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:127) の `cmd_obj.get("regs") or []` により、`{}`、`""`、`False`、`0` が空配列になります。実際に`regs={}`で`ok=True`となり、既定の角度・電流読み取りが実行されることを再現しました。 + + 「不正な指定なら何も読まずに`ok=False`」を満たすには、未指定・許容する`None`と、それ以外を分けて検証してください。各ペアの長さ・整数・bool除外・範囲の検証は妥当です。 + +2. **`-1`を無応答と断定できる条件を残してください。** + + [mecharm_daemon.py:173](/Users/shimazueiichi/r2r2r-scripts/tools/pi/mecharm_daemon.py:173) の処理について、2.9.6の`_process_single()`は空データを`-1`にします。一方、受信データが2バイトの`ffff`でも、符号付きデコードによって`-1`になります。ローカルの2.9.6ソースで再現しました。 + + 想定どおり1バイトの番地値なら`ff`は255になり、今回の扱いで問題ありません。ただし、ライブラリの実装だけでは「`-1`は必ず無応答」とは証明できません。実機の応答形式を確認するまでは、この前提を明記してください。 + +3. **「1秒以内」は保証ではなく目安にしてください。** + + [servo_current_probe.py:399](/Users/shimazueiichi/r2r2r-scripts/tools/servo_current_probe.py:399) の説明は断定的です。4項目で約0.8秒はシリアル受信待ちの目安であり、書き込み・flush・通信・OSの遅延を含む厳密な上限ではありません。通常計測でも、停止指令は進行中の読み取り完了を待ちます。 + +**据え置きでよい** + +- `[]`・`None`・`-1`の正規化と、通信失敗／項目失敗を分けた集計は、上記の前提内で妥当です。 +- `PAD_MAX_READS=4`を記録作成前に検証する配置は妥当です。 +- `get_servo_data`未対応時の理由が追加されています。 +- 導入スクリプトの返事の構造確認、角度取得の再試行、入れ替え後の復旧案内、既導入時の確認は、R1の指摘を反映しています。 +- 指定された2.9.6ソースでは取得用命令だけを呼びます。既存コマンドの分岐は変更されていません。指定環境での`_read`復元方式とPython 3.6構文互換性にも、新たな問題は見つかりませんでした。 + +検証はdaemonの偽物テスト**12件成功**、シェル構文チェック成功です。計測側テストは一時ファイルを作るため、読み取り専用環境では実行していません。Pi接続・転送・実機操作は行っていません。 diff --git a/docs/review_result_20261107_read_servo_codex_r3.md b/docs/review_result_20261107_read_servo_codex_r3.md new file mode 100644 index 0000000..b928903 --- /dev/null +++ b/docs/review_result_20261107_read_servo_codex_r3.md @@ -0,0 +1,32 @@ +# codex レビュー結果 R3：daemon の read_servo（2026-11-07 依頼 109） + +依頼文: docs/review_request_20261107_read_servo_r3.md（codex exec -s read-only） + +R2の必須指摘は解消されています。初回診断を操作受付前に移す対応は妥当です。**必須の追加指摘はありませんが、接続の後始末に推奨事項が1点あります。** ファイルは変更していません。 + +**必須** + +なし。 + +**推奨** + +1. **初回診断後、gamepad側へ接続を引き継ぐ前に終了した場合も後始末してください。** + + [servo_current_probe.py:531](/Users/shimazueiichi/r2r2r-scripts/tools/servo_current_probe.py:531) で開いた接続は、初回診断の失敗時には閉じられます。一方、診断成功後に`TermKeys.__enter__()`や、`gt.main()`の冒頭の記録フォルダ作成などで失敗すると、gamepad側の接続解放処理に到達しません。 + + また、初回診断の失敗時には、登録済みRPC observerと`pad_rpc.csv`の後始末も実行されません。通常の単独起動ではプロセス終了で回収されますが、同一プロセスから呼ぶ場合は接続やobserverが残ります。 + + `run_with_pad()`全体を覆う後始末を設け、引き継ぎ前の失敗でも接続・observer・CSVを解放することを推奨します。テストには「初回診断失敗」と「診断成功後、gamepad初期化前の失敗」を加えると確認できます。 + +**据え置きでよい** + +- 初回診断は`gt.main()`より前に完了し、`on_frame()`では計測スレッドを開始するため、R2の入力停止問題は解消されています。 +- `get_daemon_socket()`は既存接続を再利用するため、通常起動時の接続引き継ぎは妥当です。 +- 初回診断の失敗時に接続を閉じる処理と、診断をパッドのRPC集計から除外する処理は妥当です。 +- 不正な空の`regs`の拒否、`-1`の解釈に関する前提の明記、待ち時間を目安とする説明は、R2の指摘を反映しています。 +- daemonの既存コマンド分岐、読み取り専用の呼び出し、指定された2.9.6環境での`_read`復元方式について、追加の問題は見つかりませんでした。 + +検証はdaemonの偽物テスト**12件成功**、導入スクリプトのシェル構文チェック成功です。計測側10件は一時ファイルを作成するため、読み取り専用環境では実行していません。Piへの接続・転送・実機操作は行っていません。 + +## 反映（Claude Code） +- 推奨 1 を反映: run_with_pad の始めの 1 回から gamepad_teleop.main までを 1 つの try/finally で包み、どこで終わっても読み取りのスレッド・観察（rpc observer）・pad_rpc.csv を片付ける。gamepad_teleop の窓まで行かなかったときだけ、この道具がつながりを閉じる（窓まで行ったあとは gamepad_teleop.main の閉じ方のまま）。テスト test_cleanup_before_handover（始めの 1 回の失敗・gamepad_teleop の初めの失敗）を足した diff --git a/lab_train_fakes.sh b/lab_train_fakes.sh index 87a5d9f..8b8e861 100644 --- a/lab_train_fakes.sh +++ b/lab_train_fakes.sh @@ -69,13 +69,18 @@ POLL_SEC=${LAB_DRY_POLL_SEC:-1} NOTIFY_TITLE="$NOTIFY_TITLE (ドライラン)" # 偽物のチェックポイント (Pod 側の一覧と同じ中身を Hugging Face / scp で取ったことにする) -FAKE_CKPT_FILES=$'_CHECKPOINT_METADATA\t426\nassets/local/mecharm_redblock_box_teach_v1_trim2_corr_v3/norm_stats.json\t2120\nparams/_METADATA\t33052\nparams/d/0123abcd\t1048576\nparams/manifest.ocdbt\t118' -fake_make_ckpt() { # fake_make_ckpt フォルダ [ずらす] → FAKE_CKPT_FILES のとおりのファイルを作る +# assets の名前は作るモデルの学習データ (DATA_NAME. corr: ..._corr_v3 / corr2: ..._corr_v4b / corr3: ..._corr_v5). +# --stop は記録のモデルで use_model し直すので, 呼んだときの DATA_NAME で作る (依頼 109 D) +fake_ckpt_files() { + printf '_CHECKPOINT_METADATA\t426\nassets/local/%s/norm_stats.json\t2120\nparams/_METADATA\t33052\nparams/d/0123abcd\t1048576\nparams/manifest.ocdbt\t118\n' \ + "${DATA_NAME:-mecharm_redblock_box_teach_v1_trim2_corr_v3}" +} +fake_make_ckpt() { # fake_make_ckpt フォルダ [ずらす] → fake_ckpt_files のとおりのファイルを作る local d="$1" p n while IFS=$'\t' read -r p n; do [ "${2:-}" = shift ] && [ "$p" = params/manifest.ocdbt ] && n=$((n + 1)) mkdir -p "$d/$(dirname "$p")"; head -c "$n" /dev/zero > "$d/$p" - done <<<"$FAKE_CKPT_FILES" + done < <(fake_ckpt_files) } fake_n() { echo x >> "$LAB_DRY_ROOT/fake/$1.n"; wc -l < "$LAB_DRY_ROOT/fake/$1.n" | tr -d ' '; } @@ -138,11 +143,11 @@ pod_ssh() { esac ;; *": lt=list_ckpt"*|*": lt=ls_all"*) if [ "$LAB_FAKE_SCENARIO" = list_partial ] && [[ "$*" == *"lt=list_ckpt"* ]]; then - sed 's/^/F\t/' <<<"$FAKE_CKPT_FILES" | head -2; echo "find: './params/d': Input/output error"; return 1 + sed 's/^/F\t/' < <(fake_ckpt_files) | head -2; echo "find: './params/d': Input/output error"; return 1 fi if [ -f "$LAB_DRY_ROOT/fake/status.n" ] && [ "$LAB_FAKE_SCENARIO" != crash ] && [ "$LAB_FAKE_SCENARIO" != list_empty ] && [ "$LAB_FAKE_SCENARIO" != killed ] \ && [ "$LAB_FAKE_SCENARIO" != timeout ] && [ "$LAB_FAKE_SCENARIO" != stall ]; then - sed 's/^/F\t/' <<<"$FAKE_CKPT_FILES" + sed 's/^/F\t/' < <(fake_ckpt_files) fi ;; *"hf_upload_ckpt.py"*) if [ "$LAB_FAKE_SCENARIO" = hf_fail ]; then echo "HFUP error HfHubHTTPError: 401 Client Error"; return 6; fi diff --git a/logs/20261107_1420.md b/logs/20261107_1420.md new file mode 100644 index 0000000..77de8e4 --- /dev/null +++ b/logs/20261107_1420.md @@ -0,0 +1,38 @@ +# 2026-11-07 依頼 109：mechArm の関節と爪の電流が読めるかを確かめ、測る道具を作る + +## A. 読めるか（ソースで確かめた。アームは動かしていない） +Pi は今日つながらなかった（192.168.2.1:22 がタイムアウト）。なので、Pi と同じ pymycobot 2.9.6 を手元に取り寄せ（/tmp/pmc_x。読むだけ）、ソースで確かめた。あわせて 11-03 の servo_check の結果（diag/servo_check_20261103_071326）を見た。 + +- 関節の電流 `get_servo_currents()`（命令 0xE2）: 2.9.6 にある。説明は「mycobot 350 だけ」「0〜3250 mA」。返事の 12 バイトを 2 バイトずつ符号つきで読んで 6 個の数にする。返事がないと 0.1 秒待って `[]`。返事が 6 バイト（1 関節 1 バイト）の形なら、間違って 3 個の数になる。いまの pymycobot 4.0.5 の MechArm270 には、この関数がない（350 などにだけある）→ 270 のファームが答えるかは分からない。実機で確かめる +- 異常の印 `get_servo_status()`（0xE4）: 2.9.6 にある。[電圧, センサー, 温度, 電流, 角度, 過負荷]（0 = 正常）。11-03 はこの機体で `[0, 0, 0]`（6 バイトを 2 バイトずつ読んだ形） +- 同じ番号の仲間 get_servo_temps は 11-03 に `[6425, 5654, 6421]` = 0x1919・0x1616・0x1915。1 関節 1 バイトの 6 バイト（25,25,22,22,25,21 ℃）を 2.9.6 が 2 バイトずつ読み違えている。電流も 1 バイトずつで返ると読み違える → 道具では生のバイト列も残す +- `get_servo_data(id, 番地)`（0x53）: 1 つの番地を 1 回で読む。11-03 は番地 62 = 72〜74（0.1 V 単位で 7.4 V）・63 = 21〜26（℃）・65 = 0。Feetech STS の並びに合う → 番地 69/70（今の電流。STS3215 なら 1 = 6.5 mA）と 60/61（負荷。0.1% 単位・bit10 が向き）も読める見込み。爪は id 7（4.x の is_torque_gripper が get_servo_data(7, 1) を使う）。2.9.6 は 1 バイトずつしか読まないので、下位と上位を別々に読む +- 爪: 6 関節と同じ get_servo_currents には入らない（6 個だけ）。爪の電流は id 7 の番地 69/70 を読むしかない（未確認）。保護電流（GetGripperProtectCurrent 0x37。1〜500）は 4.x の MechArm270 にだけあり、2.9.6 にはない。0x37 は別の機種では JOG_INCREMENT_BASE_COORD（動かす命令）と同じ番号なので、生の 0x37 は送らない（決めてほしいこと） +- daemon（tools/pi/mecharm_daemon.py）には電流を読む命令がなかった → 読むだけの `read_servo` を足した（下） +- ぶつかり: daemon は 1 本ずつしかつながりを受けない（listen(1)・1 本を処理し終えるまで次を受けない）。別のプロセスから同時に読むことはできない。ゲームパッドと一緒に測るときは、同じプロセスで動かして鍵を共有する（run_policy_client の p と同じ形） +- 気づいたこと（直していない）: gamepad_teleop を単独で動かすと、送り口 `daemon_request` を鍵をかける前に読む（load_daemon_modules のあとに install_daemon_lock）。gamepad_teleop の send_angle・stop・get_angles は鍵を通らない。測る道具は、先に鍵をかけてから読ませるので、この問題はない + +## B. 作ったもの +- `tools/pi/mecharm_daemon.py`: `read_servo` を足した（純追記。消した行 0）。angles・currents は既定で読み、status・gripper・regs（[id, 番地] 32 個まで。id 1〜7・番地 0〜100）は頼んだときだけ読む。読み直さない。読めないときは value None・error（"no reply" は 2.9.6 が返す [] / -1）。raw=True で返事のバイト列 +- `tools/daemon_add_read_servo.sh`: Pi の daemon を持ち帰って KIOXIA の archive/ に写しを残す。11-05 の版（sha256 806336b6…）のときだけ置き換えて起動し直す。そのあと ping・get_angles・read_servo（角度が 6 個読めること）を確かめる。失敗したら戻すコマンドを出す +- `tools/servo_current_probe.py`: 読むだけ。始めに全部（電流・状態・爪の値・番地 sts の 28 個・バイト列）を 4 個ずつ 8 回で読んで first_read.json に残す。そのあと角度と電流を詰めて読み、samples.csv に書く（Mac の往復・Pi の中の時間・項目ごとの ms）。印はキー 1〜6・0、p で読むのを止める/再開。`--with-pad` は gamepad_teleop を同じプロセスで動かし、パッドの命令ごとに鍵の待ち（wait_ms）を pad_rpc.csv に書く（読んでいる間と止めた間を比べられる）。パッドと一緒のときは 1 回に読む数を 4 までにした。出力は `diag/servo_current_{日付}/{時刻}_{solo|pad}/`。KIOXIA がなければ止める +- テスト: tools/test_daemon_read_servo.py（12 件）・tools/test_servo_current_probe.py（11 件） +- codex: 3 回（docs/review_request_20261107_read_servo{,_r2,_r3}.md → docs/review_result_20261107_read_servo_codex{,_r2,_r3}.md）。R1 必須 2（始めの一括が 3 秒を超える・返事なしを成功の値にする）、R2 必須 1（始めの 1 回を窓の中で読むと停止を見ない）を反映。R3 は必須なし。推奨 1（窓まで行かずに終わったときの片付け）も反映した + +## 速さの見込み（実機では測っていない） +- 11-06 の記録（pi05_20261106_183926）: send_angle の rpc（送る + 角度を 1 回読む）0.032 秒・set_gripper_value（返事を読まない）0.013 秒 → 1 回読むのは 15〜20 ms ほど +- read_servo（角度 + 電流）: 両方答えれば 1 回 30〜45 ms → 20〜30 Hz。電流が答えなければ、毎回 0.1 秒待つ → 7 Hz ほど（そのときは --no-currents で番地を読む。爪の 69/70 なら角度 + 2 個で 15〜20 Hz の見込み） +- ゲームパッド: 命令は、読んでいる最中の 1 回が終わるまで鍵を待つ。最大で 1 回分（答えれば約 40 ms、答えない項目があれば 1 つにつき 0.1〜0.2 秒）。記録（run_policy_client の 10 Hz の角度）に足すなら、角度と電流を 1 回の read_servo で読めば、問い合わせの数は今と同じ + +## D. 学習のテストの偽物の assets +- lab_train_fakes.sh: 偽物のチェックポイントの assets の名前を、作るモデルの学習データ（DATA_NAME）にした（corr: ..._corr_v3 / corr2: ..._corr_v4b / corr3: ..._corr_v5）。--stop でモデルを読み直したあとの名前で作るよう、呼んだときに作る関数にした。本番の流れ（lab_train.sh・lab_lib.sh）は変えていない +- test_lab_train.sh: 12b・12c に assets の名前の確かめを 2 件足した → OK 216 / NG 0（足した 2 件を含む） + +## テスト +- test_lab_train.sh 216/0・test_lab_start.sh 153/0 +- pi05/test_*.py 9 本・test_provenance.py・tools/test_servo_check_pi.py・place_modules/tests 4 本・新しい 2 本: すべて OK +- tools/test_gamepad_teleop.py: 90 件中 1 件 NG（test_daemon_arm_step_not_moved_returns_and_resends の「1 秒より短く」が 1.03〜1.04 秒）。このテストが読むファイルには触っていない。HEAD の写しでも試そうとしたが、git にないファイル（auto_detect_cameras.py）がないので動かせなかった +- experiments/test_top_cam_dual_open.py は実機カメラを使う確かめなので流していない + +## 変えたファイル +tools/pi/mecharm_daemon.py・lab_train_fakes.sh・test_lab_train.sh（変更）、tools/daemon_add_read_servo.sh・tools/servo_current_probe.py・tools/test_daemon_read_servo.py・tools/test_servo_current_probe.py・docs/review_request_20261107_read_servo*.md（3）・docs/review_result_20261107_read_servo_codex*.md（3）・logs/20261107_1420.md（新規） diff --git a/test_lab_train.sh b/test_lab_train.sh index 4bf9c55..d0f84fd 100755 --- a/test_lab_train.sh +++ b/test_lab_train.sh @@ -204,6 +204,7 @@ P2="$TD/train_provenance.json" check "由来: 入力は corr_v4b と corr の 1499 (KIOXIA の原本・指紋・Hugging Face の元)" '[ "$(jq -r ".inputs[0].id" "$P2")" = mecharm_redblock_box_teach_v1_trim2_corr_v4b ] && [ "$(jq -r ".inputs[1] | [.kind,.id,.path,.hf_repo,.pod_path] | join(\" \")" "$P2")" = "checkpoint pi05_mecharm_trim_corr_lora_v1/1499 checkpoints/pi05_mecharm_trim_corr_lora_v1/1499 PixelstoMotors/pi05-mecharm-trim-corr-lora-v1 /workspace/ckpt/corr_v1_1499/params" ] && [ "$(jq -r ".inputs[1].fingerprint" "$P2")" = "$(python3 "$HERE/provenance.py" fingerprint "$T/data/checkpoints/pi05_mecharm_trim_corr_lora_v1/1499")" ]' check "由来: 設定・出力・チェックポイント側と Hugging Face にも同じもの" '[ "$(jq -r ".config | [.name,.exp_name,.init_params,.data_name] | join(\" \")" "$P2")" = "pi05_mecharm_trim_corr_v4_lora trim_corr_v4 /workspace/ckpt/corr_v1_1499/params mecharm_redblock_box_teach_v1_trim2_corr_v4b" ] && [ "$(jq -r ".outputs[0] | [.path,.hf_repo] | join(\" \")" "$P2")" = "checkpoints/pi05_mecharm_trim_corr_lora_v2/1499 PixelstoMotors/pi05-mecharm-trim-corr-lora-v2" ] && cmp -s "$P2" "$CK2/1499/train_provenance.json" && has calls.log "hf upload PixelstoMotors/pi05-mecharm-trim-corr-lora-v2 train_provenance.json"' check "由来: argv に --model corr2" 'jq -e ".producer.argv | index(\"corr2\")" "$P2" >/dev/null' +check "偽物のチェックポイントの assets は corr_v4b の名前 (依頼 109 D)" '[ -f "$CK2/1499/assets/local/mecharm_redblock_box_teach_v1_trim2_corr_v4b/norm_stats.json" ] && [ ! -e "$CK2/1499/assets/local/mecharm_redblock_box_teach_v1_trim2_corr_v3" ]' rm -rf "$T" T=$(mktemp -d); mkdir -p "$T/data/lab/LogT/train" "$T/fake" jq -n '{id:"dryrunpod0001", kind:"train", model:"corr2", config:"pi05_mecharm_trim_corr_v4_lora", gpu:"NVIDIA A40", costPerHr:0.44, created_epoch:1793449178, created_at:"2026-10-31 21:19:38", exp:"trim_corr_v4", train_started_epoch:1793449300}' > "$T/data/lab/LogT/train/lab_pod.json" @@ -226,6 +227,7 @@ check "記録 lab_pod.json に model・設定・exp・init" '[ "$(jq -r "[.model P3="$TD/train_provenance.json" check "由来: 入力は corr_v5 と corr2 の 1499 (KIOXIA の原本・指紋・Hugging Face の元)" '[ "$(jq -r ".inputs[0].id" "$P3")" = mecharm_redblock_box_teach_v1_trim2_corr_v5 ] && [ "$(jq -r ".inputs[1] | [.kind,.id,.path,.hf_repo,.pod_path] | join(\" \")" "$P3")" = "checkpoint pi05_mecharm_trim_corr_lora_v2/1499 checkpoints/pi05_mecharm_trim_corr_lora_v2/1499 PixelstoMotors/pi05-mecharm-trim-corr-lora-v2 /workspace/ckpt/corr_v2_1499/params" ] && [ "$(jq -r ".inputs[1].fingerprint" "$P3")" = "$(python3 "$HERE/provenance.py" fingerprint "$T/data/checkpoints/pi05_mecharm_trim_corr_lora_v2/1499")" ]' check "由来: 設定・出力・チェックポイント側と Hugging Face にも同じもの" '[ "$(jq -r ".config | [.name,.exp_name,.init_params,.data_name] | join(\" \")" "$P3")" = "pi05_mecharm_trim_corr_v5_lora trim_corr_v5 /workspace/ckpt/corr_v2_1499/params mecharm_redblock_box_teach_v1_trim2_corr_v5" ] && [ "$(jq -r ".outputs[0] | [.path,.hf_repo] | join(\" \")" "$P3")" = "checkpoints/pi05_mecharm_trim_corr_lora_v3/1499 PixelstoMotors/pi05-mecharm-trim-corr-lora-v3" ] && cmp -s "$P3" "$CK3/1499/train_provenance.json" && has calls.log "hf upload PixelstoMotors/pi05-mecharm-trim-corr-lora-v3 train_provenance.json"' +check "偽物のチェックポイントの assets は corr_v5 の名前 (依頼 109 D)" '[ -f "$CK3/1499/assets/local/mecharm_redblock_box_teach_v1_trim2_corr_v5/norm_stats.json" ] && [ ! -e "$CK3/1499/assets/local/mecharm_redblock_box_teach_v1_trim2_corr_v3" ]' rm -rf "$T" T=$(mktemp -d); mkdir -p "$T/data/lab/LogT/train" "$T/fake" jq -n '{id:"dryrunpod0001", kind:"train", model:"corr3", config:"pi05_mecharm_trim_corr_v5_lora", gpu:"NVIDIA A40", costPerHr:0.44, created_epoch:1793449178, created_at:"2026-10-31 21:19:38", exp:"trim_corr_v5", train_started_epoch:1793449300}' > "$T/data/lab/LogT/train/lab_pod.json" diff --git a/tools/daemon_add_read_servo.sh b/tools/daemon_add_read_servo.sh new file mode 100755 index 0000000..10462cf --- /dev/null +++ b/tools/daemon_add_read_servo.sh @@ -0,0 +1,95 @@ +#!/bin/bash +# daemon_add_read_servo.sh — Pi の mecharm_daemon.py に「read_servo」(電流などを読むだけ) を足した版を置いて起動し直す (2026-11-07 依頼 109) +# +# tools/daemon_add_read_servo.sh +# +# なぜ: 関節と爪の電流が読めるか・どれだけ速く読めるかを tools/servo_current_probe.py で測るため. +# 今の daemon には電流を読む命令がない. daemon がシリアルを握っているので, 別のプロセスからポートを開かない. +# 足す命令: {"cmd": "read_servo", ...} → get_angles / get_servo_currents / get_servo_status / get_gripper_value / +# get_servo_data を頼まれた分だけ 1 回ずつ呼ぶ (読むだけ. アームにも爪にも送らない). +# ほかの命令は 1 文字も変えない (unknown cmd の else の直前に足し, 関数を 2 つ足すだけ). +# 流れ: +# 1. Pi で今動いている /home/ubuntu/mecharm_daemon.py を Mac に持ち帰り, KIOXIA の archive/ に写しを残す +# 2. 持ち帰ったものが 11-05 の版 (tools/daemon_fast_read.sh のあと. sha256 806336b6…) なら tools/pi/mecharm_daemon.py を置く. +# すでに tools/pi/mecharm_daemon.py と同じなら置かない. どちらでもなければ止まる (Pi で誰かが変えている) +# 3. Pi へ一時名で送ってから入れ替え, systemd で起動し直し, ping・get_angles・read_servo が返事をするか確かめる +# うまくいかなければ, 元に戻すコマンドを出して止まる +# 流す前に: ゲームパッドなど daemon を使うものを止め, アームを HOME など安全な姿勢にしておく +# (daemon を起動し直すと MyCobot の初期化が走る) +set -u +PI=ubuntu@192.168.2.1 +KEY="$HOME/.ssh/id_rsa_pi" +SSH=(ssh -n -i "$KEY" -o ConnectTimeout=5 "$PI") +UNIT=mecharm-daemon.service +HERE="$(cd "$(dirname "$0")" && pwd)" +NEW="$HERE/pi/mecharm_daemon.py" +BEFORE_SHA=806336b6e55e3d4e6d7cf8941f88741cf28b604cef43addb941694e574212c81 # 11-05 の版 (c7992ca の tools/pi/mecharm_daemon.py) +DATA_ROOT=/Volumes/KIOXIA/r2r2r-data +[ -d "$DATA_ROOT/archive" ] || { echo "ABORT: $DATA_ROOT/archive がない (KIOXIA がマウントされていない?)"; exit 1; } +grep -q 'cmd == "read_servo"' "$NEW" || { echo "ABORT: $NEW に read_servo がない"; exit 1; } +python3 -c "import sys; compile(open(sys.argv[1], encoding='utf-8').read(), 'mecharm_daemon.py', 'exec')" "$NEW" || { echo "ABORT: $NEW の構文が通らない"; exit 1; } +STAMP=$(date +%Y%m%d_%H%M%S) +BK="$DATA_ROOT/archive/mecharm_daemon_premerge_readservo_$STAMP" +mkdir "$BK" || exit 1 +WORK=$(mktemp -d) + +scp -i "$KEY" -q -o ConnectTimeout=5 "$PI:/home/ubuntu/mecharm_daemon.py" "$WORK/before.py" || { echo "ABORT: Pi から持ち帰れない"; exit 1; } +cp "$WORK/before.py" "$BK/mecharm_daemon.py" || { echo "ABORT: 写しを残せない (Pi の daemon は変えていません)"; exit 1; } +echo "今の daemon の写し: $BK/mecharm_daemon.py" +SHA=$(shasum -a 256 "$WORK/before.py" | cut -d' ' -f1) +NEW_SHA=$(shasum -a 256 "$NEW" | cut -d' ' -f1) +RESTORE="scp -i $KEY $BK/mecharm_daemon.py $PI:/home/ubuntu/mecharm_daemon.py && ssh -i $KEY $PI sudo systemctl restart $UNIT" +fail() { echo "ABORT: $1"; echo "元に戻すには: $RESTORE"; exit 1; } +ALREADY=no +if [ "$SHA" = "$NEW_SHA" ]; then + echo "Pi の daemon はもう read_servo の版です。置き直さず, 起動し直さず, 返事だけ確かめます" + ALREADY=yes +fi +[ "$ALREADY" = yes ] || [ "$SHA" = "$BEFORE_SHA" ] || { echo "ABORT: Pi の daemon が 11-05 の版でも新しい版でもない (sha256 $SHA). Pi の daemon は変えていません"; exit 1; } +if [ "$ALREADY" = no ]; then +cp "$NEW" "$BK/mecharm_daemon_with_read_servo.py" || exit 1 +echo "--- 差分" +diff "$WORK/before.py" "$NEW" + +scp -i "$KEY" -q -o ConnectTimeout=5 "$NEW" "$PI:/home/ubuntu/mecharm_daemon.py.new" || { echo "ABORT: Pi へ送れない (Pi の daemon は変えていません)"; exit 1; } +"${SSH[@]}" "python3 -m py_compile /home/ubuntu/mecharm_daemon.py.new" \ + || { echo "ABORT: Pi で構文が通らない (Pi の daemon は変えていません)"; exit 1; } +# mv のあとに ssh が切れても入れ替わっているかもしれないので, ここからは戻し方を出す (codex R1 推奨) +"${SSH[@]}" "mv /home/ubuntu/mecharm_daemon.py.new /home/ubuntu/mecharm_daemon.py" \ + || fail "Pi で入れ替えられない (入れ替わったかは分からない. 起動し直していないので, 動いている daemon は前の版のまま)" +"${SSH[@]}" "sudo systemctl restart $UNIT" || fail "systemctl restart が失敗した" +UP=no +for i in 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15; do + sleep 2 + "${SSH[@]}" "ss -ltn | grep -q ':5555 '" && { UP=yes; break; } +done +[ "$UP" = yes ] || fail "30 秒たっても 5555 が開かない" +fi +python3 - <<'PY' +import json, socket +s = socket.create_connection(("192.168.2.1", 5555), timeout=5) +f = s.makefile("rwb") +import time +for c in ({"cmd": "ping"}, {"cmd": "get_angles"}, + {"cmd": "read_servo", "status": True, "gripper": True, "raw": True}): + for attempt in range(5): # read_servo は読み直さないので, 角度が空で返ったら 5 回まで頼み直す + f.write((json.dumps(c) + "\n").encode()); f.flush() + r = json.loads(f.readline().decode()) + a = (r.get("items", {}).get("angles") or {}).get("value") if c["cmd"] == "read_servo" else None + if c["cmd"] != "read_servo" or (isinstance(a, list) and len(a) == 6): + break + time.sleep(0.2) + print(c["cmd"], "→", r) + assert r.get("ok"), c["cmd"] + " が ok を返さない" + if c["cmd"] == "read_servo": + # 返事の形を確かめる (電流は読めなくてもよい. 読めるかを測るのがこの先の仕事. codex R1 推奨) + items = r.get("items", {}) + assert set(items) == {"angles", "currents", "status", "gripper_value"}, "read_servo の items が足りない" + a = items["angles"].get("value") + assert isinstance(a, list) and len(a) == 6, "read_servo で角度が読めない: " + repr(items["angles"]) + for k in ("currents", "status", "gripper_value"): + print(" ", k, "=", items[k].get("value"), "" if not items[k].get("error") else "(" + items[k]["error"] + ")") +s.close() +PY +[ $? = 0 ] || fail "ping か get_angles か read_servo が ok を返さない" +echo "できました。記録: $BK" diff --git a/tools/pi/mecharm_daemon.py b/tools/pi/mecharm_daemon.py index 6df4b30..2e9f172 100644 --- a/tools/pi/mecharm_daemon.py +++ b/tools/pi/mecharm_daemon.py @@ -64,6 +64,127 @@ def read_angles_once_for_log(): return a if isinstance(a, (list, tuple)) and len(a) == 6 else None +# ---- 2026-11-07 依頼 109: 電流などを読むだけの命令 read_servo (純追記) ---------------------- +# アームと爪には何も送らない. pymycobot の読む関数 (get_angles / get_servo_currents / get_servo_status / +# get_gripper_value / get_servo_data) だけを, 頼まれた順に 1 回ずつ呼ぶ. 読み直さない・待たない. +# 読めなかった項目は値 None と error を返す (ok=True のまま. get_angles_once と同じ. 呼び出し側が例外にしないため). +# 2.9.6 は返事がないとき 例外にせず, リストを返す関数は [] を, 1 つの値を返す関数は -1 を返す. +# これも読めなかったとして value None・error "no reply"・returned (返ってきた値そのまま) にする (codex R1 必須 2). +# 前提 (この機体で確かめていない. codex R2): 番地の値は 1 バイトで返る (ff は 255 になり -1 にならない). +# 2 バイトの ffff が返る形だと符号つきで -1 になり, 返事なしと見分けられない. raw=True のバイト列で確かめる. +# 2.9.6 の read は返事がないと 1 回 0.1 秒ほど待つ (途中まで返ると 0.2 秒近く). 頼む数を増やすと, その分 +# daemon がほかの命令 (ゲームパッドの stop も) を受けるのが遅れる. 呼び出し側で数を絞る (codex R1 必須 1). +# 頼み方がおかしいとき (regs の形・番号の範囲) だけ ok=False で, そのときは何も読まない. +# {"cmd": "read_servo", "angles": true, "currents": true, "status": false, "gripper": false, +# "regs": [[servo_id, addr], ...], "raw": false} +# angles / currents は既定で読む. status / gripper / regs は頼んだときだけ. +# regs: get_servo_data(servo_id, addr) を 1 つずつ (servo_id 1〜7. 7 = 爪. addr 0〜100. 32 個まで). +# この版の get_servo_data は番地を 1 つ渡すだけなので, 2 バイトの値は下位と上位の番地を別々に頼む. +# raw: 返ってきたバイト列 (16 進) も返す (形の分からない返事を確かめる用). +# 返事: {"ok": true, "t": Pi の時刻, "total_ms": ..., "items": {名前: {"value": ..., "ms": ..., ("error"), ("raw")}}, +# "regs": [[servo_id, addr, value, ms], ...]} +SERVO_READ_MAX_REGS = 32 + + +def _servo_read_one(fn, args, want_raw, no_reply=None): + """読む関数を 1 回だけ呼ぶ. 返事のバイト列は _read を一時的に包んで写す (daemon は 1 件ずつなので混ざらない). + no_reply: 2.9.6 が返事のないときに返す値 ([] か -1). これが返ったら value None・error にする.""" + item = {} + cap = [] + if want_raw: + orig = _mc._read + + def _cap_read(*a, **k): + d = orig(*a, **k) + cap.append(d) + return d + _mc._read = _cap_read + t0 = time.perf_counter() + try: + v = fn(*args) + if isinstance(v, tuple): + v = list(v) + if v is None or (no_reply is not None and v == no_reply): + item["value"] = None + item["error"] = "no reply" + item["returned"] = v + else: + item["value"] = v + except Exception as e: + item["value"] = None + item["error"] = f"{type(e).__name__}: {e}" + finally: + item["ms"] = round((time.perf_counter() - t0) * 1000.0, 2) + if want_raw: + del _mc._read # インスタンスに置いた包みを外す (クラスの _read に戻る) + item["raw"] = [d.hex() if isinstance(d, (bytes, bytearray)) else repr(d) for d in cap] + try: + json.dumps(item["value"]) + except (TypeError, ValueError): + item["value"] = repr(item["value"])[:200] + return item + + +def handle_servo_read(cmd_obj): + regs = cmd_obj.get("regs") + if regs is None: # 頼まないのは「ない」か None だけ ({}・""・0 は形がおかしいとして断る. codex R2) + regs = [] + if not isinstance(regs, (list, tuple)) or len(regs) > SERVO_READ_MAX_REGS: + return {"ok": False, "error": f"regs must be a list of at most {SERVO_READ_MAX_REGS} [servo_id, addr]"} + pairs = [] + for r in regs: + try: + if not isinstance(r, (list, tuple)) or len(r) != 2: + raise ValueError("length") + # 整数だけ (1.9 を 1 に丸めて読まない. True も数にしない. codex R1 推奨) + if not all(isinstance(x, int) and not isinstance(x, bool) for x in r): + raise TypeError("not int") + sid, addr = r + except (TypeError, ValueError): + return {"ok": False, "error": f"bad reg: {r!r}"} + if not (1 <= sid <= 7) or not (0 <= addr <= 100): + return {"ok": False, "error": f"reg out of range (servo_id 1..7, addr 0..100): {r!r}"} + pairs.append((sid, addr)) + want_raw = bool(cmd_obj.get("raw", False)) + plan = [] + if cmd_obj.get("angles", True): + plan.append(("angles", "get_angles", [])) + if cmd_obj.get("currents", True): + plan.append(("currents", "get_servo_currents", [])) + if cmd_obj.get("status", False): + plan.append(("status", "get_servo_status", [])) + if cmd_obj.get("gripper", False): + plan.append(("gripper_value", "get_gripper_value", -1)) + t_start = time.perf_counter() + t_wall = time.time() + items = {} + for key, name, no_reply in plan: + fn = getattr(_mc, name, None) + if fn is None: + items[key] = {"value": None, "ms": 0.0, "error": f"{name} is not available in this pymycobot environment"} + continue + items[key] = _servo_read_one(fn, (), want_raw, no_reply) + out_regs = [] + raw_regs = [] + if pairs: + fn = getattr(_mc, "get_servo_data", None) + for sid, addr in pairs: + if fn is None: + out_regs.append([sid, addr, None, 0.0]) + raw_regs.append({"servo_id": sid, "addr": addr, "raw": None, + "error": "get_servo_data is not available in this pymycobot environment"}) + continue + it = _servo_read_one(fn, (sid, addr), want_raw, -1) + out_regs.append([sid, addr, it["value"], it["ms"]]) + if want_raw or "error" in it: + raw_regs.append({"servo_id": sid, "addr": addr, "raw": it.get("raw"), "error": it.get("error")}) + resp = {"ok": True, "t": t_wall, "total_ms": round((time.perf_counter() - t_start) * 1000.0, 2), + "items": items, "regs": out_regs} + if raw_regs: + resp["regs_detail"] = raw_regs + return resp + + def handle_command(cmd_obj): """コマンドを処理してレスポンス dict を返す。""" global _fresh_mode_state @@ -259,6 +380,10 @@ def handle_command(cmd_obj): return {"ok": False, "error": f"stop failed: {res}"} return {"ok": True, "result": res} + # 2026-11-07 依頼 109: 関節と爪の電流を読む (読むだけ. 純追記). handle_servo_read の説明を参照. + elif cmd == "read_servo": + return handle_servo_read(cmd_obj) + else: return {"ok": False, "error": f"unknown cmd: {cmd}"} diff --git a/tools/servo_current_probe.py b/tools/servo_current_probe.py new file mode 100755 index 0000000..4ee2c50 --- /dev/null +++ b/tools/servo_current_probe.py @@ -0,0 +1,626 @@ +#!/usr/bin/env python3 +"""servo_current_probe.py — mechArm の関節と爪の電流 (と同じ時刻の角度) を読める限り速く読み, CSV に残す (依頼 109. 2026-11-07) + +アームと爪には何も送らない. daemon の read_servo (tools/daemon_add_read_servo.sh で足す. 読むだけ) を繰り返し呼ぶだけ. +daemon は 1 本ずつしかつながりを受けないので, ゲームパッドと一緒に測るときは --with-pad で +tools/gamepad_teleop.py をこのプロセスの中で動かす (run_policy_client の p と同じ形. 送り方・守りは gamepad_teleop のまま). +そのときは daemon とのやりとりに 1 つの鍵をかけ (policy_recorder.install_daemon_lock), ゲームパッドの命令が +鍵を待った時間 (wait_ms) とやりとりの長さ (rpc_ms) を pad_rpc.csv に残す. p キーで読むのを止める/再開でき, +読んでいる間と止めている間の wait_ms を比べられる. + + python3 tools/servo_current_probe.py --seconds 10 # 静止で 10 秒だけ読む (アームは手で触らない) + python3 tools/servo_current_probe.py # q で終わるまで読む. 手でアームを押すなど + python3 tools/servo_current_probe.py --with-pad # ゲームパッドで動かしながら読む (ウィンドウを閉じると終わる) + python3 tools/servo_current_probe.py --regs sts # 電流の関数が読めないとき: サーボの番地 69/70 (電流) と 60/61 (負荷) も読む + +印 (ターミナルで 1 文字. --with-pad のときはゲームパッドの窓が手前でも受け付ける): + 1 静止 2 爪を空で閉じた 3 つかんだ 4 引っ張った 5 離した 6 アームを押した 0 そのほか + p 読むのを止める/再開 q 終わる (--with-pad のときはウィンドウを閉じる) + +始めに 1 回だけ, 読めるものを全部 (電流・状態・爪の値・サーボの番地 sts の 28 個・返事のバイト列) 読んで +first_read.json に残し, 画面に出す. 電流の関数が返す形はこの機体で確かめていない (2.9.6 の説明は「mycobot 350 だけ」). +サーボの番地 (60/61 負荷・69/70 電流) は Feetech STS 想定の値で, この機体で確かめていない. 生の値をそのまま残す. + +出力: /Volumes/KIOXIA/r2r2r-data/diag/servo_current_{YYYYMMDD}/{HHMMSS}_{solo|pad}/ + samples.csv 1 回の読み取りごとに 1 行 (Mac の時刻・往復・Pi の中の時間・角度・電流・印) + marks.csv 印 (時刻・キー・名前) + pad_rpc.csv (--with-pad) ゲームパッドの命令 1 つごとに 鍵の待ち・やりとりの長さ・読むのを止めていたか + first_read.json 始めの 1 回 / summary.json 回数・Hz・往復の中央値など / servo_current_probe.py (使った写し) + ゲームパッドの押した記録は今までどおり runs/experiments/gamepad_teleop/teleop_*/ (summary.json に場所を書く) +""" +import argparse +import csv +import json +import os +import queue +import select +import shutil +import socket +import statistics +import sys +import threading +import time +from datetime import datetime +from pathlib import Path + +DAEMON_HOST = "192.168.2.1" +DAEMON_PORT = 5555 +RPC_TIMEOUT_SEC = 3.0 +SSD_MOUNT = "/Volumes/KIOXIA" +OUT_ROOT = Path(SSD_MOUNT) / "r2r2r-data" / "diag" +GAP_SOLO_SEC = 0.0 # 読み取りの間にあける時間 (ひとりのときは詰めて読む) +GAP_PAD_SEC = 0.01 # ゲームパッドと一緒のとき. 鍵を手放してすぐ取り直さないよう少しあける +STS_REGS = [(sid, addr) for sid in range(1, 8) for addr in (60, 61, 69, 70)] # 7 = 爪 +# 1 回の read_servo で読む数の上限. 2.9.6 は返事がないと 1 つにつき 0.1〜0.2 秒待ち, その間 daemon は +# ゲームパッドの命令 (stop も) を受けない (codex R1 必須 1). 始めの 1 回もこの数ずつに分けて頼む. +FIRST_CHUNK = 4 +PAD_MAX_READS = 4 # --with-pad のとき 毎回読む数 (角度・電流・状態・爪・番地の合計) の上限 +MARKS = {"1": "static", "2": "close_empty", "3": "grasp", "4": "pull", "5": "release", "6": "push_arm", "0": "other"} +SUMMARY_EVERY_SEC = 2.0 + + +def ensure_ssd_mounted(): + if not os.path.ismount(SSD_MOUNT): + sys.exit("[ABORT] KIOXIA がマウントされていません。記録をローカルには逃がさず、ここで止めます。") + + +# ============================================================================= +# daemon とのやりとり +# ============================================================================= + +class SoloClient: + """ひとりで daemon につなぐ (ゲームパッドなど, ほかに daemon を使うものを止めてから).""" + + def __init__(self, host=DAEMON_HOST, port=DAEMON_PORT, timeout=RPC_TIMEOUT_SEC): + self.sock = socket.create_connection((host, port), timeout=timeout) + self.sock.settimeout(timeout) + self.buf = b"" + + def request(self, obj): + self.sock.sendall((json.dumps(obj) + "\n").encode("utf-8")) + while b"\n" not in self.buf: + d = self.sock.recv(65536) + if not d: + raise RuntimeError("daemon がつながりを閉じた") + self.buf += d + line, self.buf = self.buf.split(b"\n", 1) + return json.loads(line.decode("utf-8")) + + def close(self): + try: + self.sock.close() + except Exception: + pass + + +def read_cmd(currents=True, status=False, gripper=False, regs=(), raw=False): + return {"cmd": "read_servo", "angles": True, "currents": bool(currents), "status": bool(status), + "gripper": bool(gripper), "regs": [list(r) for r in regs], "raw": bool(raw)} + + +def check_first(resp): + """始めの 1 回の返事を確かめる. daemon に read_servo がなければ止める (何も送っていない).""" + if not isinstance(resp, dict): + sys.exit(f"[ABORT] daemon の返事の形がおかしい: {resp!r}") + if not resp.get("ok"): + err = str(resp.get("error", "")) + if "unknown cmd" in err: + sys.exit("[ABORT] daemon に read_servo がありません。先に tools/daemon_add_read_servo.sh を流してください") + sys.exit(f"[ABORT] read_servo が ok を返しません: {err}") + + +def describe_first(resp): + """始めの 1 回を人が読める形にする (画面と first_read.json の notes).""" + lines = [] + items = resp.get("items", {}) + for k in ("angles", "currents", "status", "gripper_value"): + it = items.get(k) + if it is None: + continue + s = f"{k:14s} {it.get('value')!r} ({it.get('ms')} ms)" + if it.get("error"): + s += f" error={it['error']}" + if it.get("raw"): + s += f" raw={it['raw']}" + lines.append(s) + regs = resp.get("regs") or [] + by_sid = {} + for sid, addr, val, ms in regs: + by_sid.setdefault(sid, []).append(f"{addr}:{'返事なし' if val is None else val}") + for sid in sorted(by_sid): + name = "爪" if sid == 7 else f"j{sid - 1}" + lines.append(f"servo {sid} ({name}) " + " ".join(by_sid[sid])) + if regs: + lines.append(f"番地 1 つの読み取り: 中央値 {statistics.median(r[3] for r in regs):.1f} ms") + lines.append(f"Pi の中の合計 {resp.get('total_ms')} ms") + return lines + + +# ============================================================================= +# 読み取りのスレッド +# ============================================================================= + +def flat_currents(v): + """電流の値を 6 列に. 6 個の数のリストでなければ None (生の形は currents_raw 列に残す).""" + if isinstance(v, list) and len(v) == 6 and all(isinstance(x, (int, float)) for x in v): + return v + return None + + +SAMPLE_COLS = (["i", "t_mac_send", "t_mac_recv", "rtt_ms", "wait_ms", "t_pi", "pi_total_ms", + "angles_ms", "currents_ms"] + + [f"j{k}" for k in range(6)] + [f"cur{k}" for k in range(6)] + + ["currents_raw", "gripper_value", "status", "regs", "error", "mark"]) + + +class Prober: + def __init__(self, request_fn, out_dir, cmd, gap_sec, out=print, wait_of_last=None): + self.request_fn = request_fn + self.cmd = cmd + self.gap = gap_sec + self.out = out + self.wait_of_last = wait_of_last # --with-pad: 直前のやりとりで鍵を待った秒 (同じスレッド) + self.stop_flag = threading.Event() + self.paused = threading.Event() + self.marks = queue.Queue() + self.mark_lock = threading.Lock() # ターミナルとゲームパッドの窓の両方から呼ばれる + self.n = 0 + self.n_err = 0 # やりとりそのものの失敗 (つながらない・ok=False) + self.n_item_err = 0 # やりとりはできたが, どれかの項目が読めなかった (返事なしなど) + self.n_cur_ok = 0 + self.rtts = [] + self.pi_totals = [] + self.item_ms = {"angles": [], "currents": []} + self.t_first = self.t_last = None + self.f = open(Path(out_dir) / "samples.csv", "x", newline="", encoding="utf-8") + self.w = csv.writer(self.f) + self.w.writerow(SAMPLE_COLS) + self.mf = open(Path(out_dir) / "marks.csv", "x", newline="", encoding="utf-8") + self.mw = csv.writer(self.mf) + self.mw.writerow(["t_mac", "key", "name", "sample_i"]) + self.thread = threading.Thread(target=self._loop, name="probe", daemon=True) + + def mark(self, key): + name = MARKS.get(key) + if name is None: + return False + t = time.time() + with self.mark_lock: + if self.mf.closed: + return False + self.mw.writerow([f"{t:.6f}", key, name, self.n]) + self.mf.flush() + self.marks.put(name) + self.out(f"[印] {key} {name} ({datetime.fromtimestamp(t).strftime('%H:%M:%S.%f')[:-3]})") + return True + + def toggle_pause(self): + if self.paused.is_set(): + self.paused.clear() + self.out("[再開] 読みます") + else: + self.paused.set() + self.out("[止めた] 読むのを止めています (p で再開)") + + def start(self): + self.thread.start() + + def stop(self, join_sec=5.0): + self.stop_flag.set() + if self.thread.is_alive(): + self.thread.join(join_sec) + with self.mark_lock: + for fh in (self.f, self.mf): + try: + fh.close() + except Exception: + pass + + def _loop(self): + last_print = time.time() + while not self.stop_flag.is_set(): + if self.paused.is_set(): + time.sleep(0.05) + continue + mark = [] + while not self.marks.empty(): + mark.append(self.marks.get_nowait()) + t0 = time.time() + resp, err = None, "" + try: + resp = self.request_fn(self.cmd) + except Exception as e: + err = f"{type(e).__name__}: {e}" + t1 = time.time() + wait = self.wait_of_last() if self.wait_of_last is not None else None + self.write_row(t0, t1, wait, resp, err, mark) + if t1 - last_print >= SUMMARY_EVERY_SEC: + last_print = t1 + self.out(self.status_line()) + if self.gap > 0: + time.sleep(self.gap) + + def write_row(self, t0, t1, wait, resp, err, mark): + items = (resp or {}).get("items", {}) if isinstance(resp, dict) else {} + ang = (items.get("angles") or {}).get("value") + cur_it = items.get("currents") or {} + cur = flat_currents(cur_it.get("value")) + if not err and isinstance(resp, dict) and not resp.get("ok"): + err = str(resp.get("error")) + if not err and not isinstance(resp, dict): + err = f"bad response: {resp!r}"[:200] + transport_err = bool(err) + errs = [f"{k}:{v['error']}" for k, v in items.items() if isinstance(v, dict) and v.get("error")] + regs_none = sum(1 for r in ((resp or {}).get("regs") or []) if isinstance(resp, dict) and r[2] is None) + if regs_none: + errs.append(f"regs:{regs_none} no reply") + if errs: + err = (err + " " if err else "") + " ".join(errs) + rtt = (t1 - t0) * 1000.0 + row = [self.n, f"{t0:.6f}", f"{t1:.6f}", f"{rtt:.2f}", "" if wait is None else f"{wait * 1000.0:.2f}", + f"{resp['t']:.6f}" if isinstance(resp, dict) and isinstance(resp.get("t"), (int, float)) else "", + (resp or {}).get("total_ms", "") if isinstance(resp, dict) else "", + (items.get("angles") or {}).get("ms", ""), cur_it.get("ms", "")] + row += ([f"{float(v):.2f}" for v in ang] if isinstance(ang, list) and len(ang) == 6 else [""] * 6) + row += (list(cur) if cur is not None else [""] * 6) + row += ["" if cur is not None or "value" not in cur_it else json.dumps(cur_it.get("value")), + json.dumps((items.get("gripper_value") or {}).get("value")) if "gripper_value" in items else "", + json.dumps((items.get("status") or {}).get("value")) if "status" in items else "", + json.dumps((resp or {}).get("regs")) if isinstance(resp, dict) and resp.get("regs") else "", + err, "+".join(mark)] + self.w.writerow(row) + if self.n % 20 == 0: + self.f.flush() + self.n += 1 + self.t_first = self.t_first or t0 + self.t_last = t1 + if transport_err: + self.n_err += 1 + else: + if errs: + self.n_item_err += 1 + self.rtts.append(rtt) + if isinstance(resp.get("total_ms"), (int, float)): + self.pi_totals.append(resp["total_ms"]) + for k in self.item_ms: + ms = (items.get(k) or {}).get("ms") + if isinstance(ms, (int, float)): + self.item_ms[k].append(ms) + if cur is not None: + self.n_cur_ok += 1 + + def hz(self): + if self.n < 2 or self.t_last is None: + return 0.0 + return (self.n - 1) / max(self.t_last - self.t_first, 1e-9) + + def status_line(self): + med = statistics.median(self.rtts) if self.rtts else float("nan") + return (f"[読み] {self.n} 回 {self.hz():.1f} Hz 往復の中央値 {med:.1f} ms " + f"電流が 6 個の数で読めた {self.n_cur_ok} 回 項目が読めなかった {self.n_item_err} 回 " + f"やりとりの失敗 {self.n_err} 回") + + def summary(self): + def stats(xs): + if not xs: + return None + s = sorted(xs) + return {"n": len(s), "median": round(statistics.median(s), 2), "p95": round(s[int(0.95 * (len(s) - 1))], 2), + "max": round(s[-1], 2), "min": round(s[0], 2)} + return {"n": self.n, "n_error": self.n_err, "n_item_error": self.n_item_err, "n_currents_6": self.n_cur_ok, "hz": round(self.hz(), 2), + "rtt_ms": stats(self.rtts), "pi_total_ms": stats(self.pi_totals), + "item_ms": {k: stats(v) for k, v in self.item_ms.items()}} + + +# ============================================================================= +# ターミナルの 1 文字 +# ============================================================================= + +class TermKeys: + """ターミナルを 1 文字ずつ読む形にする (Ctrl+C は効くまま). 端末でなければ何もしない.""" + + def __init__(self): + self.ok = sys.stdin.isatty() + self.old = None + + def __enter__(self): + if self.ok: + import termios + import tty + self.old = termios.tcgetattr(sys.stdin) + tty.setcbreak(sys.stdin.fileno()) + return self + + def __exit__(self, *a): + if self.old is not None: + import termios + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old) + + def get(self, timeout): + if not self.ok: + time.sleep(timeout) + return None + r, _, _ = select.select([sys.stdin], [], [], timeout) + return sys.stdin.read(1) if r else None + + +def key_thread(keys, prober, quit_ev, allow_quit): + while not quit_ev.is_set(): + c = keys.get(0.1) + if c is None: + continue + c = c.lower() + if c == "q" and allow_quit: + quit_ev.set() + elif c == "p": + prober.toggle_pause() + else: + prober.mark(c) + + +# ============================================================================= +# main +# ============================================================================= + +def parse_regs(s): + if s in (None, "", "none"): + return [] + if s == "sts": + return list(STS_REGS) + out = [] + for part in s.split(","): + sid, addr = part.split(":") + out.append((int(sid), int(addr))) + return out + + +def make_out_dir(mode): + ensure_ssd_mounted() + now = datetime.now() + d = OUT_ROOT / f"servo_current_{now.strftime('%Y%m%d')}" / f"{now.strftime('%H%M%S')}_{mode}" + if d.exists(): + sys.exit(f"[ABORT] 同じ名前のフォルダがあります: {d}") + d.mkdir(parents=True) + shutil.copy2(__file__, d / Path(__file__).name) + return d + + +def write_json(path, obj): + tmp = Path(str(path) + ".tmp") + tmp.write_text(json.dumps(obj, ensure_ascii=False, indent=1), encoding="utf-8") + os.replace(tmp, path) + + +def n_reads(cmd): + return sum(1 for k in ("angles", "currents", "status", "gripper") if cmd.get(k)) + len(cmd.get("regs") or []) + + +def first_read(request_fn, out_dir): + """読めるものを全部 1 回ずつ. 1 回の頼みは FIRST_CHUNK 個までに分ける. + 返事がないときの目安は 1 回 0.4〜0.8 秒 (シリアルの待ちだけの見込み. 上限の保証ではない). + --with-pad のときは, ゲームパッドの窓を開く前 (操作を受け付ける前) に呼ぶ (codex R2 必須).""" + cmds = [read_cmd(currents=True, status=True, gripper=True, regs=(), raw=True)] + for i in range(0, len(STS_REGS), FIRST_CHUNK): + c = read_cmd(currents=False, regs=STS_REGS[i:i + FIRST_CHUNK], raw=True) + c["angles"] = False + cmds.append(c) + t0 = time.time() + resps = [] + for c in cmds: + r = request_fn(c) + check_first(r) + resps.append(r) + t1 = time.time() + resp = {"ok": True, "t": resps[0].get("t"), "items": resps[0].get("items", {}), + "total_ms": round(sum(r.get("total_ms") or 0 for r in resps), 2), + "regs": [x for r in resps for x in (r.get("regs") or [])], + "regs_detail": [x for r in resps for x in (r.get("regs_detail") or [])]} + notes = describe_first(resp) + print("---- 始めの 1 回 (読めるものを全部)") + for ln in notes: + print(" " + ln) + print(f" 往復 {1000 * (t1 - t0):.1f} ms") + write_json(out_dir / "first_read.json", {"cmds": cmds, "resps": resps, "rtt_ms": round(1000 * (t1 - t0), 2), + "notes": notes}) + return resp + + +def run_solo(args, out_dir, cmd, meta): + try: + cli = SoloClient() + except OSError as e: + sys.exit(f"[ABORT] daemon につながりません ({e}). daemon が動いているか, ゲームパッドなどほかに " + "daemon を使うものが動いていないか確かめてください") + try: + first_read(cli.request, out_dir) + prober = Prober(cli.request, out_dir, cmd, args.gap if args.gap is not None else GAP_SOLO_SEC) + quit_ev = threading.Event() + print("読み始めます。印: 1 静止 2 空で閉じた 3 つかんだ 4 引っ張った 5 離した 6 押した 0 そのほか / p 止める・再開 / q 終わる") + prober.start() + end = None + with TermKeys() as keys: + kt = threading.Thread(target=key_thread, args=(keys, prober, quit_ev, True), daemon=True) + kt.start() + t_end = time.time() + args.seconds if args.seconds else None + try: + while not quit_ev.is_set(): + if t_end is not None and time.time() >= t_end: + end = "seconds" + break + if not prober.thread.is_alive(): + end = "probe_died" + break + time.sleep(0.05) + end = end or "q" + except KeyboardInterrupt: + end = "ctrl_c" + quit_ev.set() + prober.stop() + kt.join(1.0) + meta["end_reason"] = end + return prober, None + finally: + cli.close() + + +def run_with_pad(args, out_dir, cmd, meta): + """gamepad_teleop をこのプロセスで動かす. 鍵をかけてから gamepad_teleop の送り口を読む + (gamepad_teleop は読んだときの _daemon_request を持つので, 先に鍵の版にしておく).""" + sys.path.insert(0, str(Path(__file__).resolve().parent)) + import gamepad_teleop as gt + import run_policy_client as rpcmod # gamepad_teleop が pi05 をパスに入れる. pymycobot のスタブも入る + import policy_recorder as pr + import visual_servo_pick as vsp + pr.install_daemon_lock() + locked = vsp._daemon_request + tl = threading.local() + + def on_rpc(cmd_obj, resp, t0, t1, err): + tl.last = (t0, t1) + + pr.add_rpc_observer(on_rpc) + pad_f = open(out_dir / "pad_rpc.csv", "x", newline="", encoding="utf-8") + pad_w = csv.writer(pad_f) + pad_w.writerow(["t_call", "cmd", "joint_id", "wait_ms", "rpc_ms", "probe_paused", "error"]) + pad_lock = threading.Lock() + pad_waits = {"reading": [], "paused": []} + prober_box = [] + + def timed(*a, **k): + tl.last = None + t_call = time.time() + err = "" + try: + return locked(*a, **k) + except BaseException as e: + err = f"{type(e).__name__}: {e}" + raise + finally: + last = getattr(tl, "last", None) + tl.wait = None if last is None else last[0] - t_call + if threading.current_thread().name != "probe" and not getattr(tl, "probe_own", False): + c = a[0] if a else k.get("cmd_obj", {}) + c = c if isinstance(c, dict) else {} + paused = bool(prober_box and prober_box[0].paused.is_set()) + wait_ms = None if last is None else (last[0] - t_call) * 1000.0 + rpc_ms = None if last is None else (last[1] - last[0]) * 1000.0 + with pad_lock: + pad_w.writerow([f"{t_call:.6f}", c.get("cmd"), c.get("joint_id", ""), + "" if wait_ms is None else f"{wait_ms:.2f}", + "" if rpc_ms is None else f"{rpc_ms:.2f}", int(paused), err]) + pad_f.flush() + if wait_ms is not None and c.get("cmd") != "ping": + pad_waits["paused" if paused else "reading"].append(wait_ms) + timed._policy_locked = True # gamepad_teleop.main の install_daemon_lock で包み直させない + timed._orig = locked + vsp._daemon_request = timed + gt.load_daemon_modules() + if gt.daemon_request is not timed: + sys.exit("[ABORT] gamepad_teleop の送り口が鍵の版になっていません (先に読まれていた). 何も送らずに終わります") + assert rpcmod is gt.rpc + + state = {"started": False, "teleop_log": None} + quit_ev = threading.Event() + keys = TermKeys() + + def on_frame(frame, tp): + if not state["started"]: + state["started"] = True + state["teleop_log"] = str(getattr(tp.log, "path", "")) or None + p = Prober(lambda c: vsp._daemon_request(c, timeout_override=RPC_TIMEOUT_SEC), out_dir, cmd, + args.gap if args.gap is not None else GAP_PAD_SEC, wait_of_last=lambda: getattr(tl, "wait", None)) + prober_box.append(p) + p.start() + threading.Thread(target=key_thread, args=(keys, p, quit_ev, False), daemon=True).start() + print("読み始めます。印はターミナルかゲームパッドの窓で: 1 静止 2 空で閉じた 3 つかんだ 4 引っ張った " + "5 離した 6 押した 0 そのほか / p 止める・再開 / 終わるときはウィンドウを閉じる") + for ev in gt.pygame.event.get(gt.pygame.KEYDOWN): # 窓が手前のときの印 (gamepad_teleop はキーを使わない) + ch = (ev.unicode or "").lower() + if ch == "p": + prober_box[0].toggle_pause() + elif ch: + prober_box[0].mark(ch) + + # 始めの 1 回は, ゲームパッドの窓を開く前 (操作を受け付ける前) に読む. on_frame の中で読むと, 読み終わるまで + # gamepad_teleop が L2+R2 の停止もボタンも見ない (codex R2 必須). つながりは gamepad_teleop.main がそのまま使う. + try: + tl.probe_own = True # この道具の読み取り (パッドの命令に数えない) + try: + try: + vsp.get_daemon_socket() + except Exception as e: + sys.exit(f"[ABORT] daemon につながりません ({type(e).__name__}: {e}). 何も送らずに終わります") + first_read(lambda c: vsp._daemon_request(c, timeout_override=RPC_TIMEOUT_SEC), out_dir) + finally: + tl.probe_own = False + with keys: + gt.main(["--execute"], on_frame=on_frame) + finally: + # どこで終わっても 読み取り・観察・CSV を片付ける (codex R3 推奨) + quit_ev.set() + if prober_box: + prober_box[0].stop() + pr.remove_rpc_observer(on_rpc) + with pad_lock: + pad_f.close() + if not state["started"]: + # gamepad_teleop の窓まで行かなかった (始めの 1 回の失敗・記録フォルダが作れないなど): + # つながりはこの道具が閉じる. 窓まで行ったあとは gamepad_teleop.main が閉じる + # (送っている途中の仕事が戻らないときは, わざと閉じない. それを変えない) + vsp.close_daemon_socket() + meta["teleop_log"] = state["teleop_log"] + + def stats(xs): + if not xs: + return None + s = sorted(xs) + return {"n": len(s), "median": round(statistics.median(s), 2), "p95": round(s[int(0.95 * (len(s) - 1))], 2), + "max": round(s[-1], 2)} + meta["pad_wait_ms"] = {k: stats(v) for k, v in pad_waits.items()} + return (prober_box[0] if prober_box else None), meta["pad_wait_ms"] + + +def main(argv=None): + ap = argparse.ArgumentParser(description="関節と爪の電流を読むだけ (アームには送らない)") + ap.add_argument("--with-pad", action="store_true", help="gamepad_teleop をこのプロセスで動かす (ゲームパッドで動かす)") + ap.add_argument("--seconds", type=float, default=None, help="この秒数で終わる (ひとりのときだけ)") + ap.add_argument("--regs", default="none", help="毎回読むサーボの番地: none (既定) / sts (1〜7 の 60,61,69,70) / 'id:addr,...'") + ap.add_argument("--status", action="store_true", help="毎回 get_servo_status も読む") + ap.add_argument("--gripper", action="store_true", help="毎回 get_gripper_value も読む") + ap.add_argument("--no-currents", action="store_true", help="get_servo_currents を読まない (番地だけで測るとき)") + ap.add_argument("--gap", type=float, default=None, help=f"読み取りの間の秒 (既定 ひとり {GAP_SOLO_SEC} / パッド {GAP_PAD_SEC})") + args = ap.parse_args(argv) + if args.with_pad and args.seconds: + ap.error("--seconds は --with-pad と一緒に使えません (ウィンドウを閉じて終わる)") + regs = parse_regs(args.regs) + cmd = read_cmd(currents=not args.no_currents, status=args.status, gripper=args.gripper, regs=regs) + if args.with_pad and n_reads(cmd) > PAD_MAX_READS: + ap.error(f"--with-pad のときは 1 回に読む数 (角度・電流・状態・爪・番地の合計) を {PAD_MAX_READS} までにしてください " + f"(今 {n_reads(cmd)}). daemon が読んでいる間はゲームパッドの命令 (stop も) が待たされます") + mode = "pad" if args.with_pad else "solo" + out_dir = make_out_dir(mode) + print(f"[記録] {out_dir}") + meta = {"started_at": datetime.now().isoformat(timespec="seconds"), "argv": sys.argv, "mode": mode, + "cmd": cmd, "gap_sec": args.gap, "daemon": f"{DAEMON_HOST}:{DAEMON_PORT}", "sends_to_arm": "none (read only)" + + ("; gamepad_teleop sends as usual" if args.with_pad else "")} + prober = None + try: + if args.with_pad: + prober, _ = run_with_pad(args, out_dir, cmd, meta) + else: + prober, _ = run_solo(args, out_dir, cmd, meta) + finally: + meta["ended_at"] = datetime.now().isoformat(timespec="seconds") + if prober is not None: + meta["probe"] = prober.summary() + print(prober.status_line()) + if meta.get("pad_wait_ms"): + print(f"[パッド] 鍵の待ち (ms): 読んでいる間 {meta['pad_wait_ms'].get('reading')} / " + f"止めている間 {meta['pad_wait_ms'].get('paused')}") + write_json(out_dir / "summary.json", meta) + print(f"[終了] アームには何も送っていません (ゲームパッドで送った分を除く)。記録: {out_dir}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tools/test_daemon_read_servo.py b/tools/test_daemon_read_servo.py new file mode 100644 index 0000000..d4cc5d2 --- /dev/null +++ b/tools/test_daemon_read_servo.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 +"""test_daemon_read_servo.py — tools/pi/mecharm_daemon.py の read_servo (依頼 109) を偽物の MyCobot で確かめる + + python3 tools/test_daemon_read_servo.py + +確かめること: 読むだけ (送る関数を 1 つも呼ばない)・頼んだ分だけ読む・読めないときは None と error で ok=True・ +頼み方がおかしいときは何も読まずに ok=False・raw でバイト列が返り _read が元に戻る・今までの命令の返事が同じ. +""" +import importlib.util +import json +import sys +import types +import unittest +from pathlib import Path + +DAEMON = Path(__file__).resolve().parent / "pi" / "mecharm_daemon.py" + + +def load_daemon(): + pkg = types.ModuleType("pymycobot") + mod = types.ModuleType("pymycobot.mycobot") + mod.MyCobot = object + saved = {k: sys.modules.get(k) for k in ("pymycobot", "pymycobot.mycobot")} + sys.modules["pymycobot"] = pkg + sys.modules["pymycobot.mycobot"] = mod + try: + spec = importlib.util.spec_from_file_location("mecharm_daemon_under_test", DAEMON) + m = importlib.util.module_from_spec(spec) + spec.loader.exec_module(m) + finally: + for k, v in saved.items(): + if v is None: + sys.modules.pop(k, None) + else: + sys.modules[k] = v + return m + + +def class_read(self, genre): + return bytes([0xFE, 0xFE, 0x02, genre, 0xFA]) + + +class FakeMC: + """読む関数だけを持つ. 送る関数が呼ばれたら記録する.""" + _read = class_read # 2.9.6 の MyCobot と同じくクラスに置く + + def __init__(self): + self.calls = [] + self.sent = [] + self.currents = [10, 20, 30, 40, 50, 60] + + def _rec(self, name, *a): + self.calls.append((name,) + a) + + def get_angles(self): + self._rec("get_angles") + self._read(0x20) + return [1.0, 2.0, 3.0, 4.0, 5.0, -30.0] + + def get_servo_currents(self): + self._rec("get_servo_currents") + self._read(0xE2) + if isinstance(self.currents, Exception): + raise self.currents + return self.currents + + def get_servo_status(self): + self._rec("get_servo_status") + return [0, 0, 0] + + gripper = 42 + + def get_gripper_value(self): + self._rec("get_gripper_value") + return self.gripper + + def get_servo_data(self, sid, addr): + self._rec("get_servo_data", sid, addr) + self._read(0x53) + return -1 if addr == 99 else sid * 100 + addr + + def __getattr__(self, name): # 送る関数 (send_angles など) は呼ばれたら記録して何もしない + if name.startswith(("send_", "set_", "release", "focus", "jog", "stop", "power", "pause", "resume")): + def f(*a, **k): + self.sent.append(name) + return 1 + return f + raise AttributeError(name) + + +class ReadServoTest(unittest.TestCase): + def setUp(self): + self.d = load_daemon() + self.mc = FakeMC() + self.d._mc = self.mc + + def cmd(self, **kw): + obj = {"cmd": "read_servo"} + obj.update(kw) + r = self.d.handle_command(obj) + json.dumps(r) # JSON で返せること + return r + + def test_default_reads_angles_and_currents_only(self): + r = self.cmd() + self.assertTrue(r["ok"]) + self.assertEqual(sorted(r["items"]), ["angles", "currents"]) + self.assertEqual(r["items"]["currents"]["value"], [10, 20, 30, 40, 50, 60]) + self.assertEqual(r["items"]["angles"]["value"][5], -30.0) + self.assertEqual([c[0] for c in self.mc.calls], ["get_angles", "get_servo_currents"]) + self.assertEqual(r["regs"], []) + self.assertNotIn("raw", r["items"]["angles"]) + self.assertEqual(self.mc.sent, []) + + def test_all_items_and_regs_in_order(self): + r = self.cmd(status=True, gripper=True, regs=[[7, 69], [1, 60]]) + self.assertEqual([c[0] for c in self.mc.calls], + ["get_angles", "get_servo_currents", "get_servo_status", "get_gripper_value", + "get_servo_data", "get_servo_data"]) + self.assertEqual([r_[:3] for r_ in r["regs"]], [[7, 69, 769], [1, 60, 160]]) + self.assertEqual(r["items"]["gripper_value"]["value"], 42) + self.assertEqual(self.mc.sent, []) + + def test_no_angles_no_currents(self): + r = self.cmd(angles=False, currents=False, regs=[[2, 69]]) + self.assertEqual(r["items"], {}) + self.assertEqual(self.mc.calls, [("get_servo_data", 2, 69)]) + + def test_read_error_is_none_with_ok_true(self): + self.mc.currents = ValueError("unpack") + r = self.cmd() + self.assertTrue(r["ok"]) + self.assertIsNone(r["items"]["currents"]["value"]) + self.assertIn("ValueError", r["items"]["currents"]["error"]) + self.assertEqual(r["items"]["angles"]["value"][0], 1.0) # ほかの項目は読める + + def test_missing_function(self): + del FakeMC.get_servo_currents + try: + r = self.cmd() + finally: + FakeMC.get_servo_currents = ReadServoTest._orig_cur + self.assertTrue(r["ok"]) + self.assertIsNone(r["items"]["currents"]["value"]) + self.assertIn("not available", r["items"]["currents"]["error"]) + + def test_no_reply_values_become_none(self): + # 2.9.6 は返事がないと [] / -1 を返す. 読めなかったとして扱う (codex R1 必須 2) + self.mc.currents = [] + self.mc.gripper = -1 + r = self.cmd(gripper=True, regs=[[7, 99], [7, 69]]) + self.assertTrue(r["ok"]) + cur = r["items"]["currents"] + self.assertEqual((cur["value"], cur["error"], cur["returned"]), (None, "no reply", [])) + g = r["items"]["gripper_value"] + self.assertEqual((g["value"], g["error"], g["returned"]), (None, "no reply", -1)) + self.assertEqual([x[:3] for x in r["regs"]], [[7, 99, None], [7, 69, 769]]) + self.assertEqual(r["regs_detail"][0]["error"], "no reply") + self.mc.currents = None + self.assertEqual(self.cmd()["items"]["currents"]["error"], "no reply") + + def test_missing_get_servo_data_reports_error(self): + orig = FakeMC.get_servo_data + del FakeMC.get_servo_data + try: + r = self.cmd(regs=[[1, 60]]) + finally: + FakeMC.get_servo_data = orig + self.assertEqual(r["regs"][0][:3], [1, 60, None]) + self.assertIn("not available", r["regs_detail"][0]["error"]) + + def test_unjsonable_values(self): + self.mc.currents = object() + v = self.cmd()["items"]["currents"]["value"] + self.assertIsInstance(v, str) + + def test_raw_capture_and_restore(self): + r = self.cmd(raw=True, regs=[[7, 69]]) + self.assertEqual(r["items"]["currents"]["raw"], ["fefe02e2fa"]) + self.assertEqual(r["regs_detail"][0]["raw"], ["fefe0253fa"]) + self.assertNotIn("_read", self.mc.__dict__) # 包みが外れている + self.assertIs(type(self.mc)._read, class_read) + + def test_raw_restored_after_exception(self): + self.mc.currents = RuntimeError("x") + self.cmd(raw=True) + self.assertNotIn("_read", self.mc.__dict__) + + def test_bad_regs_read_nothing(self): + for regs in ([[8, 69]], [[0, 1]], [[1, 101]], [[1]], [[1, 2, 3]], "1:69", [1], [["a", 2]], + [[1, 60]] * 33, [[1.9, 60.9]], [[True, 60]], [["1", "60"]], [{"0": 1, "1": 2}], {}, "", 0, False): + self.mc.calls.clear() + r = self.cmd(regs=regs) + self.assertFalse(r["ok"], regs) + self.assertEqual(self.mc.calls, [], regs) + self.assertEqual(self.mc.sent, []) + + def test_existing_commands_unchanged(self): + self.assertEqual(self.d.handle_command({"cmd": "ping"}), + {"ok": True, "daemon": "mecharm_daemon", "fresh_mode": 0}) + self.assertEqual(self.d.handle_command({"cmd": "get_angles_once"}), + {"ok": True, "angles": [1.0, 2.0, 3.0, 4.0, 5.0, -30.0]}) + self.assertEqual(self.d.handle_command({"cmd": "get_gripper_value"}), {"ok": True, "value": 42}) + self.assertEqual(self.d.handle_command({"cmd": "nope"}), {"ok": False, "error": "unknown cmd: nope"}) + + +ReadServoTest._orig_cur = FakeMC.get_servo_currents + + +if __name__ == "__main__": + unittest.main() diff --git a/tools/test_servo_current_probe.py b/tools/test_servo_current_probe.py new file mode 100644 index 0000000..3b02335 --- /dev/null +++ b/tools/test_servo_current_probe.py @@ -0,0 +1,308 @@ +#!/usr/bin/env python3 +"""test_servo_current_probe.py — tools/servo_current_probe.py の読む側 (Prober など) を偽物の daemon で確かめる + + python3 tools/test_servo_current_probe.py + +アームにも daemon にもつながない. 一時フォルダに書く. +""" +import csv +import json +import socket +import sys +import tempfile +import threading +import time +import unittest +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +import servo_current_probe as scp # noqa: E402 + + +def fake_resp(cur=(1, 2, 3, 4, 5, 6), ok=True, err_item=None): + items = {"angles": {"value": [0.0, 1.0, 2.0, 3.0, 4.0, -30.0], "ms": 3.0}, + "currents": {"value": list(cur) if isinstance(cur, tuple) else cur, "ms": 4.0}} + if err_item: + items["currents"]["error"] = err_item + return {"ok": ok, "t": time.time(), "total_ms": 7.5, "items": items, "regs": []} + + +class ProberTest(unittest.TestCase): + def setUp(self): + self.tmp = tempfile.TemporaryDirectory() + self.dir = Path(self.tmp.name) + + def tearDown(self): + self.tmp.cleanup() + + def rows(self, name="samples.csv"): + with open(self.dir / name, newline="", encoding="utf-8") as f: + return list(csv.DictReader(f)) + + def test_rows_and_marks(self): + seq = [fake_resp(), fake_resp(cur=[]), fake_resp(cur=None, err_item="ValueError: x")] + sent = [] + + def req(c): + sent.append(c) + if len(sent) > 3: + raise OSError("down") + return seq[len(sent) - 1] + p = scp.Prober(req, self.dir, scp.read_cmd(), 0.0, out=lambda *a: None) + p.mark("3") + self.assertFalse(p.mark("x")) + for _ in range(4): + resp, err = None, "" + try: + resp = req(p.cmd) + except Exception as e: + err = f"{type(e).__name__}: {e}" + marks = [] + while not p.marks.empty(): + marks.append(p.marks.get_nowait()) + p.write_row(time.time(), time.time() + 0.01, None, resp, err, marks) + p.stop() + r = self.rows() + self.assertEqual(len(r), 4) + self.assertEqual([r[0][f"cur{k}"] for k in range(6)], ["1", "2", "3", "4", "5", "6"]) + self.assertEqual(r[0]["mark"], "grasp") + self.assertEqual(r[0]["j5"], "-30.00") + self.assertEqual(r[1]["currents_raw"], "[]") + self.assertEqual(r[1]["cur0"], "") + self.assertIn("currents:ValueError", r[2]["error"]) + self.assertIn("OSError", r[3]["error"]) + self.assertEqual(p.n_cur_ok, 1) + self.assertEqual(p.n_err, 1) # やりとりの失敗は OSError の 1 回だけ + self.assertEqual(p.n_item_err, 1) # 項目が読めなかった (ValueError) 1 回 + m = self.rows("marks.csv") + self.assertEqual([(x["key"], x["name"]) for x in m], [("3", "grasp")]) + s = p.summary() + self.assertEqual(s["n"], 4) + self.assertEqual(s["rtt_ms"]["n"], 3) + json.dumps(s) + self.assertTrue(all(c["cmd"] == "read_servo" for c in sent)) + + def test_thread_runs_pauses_and_stops(self): + n = [0] + + def req(c): + n[0] += 1 + time.sleep(0.002) + return fake_resp() + p = scp.Prober(req, self.dir, scp.read_cmd(), 0.0, out=lambda *a: None) + p.start() + time.sleep(0.15) + p.toggle_pause() + time.sleep(0.1) # 止める前に始めた 1 回が終わるのを待つ + k = n[0] + time.sleep(0.15) + self.assertEqual(n[0], k) # 止めている間は読まない + p.toggle_pause() + time.sleep(0.1) + p.stop() + self.assertGreater(n[0], k) + self.assertFalse(p.thread.is_alive()) + self.assertGreater(p.hz(), 10) + self.assertFalse(p.mark("1")) # 閉じたあとの印は書かない + + +class HelpersTest(unittest.TestCase): + def test_parse_regs(self): + self.assertEqual(scp.parse_regs("none"), []) + self.assertEqual(len(scp.parse_regs("sts")), 28) + self.assertIn((7, 69), scp.parse_regs("sts")) + self.assertEqual(scp.parse_regs("7:69,7:70"), [(7, 69), (7, 70)]) + + def test_read_cmd_is_read_only(self): + c = scp.read_cmd(status=True, gripper=True, regs=[(7, 69)], raw=True) + self.assertEqual(c["cmd"], "read_servo") + self.assertEqual(c["regs"], [[7, 69]]) + self.assertLessEqual(len(scp.STS_REGS), 32) # daemon の上限 SERVO_READ_MAX_REGS + + def test_check_first(self): + with self.assertRaises(SystemExit) as cm: + scp.check_first({"ok": False, "error": "unknown cmd: read_servo"}) + self.assertIn("daemon_add_read_servo.sh", str(cm.exception)) + with self.assertRaises(SystemExit): + scp.check_first(None) + scp.check_first(fake_resp()) + + def test_first_read_is_chunked(self): + sent = [] + + def req(c): + sent.append(c) + return fake_resp() + with tempfile.TemporaryDirectory() as d: + scp.first_read(req, Path(d)) + j = json.loads((Path(d) / "first_read.json").read_text(encoding="utf-8")) + self.assertTrue(all(scp.n_reads(c) <= scp.FIRST_CHUNK for c in sent)) + self.assertEqual(sorted(tuple(r) for c in sent for r in c["regs"]), sorted(scp.STS_REGS)) + self.assertTrue(sent[0]["currents"] and sent[0]["status"] and sent[0]["gripper"]) + self.assertEqual(len(j["resps"]), len(sent)) + + def test_pad_read_cap(self): + with self.assertRaises(SystemExit): + scp.main(["--with-pad", "--regs", "sts"]) + with self.assertRaises(SystemExit): + scp.main(["--with-pad", "--status", "--gripper", "--regs", "7:69"]) + + def test_describe_first(self): + r = fake_resp() + r["regs"] = [[7, 69, 12, 3.0], [1, 60, 5, 2.0]] + lines = scp.describe_first(r) + self.assertTrue(any("currents" in ln for ln in lines)) + self.assertTrue(any(ln.startswith("servo 7 (爪)") for ln in lines)) + + +class SoloClientTest(unittest.TestCase): + def test_line_protocol(self): + srv = socket.socket() + srv.bind(("127.0.0.1", 0)) + srv.listen(1) + port = srv.getsockname()[1] + got = [] + + def serve(): + c, _ = srv.accept() + buf = b"" + while len(got) < 2: + buf += c.recv(4096) + while b"\n" in buf: + line, buf = buf.split(b"\n", 1) + got.append(json.loads(line)) + resp = json.dumps({"ok": True, "n": len(got)}) + "\n" + c.sendall(resp[:5].encode()) # 返事を 2 回に分けて送る + time.sleep(0.01) + c.sendall(resp[5:].encode()) + c.close() + t = threading.Thread(target=serve, daemon=True) + t.start() + cli = scp.SoloClient("127.0.0.1", port, timeout=2) + self.assertEqual(cli.request({"cmd": "a"})["n"], 1) + self.assertEqual(cli.request({"cmd": "b"})["n"], 2) + cli.close() + srv.close() + self.assertEqual([g["cmd"] for g in got], ["a", "b"]) + + +class WithPadWiringTest(unittest.TestCase): + """--with-pad のつなぎ: gamepad_teleop の送り口が鍵の版になり, パッドの命令が pad_rpc.csv に残る. + gamepad_teleop.main は偽物に差し替える (pygame の窓も daemon も使わない).""" + + def test_wiring(self): + try: + import gamepad_teleop as gt + import run_policy_client # noqa: F401 + import visual_servo_pick as vsp + except Exception as e: # pygame・cv2 がない環境 + self.skipTest(f"import できない: {e}") + if gt.rpc is not None or getattr(vsp._daemon_request, "_policy_locked", False): + self.skipTest("ほかのテストが先に読み込んだ") + calls = [] + lock_holder = [] + + def base(cmd_obj, timeout_override=None, _deadline_at=None): + calls.append((threading.current_thread().name, cmd_obj["cmd"])) + lock_holder.append(threading.current_thread().name) + time.sleep(0.003) + if cmd_obj["cmd"] == "read_servo": + return fake_resp() + return {"ok": True} + orig_req, orig_main, orig_get = vsp._daemon_request, gt.main, gt.pygame.event.get + orig_sock = vsp.get_daemon_socket + vsp.get_daemon_socket = lambda *a, **k: None + vsp._daemon_request = base + tmp = tempfile.TemporaryDirectory() + out_dir = Path(tmp.name) + + class FakeLog: + path = Path("/tmp/teleop_fake/log.jsonl") + + class FakeTp: + log = FakeLog() + + def fake_main(argv, on_frame=None): + self.assertEqual(argv, ["--execute"]) + # 始めの 1 回 (読めるものを全部) は窓を開く前に終わっている (codex R2 必須) + self.assertTrue((out_dir / "first_read.json").exists()) + self.assertEqual(len([c for c in calls if c[1] == "read_servo"]), 1 + len(scp.STS_REGS) // scp.FIRST_CHUNK) + for i in range(20): + on_frame(i, FakeTp()) + gt.daemon_request({"cmd": "send_angle", "joint_id": 2, "angle": 1.0}) + time.sleep(0.01) + return 0 + gt.main = fake_main + gt.pygame.event.get = lambda *a, **k: [] + try: + class A: + gap = 0.0 + meta = {} + prober, waits = scp.run_with_pad(A(), out_dir, scp.read_cmd(), meta) + self.assertIs(gt.daemon_request, vsp._daemon_request) + self.assertTrue(gt.daemon_request._policy_locked) + with open(out_dir / "pad_rpc.csv", newline="", encoding="utf-8") as f: + rows = list(csv.DictReader(f)) + self.assertEqual([r["cmd"] for r in rows], ["send_angle"] * 20) + self.assertTrue(all(r["wait_ms"] != "" and r["rpc_ms"] != "" for r in rows)) + self.assertEqual(waits["reading"]["n"], 20) + self.assertEqual(meta["teleop_log"], "/tmp/teleop_fake/log.jsonl") + self.assertGreater(prober.n, 5) + self.assertTrue((out_dir / "first_read.json").exists()) + self.assertIn(("probe", "read_servo"), calls) + self.assertFalse(prober.thread.is_alive()) + with open(out_dir / "samples.csv", newline="", encoding="utf-8") as f: + srows = list(csv.DictReader(f)) + self.assertTrue(all(r["wait_ms"] != "" for r in srows)) + finally: + vsp._daemon_request, gt.main, gt.pygame.event.get = orig_req, orig_main, orig_get + vsp.get_daemon_socket = orig_sock + tmp.cleanup() + + + def test_cleanup_before_handover(self): + """窓まで行かずに終わったとき (始めの 1 回の失敗・gamepad_teleop の初めの失敗) も, + つながり・観察・pad_rpc.csv を片付ける (codex R3 推奨).""" + try: + import gamepad_teleop as gt + import run_policy_client # noqa: F401 + import policy_recorder as pr + import visual_servo_pick as vsp + except Exception as e: + self.skipTest(f"import できない: {e}") + saved = (vsp._daemon_request, vsp.get_daemon_socket, vsp.close_daemon_socket, gt.main, gt.rpc, gt.daemon_request) + closed = [] + + class A: + gap = 0.0 + try: + for case in ("first_read_fails", "main_fails_early"): + closed.clear() + gt.rpc = None # gamepad_teleop の送り口を読み直させる + gt.daemon_request = None + n_obs = len(pr._rpc_observers) + + def base(cmd_obj, timeout_override=None, _deadline_at=None, case=case): + if case == "first_read_fails": + raise RuntimeError("daemon error: unknown cmd: read_servo") + return fake_resp() + vsp._daemon_request = base + vsp.get_daemon_socket = lambda *a, **k: None + vsp.close_daemon_socket = lambda: closed.append(1) + + def bad_main(argv, on_frame=None): + sys.exit("[ABORT] 同じ名前のフォルダがあります") + gt.main = bad_main + with tempfile.TemporaryDirectory() as d: + with self.assertRaises((SystemExit, RuntimeError)): + scp.run_with_pad(A(), Path(d), scp.read_cmd(), {}) + self.assertTrue((Path(d) / "pad_rpc.csv").exists()) + self.assertTrue(closed, case) # つながりを閉じた + self.assertEqual(len(pr._rpc_observers), n_obs, case) # 観察は残さない + finally: + (vsp._daemon_request, vsp.get_daemon_socket, vsp.close_daemon_socket, gt.main, gt.rpc, + gt.daemon_request) = saved + + +if __name__ == "__main__": + unittest.main()