#!/usr/bin/env python3 """policy_recorder.py — 実機推論 (run_policy_client.py --execute) の間, 上カメラと実測角を 1 秒 10 コマで残す (2026-10-29). お手本 (record_teach_demo.py + recorder.py --top) と同じ形で書くので, build_demo_episodes.build_one() がそのまま読める: {run_dir}/recording/states.csv recorder.py と同じ列. actual / gripper_cmd / stage 行. stage 列 = その時の状態 (model / rewind / correct / home / wait) {run_dir}/recording/top.mp4 上カメラ (元の大きさ. 10 コマ/秒) {run_dir}/recording/top_frame_log.csv frame_idx, capture_ts (+ mode 列. 1 コマごとの状態) {run_dir}/recording/top_meta.json コマ数・実効 fps など アームへは何も送らない (関節角を読むだけ). daemon のソケットは 1 本を本体と共用するので, install_daemon_lock() で 1 回のやりとりごとに鍵をかける (読み書きが混ざらないように). 2026-10-31 依頼 46: 送っている間の角度の抜けについて daemon は send_angles を受けると, 送ったあとに角度を読み直す (動いている間は読めずにやり直す) まで 返事をしない (約 1.6 秒. 爪は約 3.6 秒). daemon は 1 本のつながりを 1 件ずつ処理するので, その間は 記録スレッドも角度を読めない. 腕はこの間に動き終わる (Log17 の記録で 1.5〜3.6 秒の抜け). 記録の側で直せるのは次の 2 つだけ (送る指令・到着の判定・待ち時間は変えない): - 本体が読んだ角度 (到着の確かめ・送る直前の読み) も actual 行に残す (note=main) - daemon とのやりとり 1 回ごとの始まりと長さを残す (send_angles 行 / rpc 行). 送った目標と時刻が 分かるので, あとから build_correction_episodes.py --gap-fill speed20 で抜けを埋められる 抜けそのものをなくすには daemon の送ったあとの読み直しを変える必要がある (決めるのはエイイチ). 2026-11-07 依頼 116: 爪と肩の電流も残す 記録スレッドは get_angles の代わりに daemon の read_servo (角度 + サーボの番地. 読むだけ) を 1 回呼ぶ (daemon への問い合わせの回数は今と同じ). states.csv の列と行は変えない (actual 行は今までどおり). 同じ時刻の電流は {run_dir}/recording/currents.csv に書く (列は CURRENTS_COLS. ts は states.csv の actual 行と同じ値. 角度を読んだ時刻 = 返事が着いた時刻 - daemon の中で角度のあとにかかった時間. rpc_ms は鍵の待ちも含む 1 回の長さ). 番地 69 が下位・70 が上位. 上位の 128 が向き, 残りが大きさ: 符号付きの値 = (70 & 128 なら -1) × (69 + 256 × (70 & 127)) 爪 (7) の 69/70 と肩 (2) の 69 は毎回読む. 肩の 70 は SHOULDER_HI_EVERY 回に 1 回だけ読む (11-07 の測り (diag/servo_current_20261107/180051_pad) で 番地 1 つ 約 20 ms・角度 約 18 ms. 4 つを毎回読むと 1 回 約 105 ms で 10 Hz に収まらない. 3 つなら 約 85 ms). 読まなかった番地・読めなかった番地は空. 肩の符号付きの値は 69 と 70 を同じ回に読めたときだけ書く. 読めなかった回 (例外・返事なし) は値を空にして続ける. daemon に read_servo がなければ (unknown cmd) その run は get_angles に戻して角度だけ残す (currents.csv は列名だけ). daemon の時間の約 85% を記録が使うので, 同じプロセスのパッド・本体の命令は鍵を 平均 35 ms・最大 約 110 ms (読み取り 1 回分) 待つ. 読んだあとは YIELD_SEC 鍵を手放す (すぐ取り直さない. 先に通す保証ではない). read_servo が答えないときは READ_SERVO_TIMEOUT_SEC で切ってつなぎ直す (その間の stop は 約 2 秒待つ). """ import csv import json import math import threading import time from pathlib import Path import cv2 import visual_servo_pick as vsp RECORD_FPS = 10.0 FRAME_INTERVAL = 1.0 / RECORD_FPS ANGLE_INTERVAL = 1.0 / RECORD_FPS FOURCC = "avc1" # recorder.py と同じ STATES_COLS = ["ts", "event_type", "stage", "j0", "j1", "j2", "j3", "j4", "j5", "speed", "gripper_state", "ts_send", "ts_arrival", "attempt", "arrived", "note"] # recorder.py の STATES_COLS と同じ FRAME_LOG_COLS = ["frame_idx", "capture_ts", "mode"] FLUSH_INTERVAL = 1.0 YIELD_SEC = 0.005 # 記録スレッドが読んだあと, 次を読むまでに少なくともあける時間 (依頼 116) STOP_JOIN_SEC = 5.0 # stop() でスレッドの終わりを待つ上限 FRAME_WAIT_SEC = 2.0 # 本体が画像を求めたとき, 新しいコマを待つ上限 # 依頼 116: 電流 (サーボの番地 69 下位 / 70 上位). 7 = 爪, 2 = 肩 CURRENT_REGS = [(7, 69), (7, 70), (2, 69), (2, 70)] # currents.csv の生の値の列の順 EVERY_REGS = [(7, 69), (7, 70), (2, 69)] # 毎回読む SHOULDER_HI_REG = (2, 70) SHOULDER_HI_EVERY = 5 # 肩の上位 (向き) はこの回数に 1 回 (1 秒 2 回. 毎回読むと 10 Hz に収まらない) CURRENTS_COLS = ["ts", "rpc_ms", "pi_ms", "r7_69", "r7_70", "r2_69", "r2_70", "gripper_current", "shoulder_current", "note"] _rpc_observers = [] # daemon とのやりとり 1 回ごとに呼ぶ関数 (記録用. 依頼 46) def add_rpc_observer(fn): if fn not in _rpc_observers: _rpc_observers.append(fn) def remove_rpc_observer(fn): if fn in _rpc_observers: _rpc_observers.remove(fn) def _notify_rpc(cmd_obj, resp, t0, t1, err): for fn in list(_rpc_observers): try: fn(cmd_obj, resp, t0, t1, err) except Exception: pass # 記録の失敗で本体を止めない def install_daemon_lock(): """visual_servo_pick._daemon_request を「1 回のやりとりごとに鍵をかける」版に差し替える. daemon_get_angles / daemon_send_angles / daemon_set_gripper* はどれも モジュールの _daemon_request を呼ぶので, ここを差し替えれば全部に効く. 記録スレッドの get_angles と本体の send_angles が同じソケットで混ざらないようにする. 中身 (送る内容・タイムアウト) は変えない. 2 回呼んでも 1 回だけ差し替える. 依頼 46: やりとりが終わったら (鍵を外してから) add_rpc_observer で足した関数に (送った dict, 返事 or None, 始めた時刻, 終わった時刻, 例外 or None) を渡す. 返事と例外はそのまま返す. 再接続のときの入れ子の ping (外のやりとりの鍵の中) は渡さない (codex 推奨). 観察は同じスレッドで呼ぶ (states.csv に 1 行書くだけ. 本体へ返るのがその分だけ遅れる). """ if getattr(vsp._daemon_request, "_policy_locked", False): return orig = vsp._daemon_request lock = threading.RLock() depth = threading.local() # 再接続の中の ping (鍵の中で呼ばれる入れ子) は観察に渡さない def _locked(*a, **k): cmd_obj = a[0] if a else k.get("cmd_obj") resp, err, t0, t1 = None, None, None, None nested = getattr(depth, "n", 0) > 0 try: depth.n = getattr(depth, "n", 0) + 1 with lock: t0 = time.time() try: resp = orig(*a, **k) finally: t1 = time.time() except BaseException as e: err = e raise finally: depth.n -= 1 if _rpc_observers and t0 is not None and not nested: _notify_rpc(cmd_obj, resp, t0, t1, err) return resp _locked._policy_locked = True _locked._orig = orig vsp._daemon_request = _locked class _ServoCurrents: """read_servo で角度と爪・肩の番地を 1 回読み, currents.csv に 1 行書く部品 (依頼 116). 実機推論の記録 (PolicyRecorder) と教えた動きの再生の記録 (CurrentsRecorder. 依頼 121) の両方が使う (読み方を 2 か所に書かない). 使う側が持つもの: read_servo_fn・read_angles_fn・currents_mode・n_reads・ read_ms・n_currents・n_currents_fail・currents_write_errors・cur_f・cur_w・csv_lock・_valid()・_maybe_flush().""" NO_READ_SERVO_MSG = "電流は残さず, 角度だけ記録します" def _servo_cmd(self): regs = list(EVERY_REGS) if self.n_reads % SHOULDER_HI_EVERY == 0: regs.append(SHOULDER_HI_REG) return {"cmd": "read_servo", "angles": True, "currents": False, "status": False, "gripper": False, "regs": [list(r) for r in regs], "raw": False} def _read_once(self): """角度を 1 回読む. read_servo のときは同じ回の番地を currents.csv に書く. 戻り値は (角度 or None, 角度を読んだ時刻). read_servo の daemon は角度を先に読み, そのあと番地を読む. 返事が着いた時刻のままだと角度の時刻が 番地の分 (60〜80 ms) 遅れるので, daemon の中で角度より後にかかった時間 (total_ms - angles の ms) を引く (帰りの通信の分 約 2 ms は引かない. codex R1 必須 1). 引けないとき (返事の形が違う) は着いた時刻.""" if self.currents_mode != "read_servo": return self._read_angles_only() cmd_obj = self._servo_cmd() self.n_reads += 1 t0 = time.time() resp, err = None, None try: resp = self.read_servo_fn(cmd_obj) except Exception as e: err = e t1 = time.time() if err is not None and "unknown cmd" in str(err): # daemon が古い (read_servo がない): この run は角度だけにする self.currents_mode = "get_angles_fallback" print(f"[RECORD] [WARN] daemon に read_servo がありません. {self.NO_READ_SERVO_MSG} " "(tools/daemon_add_read_servo.sh)") return self._read_angles_only() self.read_ms.append((t1 - t0) * 1000.0) a, a_note, ts = None, "", t1 if err is None and isinstance(resp, dict): it = (resp.get("items") or {}).get("angles") or {} v = it.get("value") try: if v is not None and self._valid(v): a = [float(x) for x in v] except Exception: a = None a_note = "" if a is not None else ("angles=no_reply" if not v else "angles=invalid") try: after = (float(resp["total_ms"]) - float(it["ms"])) / 1000.0 if 0.0 <= after <= t1 - t0: ts = t1 - after except (KeyError, TypeError, ValueError): pass try: self._current_row(ts, t0, t1, cmd_obj, resp, err, a_note) except Exception as e: # 電流の行が書けなくても角度の記録は続ける (codex R1 必須 2). 数を top_meta.json に残す self.currents_write_errors += 1 if self.currents_write_errors <= 3: print(f"[RECORD] [WARN] currents.csv に書けません: {type(e).__name__}: {e}") return a, ts def _read_angles_only(self): try: a = self.read_angles_fn() except Exception: a = None return a, time.time() def _current_row(self, ts, t0, t1, cmd_obj, resp, err, a_note): asked = {tuple(r) for r in cmd_obj["regs"]} got, notes = {}, [] if err is not None: notes.append(f"error={type(err).__name__}: {str(err)[:80]}") elif not isinstance(resp, dict): notes.append("error=bad_response") else: for r in resp.get("regs") or []: try: sid, addr, val = int(r[0]), int(r[1]), r[2] except Exception: continue if (sid, addr) not in asked: continue if isinstance(val, int) and not isinstance(val, bool) and 0 <= val <= 255: got[(sid, addr)] = val missing = [f"{s}:{a}" for s, a in CURRENT_REGS if (s, a) in asked and (s, a) not in got] if missing: notes.append("no_reply=" + ",".join(missing)) if a_note: notes.append(a_note) pi_ms = resp.get("total_ms") if err is None and isinstance(resp, dict) else None row = [f"{ts:.6f}", f"{(t1 - t0) * 1000.0:.1f}", f"{float(pi_ms):.1f}" if isinstance(pi_ms, (int, float)) else ""] row += ["" if got.get(r) is None else str(got[r]) for r in CURRENT_REGS] g = signed_current(got.get((7, 69)), got.get((7, 70))) sh = signed_current(got.get((2, 69)), got.get((2, 70))) row += ["" if g is None else str(g), "" if sh is None else str(sh), " ".join(notes)] if any((s, a) in asked and (s, a) not in got for s, a in CURRENT_REGS) or err is not None: self.n_currents_fail += 1 with self.csv_lock: if self.cur_f.closed: return self.cur_w.writerow(row) self.n_currents += 1 self._maybe_flush(ts) class PolicyRecorder(_ServoCurrents): """カメラを読むスレッドと関節角を読むスレッドを持つ. 本体は grab() で最新のコマを受け取る.""" def __init__(self, run_dir, cap, read_angles_fn, to_model_fn, angles_ok=None, read_servo_fn=None): """read_servo_fn: daemon に read_servo の dict を送って返事の dict を返す関数 (依頼 116). None なら今までどおり read_angles_fn で角度だけ読む (currents.csv は列名だけ).""" self.dir = Path(run_dir) / "recording" self.angles_ok = angles_ok # 本体が読んだ角度を残すときの確かめ (None なら 6 個の有限な数) self.dir.mkdir(parents=True, exist_ok=True) self.cap = cap self.read_angles_fn = read_angles_fn self.read_servo_fn = read_servo_fn self.currents_mode = "read_servo" if read_servo_fn is not None else "off" self.n_reads = 0 # 記録スレッドの読み取りの回数 (肩の上位を読む回を決める) self.n_currents = 0 # currents.csv の行の数 self.n_currents_fail = 0 # 1 つでも番地が読めなかった行の数 self.read_ms = [] # read_servo 1 回の長さ (ms) self.currents_write_errors = 0 # currents.csv に書けなかった回数 (角度の記録は続ける) self.to_model_fn = to_model_fn self.mode = "model" self.stop_flag = False self.stopped_cleanly = None # stop() のあと: スレッドが止まって書き切れたか self.csv_lock = threading.Lock() self.frame_cv = threading.Condition() self.latest = None # (frame, capture_ts) self.writer = None self.size = None self.n_frames = 0 self.n_actual = 0 self.n_actual_fail = 0 self.n_actual_main = 0 # 本体が読んだ角度を残した数 (依頼 46) self.n_sends = 0 # send_angles のやりとりを残した数 self.write_errors = 0 # mp4 を開けない / 書けなかった回数 (codex R2 A5) self.started = time.time() self.last_flush = self.started self.states_f = (self.dir / "states.csv").open("w", newline="") self.states_w = csv.writer(self.states_f) self.states_w.writerow(STATES_COLS) self.flog_f = (self.dir / "top_frame_log.csv").open("w", newline="") self.flog_w = csv.writer(self.flog_f) self.flog_w.writerow(FRAME_LOG_COLS) self.cur_f = (self.dir / "currents.csv").open("w", newline="") self.cur_w = csv.writer(self.cur_f) self.cur_w.writerow(CURRENTS_COLS) self._state_row("stage", time.time()) self.t_cam = threading.Thread(target=self._cam_loop, daemon=True, name="record_camera") self.t_ang = threading.Thread(target=self._angle_loop, daemon=True, name="record_angles") def start(self): self.t_cam.start() self.t_ang.start() add_rpc_observer(self._on_rpc) # ---- 本体から呼ぶ ------------------------------------------------------- def set_mode(self, mode): if mode == self.mode: return self.mode = mode self._state_row("stage", time.time()) def gripper(self, state): """グリッパーに指令した瞬間に呼ぶ (build_demo_episodes は指令の時刻で 0/1 を切り替える).""" self._state_row("gripper_cmd", time.time(), gripper_state=int(state)) def grab(self): """呼んだあとに撮れたコマを返す (古いコマを渡さない). 戻り値は grab_rgb と同じ (rgb, bgr_small).""" t_req = time.time() deadline = t_req + FRAME_WAIT_SEC with self.frame_cv: while not self.stop_flag: if self.latest is not None and self.latest[1] >= t_req: return self.to_model_fn(self.latest[0]) left = deadline - time.time() if left <= 0: break self.frame_cv.wait(left) return None, None def stop(self): """スレッドを止めて書き切る. 何度呼んでもよい.""" if self.stop_flag: return remove_rpc_observer(self._on_rpc) self.stop_flag = True with self.frame_cv: self.frame_cv.notify_all() for t in (self.t_cam, self.t_ang): if t.is_alive(): t.join(timeout=STOP_JOIN_SEC) ended = time.time() alive = [t.name for t in (self.t_cam, self.t_ang) if t.is_alive()] # codex R1-5: スレッドが止まっていないのに mp4 を閉じると, あとから write() が走って競合する. # 止まらなかったときは mp4 を閉じず (カメラも本体で閉じない: alive() を見る), # 記録を「異常」として残す (build_correction_episodes はこの run を使わない). self.stopped_cleanly = not alive and not self.write_errors with self.csv_lock: for f in (self.states_f, self.flog_f, self.cur_f): try: f.flush(); f.close() except Exception: if f is self.cur_f: self.currents_write_errors += 1 try: f.close() except Exception: pass if self.writer is not None and not alive: try: self.writer.release() except Exception: pass el = ended - self.started meta = {"frames": self.n_frames, "elapsed_sec": round(el, 3), "effective_fps": round(self.n_frames / el, 3) if el > 0 else 0.0, "width": self.size[0] if self.size else None, "height": self.size[1] if self.size else None, "actual_samples": self.n_actual, "actual_read_failures": self.n_actual_fail, "actual_hz": round(self.n_actual / el, 2) if el > 0 else 0.0, "actual_main_samples": self.n_actual_main, "send_rpcs": self.n_sends, "target_fps": RECORD_FPS, "status": ("thread_not_stopped" if alive else "write_error" if self.write_errors else "ok"), "write_errors": self.write_errors, "threads_alive": alive, "currents_mode": self.currents_mode, "currents_rows": self.n_currents, "currents_rows_incomplete": self.n_currents_fail, "currents_write_errors": self.currents_write_errors, "currents_regs_every": [f"{a}:{b}" for a, b in EVERY_REGS], "currents_shoulder_hi_every": SHOULDER_HI_EVERY, "read_servo_ms": _ms_stats(self.read_ms)} (self.dir / "top_meta.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False)) if alive: print(f"[RECORD] [WARN] 記録スレッドが止まりません ({', '.join(alive)}). " "mp4 を閉じずに終えます (この記録は学習データに使いません)") print(f"[RECORD] 記録終了 コマ {self.n_frames} ({meta['effective_fps']} fps) " f"実測角 {self.n_actual} 回 ({meta['actual_hz']} Hz) 電流 {self.n_currents} 行" f" ({self.currents_mode}) → {self.dir}") # ---- 中身 --------------------------------------------------------------- def alive(self): """記録スレッドがまだ動いているか (stop() のあとに見る. 動いていればカメラを閉じない).""" return self.t_cam.is_alive() or self.t_ang.is_alive() def _state_row(self, ev, ts, angles=None, gripper_state=None, speed=None, ts_send=None, note=""): row = [f"{ts:.6f}", ev, self.mode] for k in range(6): row.append("" if angles is None else f"{float(angles[k]):.3f}") row += ["" if speed is None else str(speed), "" if gripper_state is None else str(gripper_state), "" if ts_send is None else f"{ts_send:.6f}", "", "", "", note] with self.csv_lock: if self.states_f.closed: return False self.states_w.writerow(row) self._maybe_flush(ts) return True def _valid(self, a): if self.angles_ok is not None: return bool(self.angles_ok(a)) try: return len(a) == 6 and all(math.isfinite(float(v)) for v in a) except Exception: return False def _on_rpc(self, cmd_obj, resp, t0, t1, err): """daemon とのやりとり 1 回ごと (install_daemon_lock から). 何も送らない・読み直さない. get_angles: 本体 (記録スレッド以外) が読めた角度を actual 行に残す (note=main. 記録スレッドの分は _angle_loop が残すので二重にしない) send_angles: 始めた時刻 (ts / ts_send)・目標・速度・やりとりの長さ (note) を send_angles 行に残す そのほか (ping 以外): rpc 行に cmd とやりとりの長さを残す """ if self.stop_flag or not isinstance(cmd_obj, dict): return cmd = cmd_obj.get("cmd") dur = f"rpc_sec={t1 - t0:.3f}" + ("" if err is None else f" error={type(err).__name__}") if cmd == "read_servo" and threading.current_thread() is self.t_ang: return # 記録スレッドの読み取り (actual 行と currents.csv に残す. 依頼 116) if cmd == "get_angles": if threading.current_thread() is self.t_ang or err is not None or not isinstance(resp, dict): return a = resp.get("angles") if a is None or not self._valid(a): return if self._state_row("actual", t1, angles=a, note="main"): # 書けた行だけ数える (codex 推奨) self.n_actual_main += 1 elif cmd == "send_angles": ang = cmd_obj.get("angles") if self._state_row("send_angles", t0, angles=ang if ang is not None and len(ang) == 6 else None, speed=cmd_obj.get("speed"), ts_send=t0, note=dur): self.n_sends += 1 elif cmd != "ping": self._state_row("rpc", t0, ts_send=t0, note=f"cmd={cmd} {dur}") def _maybe_flush(self, now): if now - self.last_flush >= FLUSH_INTERVAL: try: self.states_f.flush(); self.flog_f.flush() except Exception: pass try: self.cur_f.flush() except Exception: self.currents_write_errors += 1 # 電流が書けていないことを残す (codex R2 推奨 3) self.last_flush = now def _cam_loop(self): last_written = 0.0 while not self.stop_flag: ok, frame = self.cap.read() ts = time.time() if self.stop_flag: break if not ok or frame is None: time.sleep(0.02) continue with self.frame_cv: self.latest = (frame, ts) self.frame_cv.notify_all() if ts - last_written < FRAME_INTERVAL - 1e-3: continue last_written = ts try: if self.writer is None: h, w = frame.shape[:2] self.size = (w, h) self.writer = cv2.VideoWriter(str(self.dir / "top.mp4"), cv2.VideoWriter_fourcc(*FOURCC), RECORD_FPS, (w, h)) if not self.writer.isOpened(): raise RuntimeError(f"VideoWriter を開けない ({FOURCC})") if not self.writer.isOpened(): raise RuntimeError("VideoWriter が閉じている") self.writer.write(frame) with self.csv_lock: if not self.flog_f.closed: self.flog_w.writerow([self.n_frames, f"{ts:.6f}", self.mode]) self._maybe_flush(ts) self.n_frames += 1 except Exception as e: self.write_errors += 1 if self.write_errors <= 3: print(f"[RECORD] [WARN] コマの書き込みに失敗: {e}") def _angle_loop(self): nxt = time.time() while not self.stop_flag: a, ts = self._read_once() # read_servo のときは currents.csv と同じ時刻で actual 行を書く if a is None: self.n_actual_fail += 1 else: self._state_row("actual", ts, angles=a) self.n_actual += 1 nxt = _pace(nxt) def _pace(nxt): """記録の読み取りの間合い (1 秒 RECORD_FPS 回). 次に読む予定の時刻を返す (依頼 116. 依頼 121 で 2 か所から使う).""" nxt += ANGLE_INTERVAL wait = nxt - time.time() if -wait > ANGLE_INTERVAL: nxt = time.time() # 1 回分より大きく遅れた分は取り戻さない (詰めて読まない) # 1 回分までの遅れ (肩の上位も読む回は 100 ms を少し超える. 依頼 116) は, 次を早めに読んで取り戻す. # ただし読んだあとは必ず YIELD_SEC は鍵を手放す (すぐ取り直すと, 待っているパッドの命令が何回分も待つ) time.sleep(max(wait, YIELD_SEC)) return nxt WINDOW_MARGIN_SEC = 0.02 # 本体が眠る時間の終わりから引く分 (依頼 121) READ_BUDGET_SEC = 0.15 # 1 回の読み取りに見込む時間 (11-07 の測りで 中央値 約 85 ms・肩の上位を読む回 約 105 ms) WINDOW_POLL_SEC = 0.005 # 読んでよい時間を待つ間合い CLOSE_LIMIT_SEC = 1.0 # stop() で currents.csv を閉じる・meta を書くのに待つ上限 def _bounded(fn, sec): """fn を別のスレッドで呼び, sec 秒だけ待つ. 終われば True (ファイルの書き込みで止まらないように. 依頼 121).""" t = threading.Thread(target=fn, daemon=True, name="record_currents_close") t.start() t.join(timeout=sec) return not t.is_alive() class CurrentsRecorder(_ServoCurrents): """教えた動きの再生 (record_teach_demo.py run --execute) の間, 爪と肩の電流を currents.csv に残す (依頼 121). 読み方・列・肩の上位を SHOULDER_HI_EVERY 回に 1 回にする扱い・読めない回を空にして続ける扱いは 実機推論の記録 (PolicyRecorder. 依頼 116) と同じ部品 (_ServoCurrents) を使う. 違うのは次: - **本体 (再生) が daemon を使わずに眠っている間だけ読む**. 本体は眠る前に window(秒) で知らせる (到達の確かめ・静止の確かめの 0.3 秒の間・待ちの間の読み取りの合間). 読み始めてよいのは, 読み終わる見込み (READ_BUDGET_SEC) が眠りの終わり - WINDOW_MARGIN_SEC より前のときだけ. なので見込みどおりに答える回は, 本体の daemon とのやりとりは電流の記録を待たない (到達・静止の確かめの 回数と時刻は今までと同じ. codex R1・R2 必須). **残る場合**: read_servo が見込みより長くかかり, 本体が 起きる時刻を超えた回は, 本体の次のやりとりがその分 (最大 READ_SERVO_TIMEOUT_SEC) 待つ. daemon とのやりとりが 眠りの終わりを超えて続いた回を overruns に数える (本体がちょうどその時に daemon を使ったかは見ていない). 間合いは 1 秒 RECORD_FPS 回まで (それより詰めて読まない). 本体が眠らない間 (send_angles の返事待ち 約 1.6 秒・爪の命令と待ち・再送の前の待ち) は読まないので, 1 秒あたりの行は 10 より少ない - pause() から resume() までは window があっても読まない (爪の命令と待ち・再送の前の待ち. 「爪を動かした直後にシリアルを触らない」(2026-10-19)) - states.csv・top.mp4 は今までどおり recorder.py (別のプロセス) が書く. ここは currents.csv と currents_meta.json だけを書く. read_servo で読んだ角度は ts を決めるのに使い, angle_sink があれば angle_sink(角度, 読んだ時刻) で渡す (依頼 202: record_teach_demo が states.csv の actual 行にする. 渡すのは読めた回だけ. 読み方・読む時機は変えない. 渡す側の失敗は loop_errors に数えて続ける) - daemon に read_servo がなければ (unknown cmd) 読むのをやめる (currents.csv は列名だけ) 記録の失敗で再生を止めない (例外は外へ出さない. stop()・pause() は書き込みで止まったスレッドを待ち続けない). daemon のソケットは本体と共用なので, 使う前に install_daemon_lock() を呼ぶこと. """ NO_READ_SERVO_MSG = "電流は残しません (再生と記録はそのまま続けます)" def __init__(self, run_dir, read_servo_fn): self.dir = Path(run_dir) / "recording" self.dir.mkdir(parents=True, exist_ok=True) self.read_servo_fn = read_servo_fn self.read_angles_fn = lambda: None self.angles_ok = None self.angle_sink = None # 依頼 202: 読めた角度を渡す先 (角度, 読んだ時刻). None なら渡さない self.currents_mode = "read_servo" self.n_reads = 0 self.n_currents = 0 self.n_currents_fail = 0 self.read_ms = [] self.currents_write_errors = 0 self.loop_errors = 0 # 読み取りの外側で起きた例外の数 (起きても続ける) self.overruns = 0 # 眠りの終わりを超えて読んでいた回 (本体が待った回) self.overrun_ms = [] self.stop_flag = False self.paused = False self.allow_until = 0.0 # この時刻 (time.time) までに読み終わるなら読んでよい self.window_sec = 0.0 # 知らされた眠りの合計 (重なりを含む. 目安) self.next_at = 0.0 self.gate = threading.Lock() # 1 回の読み取り (と書き込み) の間持つ self.n_pauses = 0 self.pause_gate_timeouts = 0 self.paused_sec = 0.0 self._paused_at = None self.csv_lock = threading.Lock() self.started = None self.meta = None self.last_flush = time.time() self.cur_f = (self.dir / "currents.csv").open("w", newline="") self.cur_w = csv.writer(self.cur_f) self.cur_w.writerow(CURRENTS_COLS) self.t = threading.Thread(target=self._loop, daemon=True, name="record_currents") def start(self): self.started = time.time() self.t.start() def window(self, sec): """本体が今から sec 秒 daemon を使わない (眠る) と知らせる. 読むのはこの間だけ.""" if sec <= 0 or self.paused: # 止めている間に知らされた眠りは使わない return now = time.time() self.window_sec += sec self.allow_until = max(self.allow_until, now + sec - WINDOW_MARGIN_SEC) def pause(self): """読み取りを止める. 読んでいる途中ならその 1 回が終わるまで待つ (上限 READ_SERVO_TIMEOUT_SEC + 0.5 秒. 書き込みで止まったスレッドを待ち続けない. 上限を超えたら数えて止める印だけ立てる).""" got = self.gate.acquire(timeout=READ_SERVO_TIMEOUT_SEC + 0.5) try: if not got: self.pause_gate_timeouts += 1 if not self.paused: self.paused = True self.n_pauses += 1 self._paused_at = time.time() self.allow_until = 0.0 finally: if got: self.gate.release() def resume(self): self.allow_until = 0.0 # 再開したら, 次に眠りを知らされるまで読まない if self.paused: self.paused = False if self._paused_at is not None: self.paused_sec += time.time() - self._paused_at self._paused_at = None def stop(self): """スレッドを止めて書き切り, 中身の数を dict で返す (currents_meta.json にも書く). 何度呼んでもよい. 待つのは STOP_JOIN_SEC (スレッド) + CLOSE_LIMIT_SEC × 2 (閉じる・meta) まで.""" if self.meta is not None: return self.meta self.stop_flag = True if self.t.is_alive(): self.t.join(timeout=STOP_JOIN_SEC) ended = time.time() alive = self.t.is_alive() if self.paused and self._paused_at is not None: self.paused_sec += ended - self._paused_at # 止めたまま終わった分 self._paused_at = None def _close(): with self.csv_lock: try: self.cur_f.flush(); self.cur_f.close() except Exception: self.currents_write_errors += 1 try: self.cur_f.close() except Exception: pass closed = _bounded(_close, CLOSE_LIMIT_SEC) el = (ended - self.started) if self.started else 0.0 run_sec = max(el - self.paused_sec, 0.0) self.meta = {"currents_mode": self.currents_mode, "currents_rows": self.n_currents, "currents_rows_incomplete": self.n_currents_fail, "currents_write_errors": self.currents_write_errors, "loop_errors": self.loop_errors, "currents_regs_every": [f"{a}:{b}" for a, b in EVERY_REGS], "currents_shoulder_hi_every": SHOULDER_HI_EVERY, "elapsed_sec": round(el, 3), "paused_sec": round(self.paused_sec, 3), "pauses": self.n_pauses, "pause_gate_timeouts": self.pause_gate_timeouts, "window_sec": round(self.window_sec, 3), "currents_hz": round(self.n_currents / el, 2) if el > 0 else 0.0, "currents_hz_excl_paused": round(self.n_currents / run_sec, 2) if run_sec > 0 else 0.0, "target_hz_max": RECORD_FPS, "read_budget_sec": READ_BUDGET_SEC, "overruns": self.overruns, "overrun_ms": _ms_stats(self.overrun_ms), "read_servo_ms": _ms_stats(self.read_ms), "status": ("thread_not_stopped" if alive else "ok" if closed else "close_timeout"), "source": "record_teach_demo (依頼 121)", "reads_only_while_main_sleeps": True} saved = {"ok": False, "error": None} def _save(): try: (self.dir / "currents_meta.json").write_text(meta_text) saved["ok"] = True except Exception as e: saved["error"] = f"{type(e).__name__}: {e}" meta_text = json.dumps(self.meta, indent=2, ensure_ascii=False) if not _bounded(_save, CLOSE_LIMIT_SEC): saved["error"] = "timeout" # 書けたかを戻り値 (= 試行 JSON) に残す (codex R3 推奨 2. ファイルの中には書かない) self.meta["meta_file"] = "ok" if saved["ok"] else saved["error"] if not saved["ok"]: print(f"[RECORD] [WARN] currents_meta.json を書けません ({saved['error']}. 試行 JSON には残します)") print(f"[RECORD] 電流の記録終了 {self.n_currents} 行 ({self.meta['currents_hz']} 行/秒. 本体が眠っている間だけ" f"読む. 爪の間 {self.meta['paused_sec']} 秒は読まない. 見込み超え {self.overruns} 回. {self.currents_mode})" f" → {self.dir / 'currents.csv'}") return self.meta def _maybe_flush(self, now): if now - self.last_flush >= FLUSH_INTERVAL: try: self.cur_f.flush() except Exception: self.currents_write_errors += 1 self.last_flush = now def _valid(self, a): try: return len(a) == 6 and all(math.isfinite(float(v)) for v in a) except Exception: return False def _read_budget(self): """次の 1 回の読み取りに見込む時間 (依頼 156 の PowerRegsRecorder は番地を足す回だけ長くとる).""" return READ_BUDGET_SEC def _loop(self): while not self.stop_flag and self.currents_mode == "read_servo": now = time.time() if self.paused or now < self.next_at or now + self._read_budget() > self.allow_until: time.sleep(WINDOW_POLL_SEC) continue with self.gate: if self.paused or self.stop_flag or time.time() + self._read_budget() > self.allow_until: continue t0 = time.time() limit = self.allow_until + WINDOW_MARGIN_SEC # 本体が起きる時刻 (の見込み) self.next_at = t0 + ANGLE_INTERVAL n_before = len(self.read_ms) try: a, ts = self._read_once() if a is not None and self.angle_sink is not None: self.angle_sink(a, ts) # 依頼 202: states.csv の actual 行へ except Exception as e: # 記録の失敗で再生を止めない self.loop_errors += 1 if self.loop_errors <= 3: print(f"[RECORD] [WARN] 電流の読み取りで例外: {type(e).__name__}: {e}") # daemon とのやりとりが終わった時刻 (currents.csv への書き込みは含めない. codex R3 推奨 1) rpc_end = (t0 + self.read_ms[-1] / 1000.0) if len(self.read_ms) > n_before else time.time() over = rpc_end - limit if over > 0: self.overruns += 1 self.overrun_ms.append(over * 1000.0) def signed_current(lo, hi): """番地 69 (下位) と 70 (上位) から符号付きの値. どちらかがなければ None (依頼 116).""" if lo is None or hi is None: return None v = lo + 256 * (hi & 127) return -v if hi & 128 else v def _ms_stats(xs): if not xs: return None ys = sorted(xs) return {"n": len(ys), "median": round(ys[len(ys) // 2], 1), "p95": round(ys[min(len(ys) - 1, int(len(ys) * 0.95))], 1), "max": round(ys[-1], 1)} READ_SERVO_TIMEOUT_SEC = 1.5 # 記録の read_servo 1 回の上限. 鍵を持ったまま既定のタイムアウトまで待つと # パッドの stop も待たせるので短くする (いつもは 0.1 秒. 番地が 4 つとも # 返事なしでも 0.9 秒ほど. codex R1 推奨 3). 超えたら次の回はつなぎ直す def daemon_read_servo(cmd_obj): """記録スレッドの read_servo (依頼 116). 呼んだときの visual_servo_pick._daemon_request を使う (install_daemon_lock の鍵を通る).""" return vsp._daemon_request(cmd_obj, timeout_override=READ_SERVO_TIMEOUT_SEC) # ---- 2026-11-12 依頼 156: 電源 (12V・2A → 5A) の前後比較のために, 爪の位置とサーボの電圧も読む ---------------- # record_teach_demo.py run --power 2A のときだけ使う (付けなければ CurrentsRecorder のまま. 実機推論は変えない). # 番地は Feetech STS 想定で, この機体では確かめていない (tools/pi/servo_check_pi.py と同じ前提). 値は生のまま残す: # 爪の位置 (7, 56) 下位・(7, 57) 上位 → gripper_pos (signed_current と同じ組み立て. 上位の 128 を向きとみなす) # 電圧 (2, 62) 肩・(7, 62) 爪 (0.1V 想定) # get_gripper_value は使わない (2026-10-19 段階C で, 開いた直後の gv のあとに daemon が固まった. 番地 69/70 と同じ # get_servo_data で読む). 足した番地は currents.csv に入れず power_regs.csv に書く (currents.csv の列と行は今までどおり). # 1 回に足すのは 2 つまで (番地 1 つ 約 20 ms. 肩の上位を読む回には足さない) なので, 1 回の読み取りは多くて 約 125 ms. # 読み始めてよいかの決め方 (本体が眠っている間だけ) は同じで, 番地を足す回だけ見込みを 1 つにつき # POWER_REG_BUDGET_SEC 長くとる (READ_BUDGET_SEC 0.15 → 0.19 秒). 足した番地が POWER_GIVE_UP 回続けて返事なしなら, # その番地はその run ではもう頼まない (本体を待たせるのは多くてその回数まで. overruns に数える). POWER_POS_REGS = [(7, 56), (7, 57)] POWER_VOLT_REGS = [(2, 62), (7, 62)] POWER_REGS = POWER_POS_REGS + POWER_VOLT_REGS POWER_COLS = ["ts", "r7_56", "r7_57", "gripper_pos", "r2_62", "r7_62", "note"] POWER_REG_BUDGET_SEC = 0.02 # 足した番地 1 つにつき, 読み始めてよいかの見込みに足す時間 (番地 1 つ 約 20 ms) POWER_GIVE_UP = 2 # 足した番地が続けてこの回数 返事なしなら, その run ではもう頼まない (codex R1 必須 1: # 2.9.6 は返事のない番地 1 つに 約 0.1 秒待つので, 頼み続けると本体を待たせる) def power_extra_regs(n): """n 回目 (0 始まり) の読み取りに足す番地. 肩の上位を読む回 (n % SHOULDER_HI_EVERY == 0) には足さない. 残りの回は 爪の位置 → 電圧 → 爪の位置 → 電圧 の順 (5 回に 2 回ずつ).""" k = n % SHOULDER_HI_EVERY if k == 0: return [] return list(POWER_POS_REGS) if k % 2 == 1 else list(POWER_VOLT_REGS) class PowerRegsRecorder(CurrentsRecorder): """CurrentsRecorder に爪の位置と電圧の番地を足す (依頼 156). 足した番地は {run}/recording/power_regs.csv. 読めなかった番地は空にして note に no_reply を書く (止めない).""" def __init__(self, run_dir, read_servo_fn): super().__init__(run_dir, read_servo_fn) self.pw_f = (self.dir / "power_regs.csv").open("w", newline="") self.pw_w = csv.writer(self.pw_f) self.pw_w.writerow(POWER_COLS) self.pw_asked = {f"{s}:{a}": 0 for s, a in POWER_REGS} self.pw_got = {f"{s}:{a}": 0 for s, a in POWER_REGS} self.pw_rows = 0 self.pw_write_errors = 0 self.pw_noreply_run = {k: 0 for k in self.pw_asked} # 続けて返事なしの回数 self.pw_given_up = [] # もう頼まない番地 ("2:62" など) def _extras(self): return [r for r in power_extra_regs(self.n_reads) if f"{r[0]}:{r[1]}" not in self.pw_given_up] def _read_budget(self): return READ_BUDGET_SEC + POWER_REG_BUDGET_SEC * len(self._extras()) def _servo_cmd(self): cmd = super()._servo_cmd() cmd["regs"] += [list(r) for r in self._extras()] return cmd def _current_row(self, ts, t0, t1, cmd_obj, resp, err, a_note): try: self._power_row(ts, cmd_obj, resp, err) except Exception as e: self.pw_write_errors += 1 if self.pw_write_errors <= 3: print(f"[RECORD] [WARN] power_regs.csv に書けません: {type(e).__name__}: {e}") super()._current_row(ts, t0, t1, cmd_obj, resp, err, a_note) def _power_row(self, ts, cmd_obj, resp, err): asked = [tuple(r) for r in cmd_obj["regs"] if tuple(r) in POWER_REGS] if not asked: return got = {} if err is None and isinstance(resp, dict): for r in resp.get("regs") or []: try: sid, addr, val = int(r[0]), int(r[1]), r[2] except Exception: continue if (sid, addr) in asked and isinstance(val, int) and not isinstance(val, bool) and 0 <= val <= 255: got[(sid, addr)] = val replied = err is None and isinstance(resp, dict) for r in asked: k = f"{r[0]}:{r[1]}" self.pw_asked[k] += 1 if r in got: self.pw_got[k] += 1 self.pw_noreply_run[k] = 0 elif replied: # daemon は答えたが, その番地だけ返事がない self.pw_noreply_run[k] += 1 if self.pw_noreply_run[k] >= POWER_GIVE_UP and k not in self.pw_given_up: self.pw_given_up.append(k) print(f"[RECORD] [WARN] 番地 {k} が {POWER_GIVE_UP} 回続けて返事なし. この run ではもう読みません " "(本体を待たせないため. 再生は続けます)") notes = [] if err is not None: notes.append(f"error={type(err).__name__}: {str(err)[:80]}") elif not isinstance(resp, dict): notes.append("error=bad_response") missing = [f"{s}:{a}" for s, a in asked if (s, a) not in got] if missing and err is None and isinstance(resp, dict): notes.append("no_reply=" + ",".join(missing)) pos = signed_current(got.get((7, 56)), got.get((7, 57))) def v(r): return "" if got.get(r) is None else str(got[r]) row = [f"{ts:.6f}", v((7, 56)), v((7, 57)), "" if pos is None else str(pos), v((2, 62)), v((7, 62)), " ".join(notes)] with self.csv_lock: if self.pw_f.closed: return self.pw_w.writerow(row) self.pw_rows += 1 def _maybe_flush(self, now): if now - self.last_flush >= FLUSH_INTERVAL: try: self.pw_f.flush() except Exception: self.pw_write_errors += 1 super()._maybe_flush(now) def stop(self): if self.meta is not None: return self.meta meta = super().stop() def _close(): with self.csv_lock: try: self.pw_f.flush(); self.pw_f.close() except Exception: self.pw_write_errors += 1 try: self.pw_f.close() except Exception: pass closed = _bounded(_close, CLOSE_LIMIT_SEC) meta["power_regs"] = { "file": "power_regs.csv", "rows": self.pw_rows, "asked": dict(self.pw_asked), "got": dict(self.pw_got), "write_errors": self.pw_write_errors, "closed": closed, "given_up": list(self.pw_given_up), "regs": {"gripper_pos": ["7:56", "7:57"], "voltage": ["2:62", "7:62"]}, "schedule": f"肩の上位を読まない回に 2 つずつ (爪の位置と電圧を交互). {SHOULDER_HI_EVERY} 回に 2 回ずつ", "note": "Feetech STS 想定の番地. この機体で未確認. 生の値 (電圧は 0.1V 想定)", "source": "record_teach_demo --power (依頼 156)"} # currents_meta.json は依頼 121 の中身のまま. 足した番地の数は power_regs_meta.json に書く (codex R1 推奨 6) pw_text = json.dumps(meta["power_regs"], indent=2, ensure_ascii=False) saved = {"ok": False} def _save(): try: (self.dir / "power_regs_meta.json").write_text(pw_text) saved["ok"] = True except Exception: pass _bounded(_save, CLOSE_LIMIT_SEC) meta["power_regs"]["meta_file"] = "ok" if saved["ok"] else "error" print(f"[RECORD] 爪の位置と電圧 {self.pw_rows} 行 (読めた数 {self.pw_got}" f"{'. もう読まなかった番地 ' + ','.join(self.pw_given_up) if self.pw_given_up else ''})" f" → {self.dir / 'power_regs.csv'}") return meta