--- dynamic_tracking_limits/drag_teaching/record_teach_demo.py.bak_pre_req213_20261115 2026-11-15 00:26:31.699619895 +0000 +++ dynamic_tracking_limits/drag_teaching/record_teach_demo.py 2026-11-15 09:21:53.001091052 +0000 @@ -220,6 +220,19 @@ # あと wait 秒 sleep してから送信後の角度を読む。wait=0 だと書き込み直後に読むため # 読み直し 12 回(≒3.7s)に化けやすい(段階C 1回目の lift が実測 3.7196s)。 SEND_WAIT_SEC = rpp.SEND_WAIT_SEC # 1.0 +# 依頼 213(依頼 197 の案 2・エイイチ判断 2026-11-15): 送信のあとの 1.0 秒を **どこで待つか**。 +# "mac"(既定): daemon には wait=0 で送り、返ってから Mac 側で SEND_WAIT_SEC 待つ。 +# この間は電流の記録スレッドが読める(_sleep_open で眠りを知らせる)。 +# "daemon" : 依頼 202 までと同じ。daemon が wait=SEND_WAIT_SEC で寝てから返す(この間は読めない)。 +# 待つ長さ(1.0 秒・DAY266: 短縮しない)は変えない。場所だけを移す。cmd_run が --send-wait-on の値を入れる。 +SEND_WAIT_ON_CHOICES = ("mac", "daemon") +SEND_WAIT_ON = "mac" +# 送信のソケットの期限。"daemon" では visual_servo_pick._daemon_request が wait+3.0 = 4.0 秒にする。 +# "mac" の wait=0 では既定 DAEMON_TIMEOUT_SEC(35 秒)になってしまうので、同じ 4.0 秒を timeout_override で渡す。 +SEND_SOCKET_TIMEOUT_SEC = SEND_WAIT_SEC + 3.0 +# 直近の _send_angles_guarded の daemon とのやりとりだけの往復(Mac 側の待ちを含まない)。"daemon" では +# daemon の中の 1.0 秒を含む。区間の記録に残す(send_rtt_sec はどちらでも待ちを含む送信の始めから終わりまで)。 +_LAST_SEND = {"daemon_rtt_sec": None} FIRST_MOVE_WARN_DEG = wpsr.FIRST_MOVE_WARN_DEG # 30.0 # 2026-10-19(エイイチ判断・項目1): 一律 8.0° はやめた。 @@ -2699,18 +2712,21 @@ _log("daemon の新 cmd 確認 OK(send_angles_fast / get_angles_once)") -def _send_angles_guarded(pose, speed, where): +def _send_angles_guarded(pose, speed, where, recorder=None): """送信直前の backstop(codex R1 C2). 送る姿勢ごとに「有限値 → JOINT_LIMITS → J4_SAFE」を確かめ、 外れていたら **送らずに** ABORT する。クランプして黙って姿勢を変えることはしない (教示姿勢を勝手に書き換えない)。 計画時に全姿勢を検査済みでも、ここが最後の砦として全送信を通る。 + 依頼 213: SEND_WAIT_ON が "mac" なら、送ったあとの SEND_WAIT_SEC をここ(Mac 側)で待ってから返る。 + recorder を渡すとその間を電流の記録スレッドに知らせる(読める)。 """ reason = _pose_violation(pose) if reason is not None: raise _Aborted(where, "送信直前の姿勢検査で弾きました(送信していません): " + reason) + _LAST_SEND["daemon_rtt_sec"] = None if USE_FAST_DAEMON_CMDS: # LOG352: 読み直し無しの送信(Pi 直と同じ送り方) return daemon_send_angles_fast( @@ -2733,7 +2749,37 @@ # アームは目標まで動き続けうる。止めるのは電源を切ることだけ #(2026-10-18 からの運用)。周囲の空間を空けて監視下で走らせること。 # 送信回数は増やさない。待つのは daemon 側の 1 回だけ。 - return daemon_send_angles(pose, speed, wait=SEND_WAIT_SEC) + if SEND_WAIT_ON == "daemon": + t0 = time.monotonic() + try: + return daemon_send_angles(pose, speed, wait=SEND_WAIT_SEC) + finally: + _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0 # 例外でも残す(codex 2 回目 R2) + # 依頼 213("mac"): daemon は wait=0 で、書き込みのあと 1 回だけ角度を読んで(read_angles_once_for_log) + # すぐ返す。返ってから Mac 側で同じ SEND_WAIT_SEC 待つので、到達の確かめが始まる時刻は今までと同じ + # (書き込み → 1.0 秒 → 読み 1 回 → 返事、が 書き込み → 読み 1 回 → 返事 → 1.0 秒 になる)。 + # 送信は install_daemon_lock の鍵の中の 1 回のやりとりなので、電流の記録スレッドの読みは + # 送信の途中に割り込まない(返事が来て鍵が外れてから、眠りを知らされた間だけ読む)。 + # 送信が例外でも 1.0 秒は待つ(届いたかは分からないので、待ちを縮めずに到達の確かめへ進む)。 + t0 = time.monotonic() + try: + resp = daemon_send_angles(pose, speed, wait=0, + timeout_override=SEND_SOCKET_TIMEOUT_SEC) + except Exception: + _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0 + _send_wait_on_mac(recorder) + raise + _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0 + _send_wait_on_mac(recorder) + return resp + + +def _send_wait_on_mac(recorder): + """依頼 213: 送ったあとの SEND_WAIT_SEC(1.0 秒)を Mac 側で待つ。**長さは縮めない**(time.sleep と同じ).""" + if recorder is not None: + _sleep_open(recorder, SEND_WAIT_SEC) + else: + time.sleep(SEND_WAIT_SEC) def _note_send_failure(streak, label, detail, exc): @@ -2832,6 +2878,8 @@ t_first_send = None # 送信の計時は送信直前に始める(R11b T5) first_rtt = None first_rtt_retry = None + first_daemon_rtt = None # 依頼 213: daemon とのやりとりだけの往復(Mac 側の待ちを含まない) + first_daemon_rtt_retry = None first_arrived = None first_max = None first_actual = None @@ -2877,7 +2925,9 @@ first_send_attempted = True try: t_first_send = time.monotonic() - _send_angles_guarded(HOME_ANGLES, SPEED_FIRST, "step1_HOME_START") + _send_angles_guarded(HOME_ANGLES, SPEED_FIRST, "step1_HOME_START", + recorder=recorder) + first_daemon_rtt = _LAST_SEND["daemon_rtt_sec"] first_rtt = time.monotonic() - t_first_send log_record["first_move_send_rtt_sec"] = round(first_rtt, 4) streak["consecutive_send_failures"] = 0 @@ -2886,6 +2936,7 @@ raise except Exception as e: # 2026-10-19 方針変更: 送信の例外でも止めない + first_daemon_rtt = _LAST_SEND["daemon_rtt_sec"] # 依頼 213 codex R2: 失敗でも残す _note_send_failure(streak, "先頭移動 (current → HOME)", "step1_HOME_START", e) _warn("先頭移動の send_angles が失敗しました。止めません。" @@ -2970,7 +3021,9 @@ _t_retry = time.monotonic() try: _send_angles_guarded(HOME_ANGLES, SPEED_FIRST, - "step1_HOME_START_retry") + "step1_HOME_START_retry", + recorder=recorder) + first_daemon_rtt_retry = _LAST_SEND["daemon_rtt_sec"] first_rtt_retry = time.monotonic() - _t_retry _log("再送 OK(daemon 往復 {:.3f}s)".format(first_rtt_retry)) except _Aborted: @@ -2978,6 +3031,7 @@ first_aborted_at = "retry_send_backstop" raise except Exception as e: + first_daemon_rtt_retry = _LAST_SEND["daemon_rtt_sec"] _note_send_failure(streak, "先頭移動 (current → HOME)", "step1_HOME_START_retry", e) _warn("先頭移動の再送 send_angles が失敗しました。" @@ -3074,6 +3128,12 @@ "warnings": list(first_warnings), "send_rtt_retry_sec": (round(first_rtt_retry, 4) if first_rtt_retry is not None else None), + # 依頼 213: 送信のあとの 1.0 秒をどこで待ったか と daemon とのやりとりだけの往復 + "send_wait_on": SEND_WAIT_ON, + "send_daemon_rtt_sec": (round(first_daemon_rtt, 4) + if first_daemon_rtt is not None else None), + "send_daemon_rtt_retry_sec": (round(first_daemon_rtt_retry, 4) + if first_daemon_rtt_retry is not None else None), }) # codex R11 A1: 爪の直前の静止確認に「直前の move の目標と許容差」を使う。 @@ -3281,6 +3341,8 @@ send_failures = 0 send_rtt_sec = None send_rtt_retry_sec = None + send_daemon_rtt_sec = None # 依頼 213 + send_daemon_rtt_retry_sec = None arrived = None last_max = None last_actual = None @@ -3317,7 +3379,9 @@ try: _send_angles_guarded( s["to"], SPEED_SINGLE, - "step{}_{}".format(step_no, _stage_name(s["to_name"]))) + "step{}_{}".format(step_no, _stage_name(s["to_name"])), + recorder=recorder) + send_daemon_rtt_sec = _LAST_SEND["daemon_rtt_sec"] send_rtt_sec = time.monotonic() - t_send send_rtts.append(send_rtt_sec) streak["consecutive_send_failures"] = 0 @@ -3332,6 +3396,7 @@ # 分かる**ので、そこで判断する(届いていなければ # 「1.0° 未満 → 再送 → それでも 1.0° 未満」で ABORT になる)。 send_failures += 1 + send_daemon_rtt_sec = _LAST_SEND["daemon_rtt_sec"] # 依頼 213 codex R2: 失敗でも残す _note_send_failure(streak, label, "単発送信", e) _warn("send_angles が失敗しました(区間 {})。止めません。" "到達確認で動いたかを見ます: {}: {}".format( @@ -3427,7 +3492,9 @@ _send_angles_guarded( s["to"], SPEED_SINGLE, "step{}_{}_retry".format( - step_no, _stage_name(s["to_name"]))) + step_no, _stage_name(s["to_name"])), + recorder=recorder) + send_daemon_rtt_retry_sec = _LAST_SEND["daemon_rtt_sec"] send_rtt_retry_sec = time.monotonic() - t_send_retry _log("再送 OK(daemon 往復 {:.3f}s)".format( send_rtt_retry_sec)) @@ -3438,6 +3505,7 @@ raise except Exception as e: send_failures += 1 + send_daemon_rtt_retry_sec = _LAST_SEND["daemon_rtt_sec"] _note_send_failure(streak, label, "再送", e) _warn("再送の send_angles が失敗しました。止めません。" "動いたかを到達確認で見ます: {}: {}".format( @@ -3541,6 +3609,15 @@ "send_rtt_retry_sec": (round(send_rtt_retry_sec, 4) if send_rtt_retry_sec is not None else None), + # 依頼 213: send_rtt_sec は どちらの待ち方でも 1.0 秒の待ちを含む(送信の始めから + # 到達の確かめに入るまで)。send_daemon_rtt_sec は daemon とのやりとりだけ + # ("mac" では待ちを含まない・"daemon" では含む) + "send_wait_on": SEND_WAIT_ON, + "send_daemon_rtt_sec": (round(send_daemon_rtt_sec, 4) + if send_daemon_rtt_sec is not None else None), + "send_daemon_rtt_retry_sec": (round(send_daemon_rtt_retry_sec, 4) + if send_daemon_rtt_retry_sec is not None + else None), # 2026-10-19 方針変更: 止めずに続けた事象はここに残す "warnings": list(seg_warnings), }) @@ -3860,7 +3937,7 @@ def cmd_run(args): - global GRIP_VALUE, POWER + global GRIP_VALUE, POWER, SEND_WAIT_ON _note_tol_source() # 依頼 152: 爪の閉じる値(計画の表示・再生の閉じる指令・試行 JSON がすべてこの値を使う) @@ -3885,6 +3962,18 @@ _log("電源: {}(--power。試行 JSON の power に残し、爪の位置と電圧も読みます)".format(power)) POWER = power + # 依頼 213: 送信のあとの 1.0 秒をどこで待つか(長さは変えない) + send_wait_on = getattr(args, "send_wait_on", None) or "mac" + if send_wait_on not in SEND_WAIT_ON_CHOICES: + _abort_msg("--send-wait-on は {} のどれかで指定してください(指定: {})".format( + " / ".join(SEND_WAIT_ON_CHOICES), send_wait_on)) + sys.exit(2) + SEND_WAIT_ON = send_wait_on + _log("送信のあとの {:.1f}s の待ち: {}".format( + SEND_WAIT_SEC, + "Mac 側(daemon へは wait=0。この間も電流の記録スレッドが読みます)" if SEND_WAIT_ON == "mac" + else "daemon 側(依頼 202 までと同じ。この間は読みません)")) + if args.teach_local and args.execute: _abort_msg("--teach-local は ドライラン専用です(--execute とは併用できません)。" "実機では Pi から取得した教えた記録だけを再生します") @@ -4196,6 +4285,9 @@ "grip_value": GRIP_VALUE, # 依頼 156: 電源の種類(--power。付けなければ None) "power": POWER, + # 依頼 213: 送信のあとの 1.0 秒をどこで待ったか(mac = Mac 側・電流の記録スレッドが読める / + # daemon = 依頼 202 までと同じ) + "send_wait_on": SEND_WAIT_ON, # 肩が上限を超えた姿勢の置き換え(計画段階で決まったもの) "shoulder_fix": shoulder_fix_payload, "teach": { @@ -4300,10 +4392,14 @@ "DAEMON_SET_GRIPPER_PRE_SLEEP_SEC": DAEMON_SET_GRIPPER_PRE_SLEEP_SEC, "gripper_open_check": "none (gv unused / no pre-start y/n)", "SEND_WAIT_SEC": SEND_WAIT_SEC, - "send_socket_timeout_sec": SEND_WAIT_SEC + 3.0, + "send_wait_on": SEND_WAIT_ON, + "send_socket_timeout_sec": SEND_SOCKET_TIMEOUT_SEC, "send_socket_timeout_source": ( "visual_servo_pick._daemon_request: wait>0 なら wait+3.0。" - "timeout_override は短縮しかできないので渡していない"), + "timeout_override は短縮しかできないので渡していない" + if SEND_WAIT_ON == "daemon" else + "依頼 213: daemon へは wait=0 で送り、SEND_WAIT_SEC は Mac 側で待つ。" + "期限は daemon で待っていたときと同じ wait+3.0 を timeout_override で渡す"), "GRIPPER_STILL_TOL_DEG": GRIPPER_STILL_TOL_DEG, "GRIPPER_STILL_READS": GRIPPER_STILL_READS, "GRIPPER_STILL_TIMEOUT_SEC": GRIPPER_STILL_TIMEOUT_SEC, @@ -4519,6 +4615,10 @@ ap_r.add_argument("--power", default=None, help="電源の種類(例 2A / 5A。試行 JSON の power に残し、爪の位置と電圧の番地も " "recording/power_regs.csv に読む。動き・待ち時間は変えない。依頼 156。pi05/POWER_AB.md)") + ap_r.add_argument("--send-wait-on", choices=SEND_WAIT_ON_CHOICES, default="mac", + help="送信のあとの {:.1f} 秒をどこで待つか(既定 mac: daemon へは wait=0 で送り Mac 側で待つ。" + "この間も電流の記録スレッドが角度を読む。daemon: 依頼 202 までと同じ)。" + "待つ長さは変えない。試行 JSON の send_wait_on に残す。依頼 213".format(SEND_WAIT_SEC)) ap_r.add_argument("--repeat", type=int, default=MEASURE_REPEAT_DEFAULT, help="再生直前の検出回数(既定 {})".format(MEASURE_REPEAT_DEFAULT)) ap_r.add_argument("--interval", type=float, default=MEASURE_INTERVAL_DEFAULT,