--- dynamic_tracking_limits/drag_teaching/record_teach_demo.py.bak_pre_req202_20261115 2026-11-12 02:44:38.376909959 +0000 +++ dynamic_tracking_limits/drag_teaching/record_teach_demo.py.bak_pre_req213_20261115 2026-11-15 00:26:31.699619895 +0000 @@ -49,6 +49,7 @@ import subprocess import sys import tempfile +import threading import time import traceback import types @@ -343,6 +344,10 @@ # 3 tick に 1 回 = 約 3.3Hz(これまでの 19 本と同程度の 3Hz 以上)。 ACTUAL_READ_EVERY_N_TICKS = 3 ACTUAL_READ_TARGET_HZ = 3.0 +# 依頼 202: 電流の記録スレッド(read_servo・本体が眠っている間だけ)が読んだ角度も states.csv の actual 行にする。 +# 列は足さず note 列に入れて、本体の読み(到達・静止の確かめなど。note は空のまま)と区別する。 +CURRENT_THREAD_NOTE = "source=current_thread" +CURRENT_ACTUALS_FLUSH_SEC = 1.0 # trial_end の前に、積んだ通知を送り切るのを待つ上限(ふつうは数 ms で終わる) # 1 コマンドに許す時間。既定 DAEMON_TIMEOUT_SEC=35.0 のままだと 1 回の通信失敗で # ストリームが長時間止まる(codex R1 C6)。send / get の両方に override を付ける。 # ---- LOG352: daemon の新 cmd を使う(段階C の実測を受けて)------------------- @@ -1981,6 +1986,81 @@ self.send_failures = 0 self.last_send_error = None self.currents = None # 依頼 121: policy_recorder.CurrentsRecorder(記録するときだけ) + # 依頼 202: 電流の記録スレッドが読んだ角度も actual 行にする。states.csv の行が時刻順に並ぶよう、 + # start_current_actuals() から close_current_actuals() までは stage / actual / gripper_cmd の通知を + # 鍵の中で 1 本の列に積み(鍵の中では送らない)、送り手のスレッドが積んだ順に送る + # (codex R1 必須: 送信が遅れても本体と電流の記録スレッドは待たない)。 + self._state_lock = threading.Lock() + self._state_cv = threading.Condition(self._state_lock) + self._last_state_ts = None + self._queue = None # 積んだ通知(列を使っている間だけ list) + self._sender = None + self._sender_stop = False + self._current_actuals_open = False # start_current_actuals() で開き、trial_end の前に閉じる + self.actual_sent_current_thread = 0 # 電流の記録スレッドの角度で積んだ actual 行 + self.actual_current_thread_late = 0 # 先に積んだ行より前の時刻だったので積まなかった数 + self.actual_current_thread_errors = 0 # 角度・時刻が数にならず積まなかった数 + self.state_ts_backwards = 0 # 本体の行の時刻が前の行より前だった数(時計が戻ったとき. codex R2) + self.current_actuals_unsent = 0 # close の期限の時点でまだ送り終えていなかった通知の数(途中の 1 件を含む) + self.current_actuals_flush_timeout = False + self.state_rows_sent_by_sender = 0 # 送り手のスレッドが送り終えた通知の数(試行 JSON は trial_end の直後の値) + self.trial_end_via_sender = False # 送り手が止まっていたので trial_end も列の最後に積んだ + self._inflight = 0 # 送り手が送っている途中の通知の数(0 か 1) + self._sender_exited = False # 送り手が列を空にして終わった(鍵の中で立てる) + + def start_current_actuals(self): + """依頼 202: 電流の記録スレッドの角度を受け付け、通知を送り手のスレッドから送る形にする.""" + with self._state_lock: + if self._sender is not None: + return + self._queue = [] + self._sender_stop = False + self._sender_exited = False + self._current_actuals_open = True + self._sender = threading.Thread(target=self._send_loop, args=(self._queue,), daemon=True, + name="record_state_sender") + self._sender.start() + + def _send_loop(self, q): + """積んだ通知を 1 件ずつ取り出して送る(close の期限を過ぎても捨てずに、trial_end まで積んだ順に送り切る).""" + while True: + with self._state_cv: + while not q and not self._sender_stop: + self._state_cv.wait() + if not q: + self._sender_exited = True + return + p = q.pop(0) + self._inflight = 1 + self._send(p) # 鍵の外で送る(_send は例外を外へ出さない) + with self._state_cv: + self._inflight = 0 + self.state_rows_sent_by_sender += 1 + + def _send_state(self, payload, state_row=True): + """依頼 202: states.csv の行になる通知(stage / actual / gripper_cmd). + + ts が無ければ鍵の中で今の時刻を入れる(rpp.recorder_notify が入れるのと同じ時刻)。 + 列を使っている間は鍵の中で積むだけ(recorder.py は届いた順に書くので、積んだ順 = states.csv の順)。 + 列を使っていなければ今までどおりその場で送る。 + """ + with self._state_cv: + if state_row: + if "ts" not in payload: + payload = dict(payload) + payload["ts"] = time.time() + ts = float(payload["ts"]) + if self._last_state_ts is not None and ts < self._last_state_ts: + self.state_ts_backwards += 1 # 直さずに数えるだけ(codex R2) + else: + self._last_state_ts = ts + if self._queue is not None and not self._sender_exited: + self._queue.append(payload) + if not state_row: + self.trial_end_via_sender = True + self._state_cv.notify() + return + self._send(payload) def _send(self, payload): """通知は fire-and-forget。**例外を外に出さない**(codex U1). @@ -2009,10 +2089,12 @@ self._send({"type": "trial_start", "run_stem": self.run_stem}) def trial_end(self): - self._send({"type": "trial_end", "run_stem": self.run_stem}) + # 依頼 202: 送り手がまだ送り終えていなければ(close の期限切れ)、trial_end も列の最後に積む。 + # 送り手が積んだ順に送るので、states.csv の行が trial_end を追い越さない(本体は待たない)。 + self._send_state({"type": "trial_end", "run_stem": self.run_stem}, state_row=False) def stage(self, name): - self._send({"type": "stage", "stage": name}) + self._send_state({"type": "stage", "stage": name}) def gripper_cmd(self, state, note, ts=None): """build_demo_episodes.py は gripper_cmd の gripper_state を 0/1 で読む. @@ -2028,7 +2110,7 @@ payload = {"type": "gripper_cmd", "state": int(state), "note": str(note)} if ts is not None: payload["ts"] = float(ts) - self._send(payload) + self._send_state(payload) def currents_window(self, sec): """依頼 121: これから sec 秒 daemon を使わない(眠る)と電流の記録に知らせる.""" @@ -2064,11 +2146,63 @@ type(e).__name__, e)) def actual(self, angles): - self._send({"type": "actual", - "angles": [float(x) for x in angles], - "ts": time.time()}) + self._send_state({"type": "actual", + "angles": [float(x) for x in angles]}) self.actual_sent += 1 + def actual_from_currents(self, angles, ts): + """依頼 202: 電流の記録スレッド(read_servo)が読んだ角度を actual 行として送る. + + 時刻 ts は角度を読んだ時刻(currents.csv の ts と同じ)。本体の読み(到達・静止の確かめなど。 + note は空のまま)と区別するため note に CURRENT_THREAD_NOTE を入れる。 + 先に積んだ行(本体の actual・stage・gripper_cmd)より前の時刻なら積まずに数える + (states.csv の行を時刻順に保つため。電流スレッドは本体が眠っている間だけ読むので、ふつうは起きない)。 + 電流スレッドから呼ばれる。鍵の中では積むだけで送らない(送るのは送り手のスレッド)。例外は外へ出さない。 + start_current_actuals() の前と close_current_actuals() のあとは何もしない。 + """ + try: + vals = [float(x) for x in angles] + ts = float(ts) + if len(vals) != 6 or not all(math.isfinite(v) for v in vals) or not math.isfinite(ts): + raise ValueError("bad angles") + with self._state_cv: + if not self._current_actuals_open or self._queue is None: + return + if self._last_state_ts is not None and ts < self._last_state_ts: + self.actual_current_thread_late += 1 + return + self._queue.append({"type": "actual", "angles": vals, "ts": ts, + "note": CURRENT_THREAD_NOTE}) + self._last_state_ts = ts + self.actual_sent_current_thread += 1 + self._state_cv.notify() + except Exception: + self.actual_current_thread_errors += 1 # 壊れた値は積まずに数える + + def close_current_actuals(self, timeout=None): + """依頼 202: 電流の記録スレッドの角度を受け付けるのをやめ、積んだ通知を送り切る(trial_end の前に呼ぶ). + + 送り手のスレッドを待つのは timeout 秒(既定 CURRENT_ACTUALS_FLUSH_SEC)まで。送り切れたら、このあとの + 通知は今までどおりその場で送る。送り切れなければ、まだの数を current_actuals_unsent に残し、 + 積んだ分とこのあとの通知(trial_end)は送り手が順に送る(本体は待たない。追い越さない)。 + """ + timeout = CURRENT_ACTUALS_FLUSH_SEC if timeout is None else timeout + with self._state_cv: + self._current_actuals_open = False + sender = self._sender + if sender is None: + return + self._sender_stop = True + self._state_cv.notify() + sender.join(timeout=timeout) + with self._state_cv: + if not self._sender_exited: + # 期限切れ: 積んだ分は捨てずに送り手に任せる(このあとの trial_end も列の最後に積む) + self.current_actuals_flush_timeout = True + self.current_actuals_unsent = len(self._queue or []) + self._inflight + else: + self._queue = None + _STAGE_SAFE_RE = re.compile(r"[^A-Z0-9]+") @@ -2117,7 +2251,7 @@ _ACTIVE_CURRENTS.remove(cur) -def _start_currents(run_dir, power=None): +def _start_currents(run_dir, power=None, angle_sink=None): """依頼 121: 爪と肩の電流の記録スレッドを始める(失敗しても再生は止めない → None). 読み方・列・頻度は実機推論の記録(依頼 116)と同じ部品(policy_recorder.CurrentsRecorder)。 @@ -2126,12 +2260,15 @@ states.csv(recorder.py が書く)の列と行は変えない。 依頼 156: power(省略時は cmd_run が --power から入れた POWER。2A など)があれば、爪の位置と電圧の番地も読む (policy_recorder.PowerRegsRecorder → recording/power_regs.csv。読み始めてよい時の決め方は同じ)。 + 依頼 202: angle_sink(_Recorder.actual_from_currents)を渡すと、read_servo で読めた角度を + states.csv の actual 行(note = CURRENT_THREAD_NOTE)にも送る。読む命令・読む時機は変えない。 """ power = POWER if power is None else power try: _prec.install_daemon_lock() cls = _prec.PowerRegsRecorder if power else _prec.CurrentsRecorder cur = cls(run_dir, _prec.daemon_read_servo) + cur.angle_sink = angle_sink _ACTIVE_CURRENTS.append(cur) cur.start() except Exception as e: @@ -4230,7 +4367,9 @@ time.sleep(RECORDER_START_SETTLE_SEC) # 依頼 121: 爪と肩の電流を {run}/recording/currents.csv に残す(再生の間ずっと) if not getattr(args, "no_currents", False): - recorder.currents = _start_currents(run_dir) + recorder.start_current_actuals() # 依頼 202 + recorder.currents = _start_currents( + run_dir, angle_sink=recorder.actual_from_currents) else: _log("--no-currents: 爪と肩の電流は残しません") @@ -4270,7 +4409,23 @@ _warn("電流の記録を閉じるときに例外(続行): {}: {}".format( type(e).__name__, e)) if recorder is not None: + try: + recorder.close_current_actuals() # 依頼 202: 積んだ通知を trial_end の前に送り切る + except BaseException as e: + _warn("電流の記録の角度の通知を閉じるときに例外(続行): {}: {}".format( + type(e).__name__, e)) log_record["recorder_actual_events_sent"] = recorder.actual_sent + # 依頼 202: 電流の記録スレッドの角度で送った actual 行(上の数には入れない) + log_record["recorder_actual_events_sent_current_thread"] = ( + recorder.actual_sent_current_thread) + log_record["recorder_actual_current_thread_late"] = ( + recorder.actual_current_thread_late) + log_record["recorder_actual_current_thread_errors"] = ( + recorder.actual_current_thread_errors) + log_record["recorder_state_ts_backwards"] = recorder.state_ts_backwards + log_record["recorder_current_actuals_unsent"] = recorder.current_actuals_unsent + log_record["recorder_current_actuals_flush_timeout"] = ( + recorder.current_actuals_flush_timeout) log_record["recorder_send_failures"] = recorder.send_failures log_record["recorder_last_send_error"] = ( recorder.last_send_error) @@ -4279,6 +4434,9 @@ except BaseException as e: _warn("recorder への trial_end 通知で例外(続行): {}: {}".format( type(e).__name__, e)) + # 依頼 202: 送り手が送り終えた数(trial_end の直後の値)と、trial_end も送り手に任せたか(ふつうは False) + log_record["recorder_state_rows_sent_by_sender"] = recorder.state_rows_sent_by_sender + log_record["recorder_trial_end_via_sender"] = recorder.trial_end_via_sender result = log_record.get("result") or result print("=" * 78)