commit 05317174b9bd7eec825671d72ebf99b6f90e5231 Author: Eiichi Shimazu Date: Tue Nov 10 17:40:03 2026 +0900 上カメラの傾き (マットの下の辺 左下→右下 の角度. プラス=右下がり) を数字で出す pi05/camera_tilt.py を足す (tilt_deg は count_by_position.detect_corners の下の 2 つの角を使い, 取れなければ None. --dataset で各エピソードの 1 枚目を位置ごとに 中央値・最小〜最大 の表に, --live で上カメラ 1 枚の傾きとお手本の全体の中央値との差, --video で動画の 1 コマ目. 何も書き出さない). camera_align.py の画面と [ALIGN ] の行に 今の傾き・お手本の中央値・差 を出し, 差が ±0.5° を超えたら色を変えて TILT DIFFERS と出す (止めない. お手本は --tilt-raw-dir, 未指定なら --raw-dir). lab_start.sh は --tilt-raw-dir に安全範囲と同じ raw_episodes (deep1 なら deep_even10_v1) を渡す. run_policy_client.py の [CAMERA] に傾きと差を 1 行 (表示だけ・判定と停止は変えない). テストを足す (test_camera_tilt 8・lab_start 178) (依頼 142. codex 2 回) Co-Authored-By: Claude Opus 5.5 diff --git a/lab_start.sh b/lab_start.sh index 8c18032..bd1ce6a 100755 --- a/lab_start.sh +++ b/lab_start.sh @@ -25,6 +25,8 @@ # raw_episodes (corr_v3・corr_v4 と合わせても範囲はほぼ同じで, trim2 の方が狭い. corr_v5 は 11-06 の 184650 の j2 が # trim2 の上限 +31.4° を最大 11° 超える). 位置合わせ (camera_align.py) のお手本と, 実機推論の写り方の確かめの基準画像 # (run_policy_client.py --framing-raw-dir. 依頼 137) はどれも trim2 の raw_episodes +# 上カメラの傾きのお手本 (camera_align.py --tilt-raw-dir・run_policy_client.py の [CAMERA] の傾き. 依頼 142) は +# 安全範囲と同じ raw_episodes (deep1・deep1b なら deep_even10_v1). 表示だけで止めない # つなぎ直しで, 残っている Pod のサーバーが別のモデルで動いていれば, そのことを出して止まる # # 1. Pod を作る (A40×1・Runpod Pytorch 2.8.0・コンテナのディスク 200GB・22/tcp を外へ出す) @@ -115,6 +117,7 @@ RAW_RANGE=/Volumes/KIOXIA/r2r2r-data/datasets/$MODEL_RANGE_DATA/raw_episodes [ -d "$RAW_RANGE" ] || { echo "[ERROR] 安全範囲を作る学習データがありません: $RAW_RANGE" >&2; exit 1; } echo "[RANGE ] 安全範囲 (学習データ ±10°) は $RAW_RANGE から (--model $MODEL)" echo "[FRAME ] 写り方の確かめの基準画像は $RAW の最初のエピソードから (位置合わせと同じ trim2. 依頼 137)" +echo "[TILT ] 上カメラの傾きのお手本 (各エピソードの 1 枚目の中央値) は $RAW_RANGE から (安全範囲と同じ. 表示だけ. 依頼 142)" lab_ssh_opts HF_REPO=$MODEL_REPO # --model で選んだモデル (lab_lib.sh の lab_model_def) @@ -448,7 +451,7 @@ while :; do python3 dynamic_tracking_limits/place/place_modules/08_home_from_anywhere.py || fail "HOME に戻せない" python3 -c "from visual_servo_pick import get_daemon_socket, daemon_set_gripper, close_daemon_socket; get_daemon_socket(); daemon_set_gripper(0); close_daemon_socket()" \ || fail "爪を開けない" - python3 pi05/camera_align.py --raw-dir "$RAW" --episode "$EPISODE" --scale 5 --alpha 0.7 + python3 pi05/camera_align.py --raw-dir "$RAW" --episode "$EPISODE" --scale 5 --alpha 0.7 --tilt-raw-dir "$RAW_RANGE" rc=$? [ $rc = 0 ] || [ $rc = 130 ] || fail "camera_align.py が失敗 (終了コード $rc)" stage_done diff --git a/pi05/camera_align.py b/pi05/camera_align.py index 93c785e..4e3c4af 100644 --- a/pi05/camera_align.py +++ b/pi05/camera_align.py @@ -18,6 +18,10 @@ iPhone を付けたあと, この画面で写り方を合わせてから実機 OK しきい値以内 ZURE dx か dy がしきい値の外 (= ずれ) NO MATCH 相関が弱く dx/dy が当てにならない (= 測れない) + その下に上カメラの傾き (マットの下の辺. プラス=右下がり・camera_tilt.py) と, お手本の傾きの中央値・差を出す + (依頼 142). 差が ±0.5° を超えたら色を変えて「TILT DIFFERS」(傾きがお手本と違う) と出す. 止めはしない. + 傾きのお手本は --tilt-raw-dir (未指定なら --raw-dir) の各エピソードの 1 枚目の中央値. + lab_start.sh は安全範囲と同じ raw_episodes (deep1 なら deep_even10_v1) を渡す. q または Esc で終了. **表示だけで, ファイルには何も書かない.** run_policy_client.py は開始時に この measure_framing / framing_verdict で同じ検査をして, @@ -32,6 +36,7 @@ Usage: python3 pi05/camera_align.py --raw-dir $RAW --episode run_0001_20261019_192343 # 位置を変える python3 pi05/camera_align.py --raw-dir $RAW --episode 192343 # 部分一致でもよい python3 pi05/camera_align.py --raw-dir $RAW --alpha 0.7 --scale 2 # 基準を濃く / 画面を小さく + python3 pi05/camera_align.py --raw-dir $RAW --tilt-raw-dir $DEEP # 傾きのお手本を別のデータにする """ import argparse import sys @@ -48,6 +53,7 @@ for p in (_HERE, _ROOT, _PLACE): sys.path.insert(0, str(p)) import camera_resolver # noqa: E402 +from camera_tilt import TILT_WARN_DEG, reference_tilt, tilt_deg, tilt_text # noqa: E402 IMG_LONG = 384 # build_demo_episodes.py / run_policy_client.py と同じ (学習画像の長辺) FRAMING_MAX_DY = 20.0 # |dy| がこれを超えたら「ずれ」. 下の実測値の間を取った @@ -182,7 +188,7 @@ def _put(img, text, org, scale, color, thick): cv2.putText(img, text, org, cv2.FONT_HERSHEY_SIMPLEX, scale, color, thick, cv2.LINE_AA) -def draw_overlay(small_bgr, ref_bgr, dx, dy, resp, alpha, scale, ref_label=""): +def draw_overlay(small_bgr, ref_bgr, dx, dy, resp, alpha, scale, ref_label="", tilt=None, tilt_ref=None): ref = ref_bgr if ref.shape[:2] != small_bgr.shape[:2]: ref = cv2.resize(ref, (small_bgr.shape[1], small_bgr.shape[0]), interpolation=cv2.INTER_AREA) @@ -196,6 +202,18 @@ def draw_overlay(small_bgr, ref_bgr, dx, dy, resp, alpha, scale, ref_label=""): _put(big, f"dy={dy:+6.1f} dx={dx:+6.1f} (OK: both <= {FRAMING_MAX_DY:.0f})", (16, 92), 0.6, color, 2) _put(big, f"resp={resp:.3f} (OK: >= {FRAMING_MIN_RESPONSE:.2f}) q: quit", (16, 116), 0.6, (230, 230, 230), 1) + # 傾き (依頼 142. 表示だけ. 写り方の判定には入れない) + if tilt is None: + _put(big, "tilt: NO CORNERS (mat corners not found)", (16, 140), 0.6, (0, 200, 255), 2) + elif tilt_ref is None: + _put(big, f"tilt={tilt:+.2f} deg (+ = right side down) ref: n/a", (16, 140), 0.6, (230, 230, 230), 1) + else: + d = tilt - tilt_ref + warn = abs(d) > TILT_WARN_DEG + _put(big, f"tilt={tilt:+.2f} ref={tilt_ref:+.2f} diff={d:+.2f} deg (+ = right side down)", (16, 140), + 0.6, (0, 200, 255) if warn else (0, 200, 0), 2) + if warn: + _put(big, f"TILT DIFFERS (> {TILT_WARN_DEG:g} deg)", (16, 176), 1.0, (0, 200, 255), 3) if ref_label: # どのお手本を重ねているか (ブロックの位置を合わせるときの目印) _put(big, f"ref: {ref_label}", (16, big.shape[0] - 14), 0.6, (230, 230, 230), 1) return big @@ -209,6 +227,9 @@ def main(): help="基準にするエピソード名 (例 run_0001_20261019_192343. " "1 本だけに当てはまる部分一致 192343 でもよい). " "未指定なら最初のエピソード") + ap.add_argument("--tilt-raw-dir", default=None, + help="傾きのお手本 (各エピソードの 1 枚目の傾きの中央値) を取る学習エピソード. " + "未指定なら --raw-dir (依頼 142)") ap.add_argument("--alpha", type=float, default=0.5, help="基準画像の重ね具合 (0=ライブのみ / 1=基準のみ)") ap.add_argument("--scale", type=float, default=3.0, help="画面の拡大倍率") args = ap.parse_args() @@ -222,10 +243,16 @@ def main(): print(f"[REF ] お手本 {ep_dir.name} ({pos})" + ("" if args.episode else " ← --episode 未指定なので最初のエピソード")) print("[REF ] 重なった画像のブロックに実物のブロックを合わせると, お手本と同じ位置に置ける") + tilt_dir = Path(args.tilt_raw_dir or args.raw_dir) + tilt_ref, n_ok, n_all = reference_tilt(tilt_dir) + print(f"[TILT ] 傾きのお手本: {tilt_dir} 中央値 " + + ("測れない" if tilt_ref is None else f"{tilt_ref:+.2f}°") + f" (測れた {n_ok}/{n_all} 本. プラス=右下がり)") + print(f"[TILT ] 傾きの差が ±{TILT_WARN_DEG:g}° を超えたら画面の色を変える (表示だけ. 止めない)") print(f"[ALIGN ] |dy| <= {FRAMING_MAX_DY:.0f} なら OK. アームは HOME に置いた状態で合わせる. " "q または Esc で終了 (ファイルには何も書かない)") cap = open_top_camera() last = None + last_tilt = None try: while True: for _ in range(2): # バッファに溜まった古いフレームを捨てる @@ -237,13 +264,16 @@ def main(): continue small = to_small_bgr(frame) dx, dy, resp = measure_framing(ref_bgr, small) + tilt = tilt_deg(small) # 学習画像と同じ大きさで測る (お手本の中央値と同じ条件) cv2.imshow(WINDOW, draw_overlay(small, ref_bgr, dx, dy, resp, - args.alpha, args.scale, ref_label)) - if last is None or abs(dy - last) > 1.0: # 端末は値が動いたときだけ出す + args.alpha, args.scale, ref_label, tilt, tilt_ref)) + tilt_moved = (tilt is None) != (last_tilt is None) or ( + tilt is not None and abs(tilt - last_tilt) > 0.15) + if last is None or abs(dy - last) > 1.0 or tilt_moved: # 端末は値が動いたときだけ出す verdict, why = framing_verdict(dx, dy, resp) print(f"[ALIGN ] dy={dy:+6.1f} dx={dx:+6.1f} response={resp:.3f} " - f"→ {verdict} ({why})") - last = dy + f"→ {verdict} ({why}) {tilt_text(tilt, tilt_ref)[0]}") + last, last_tilt = dy, tilt if cv2.waitKey(30) & 0xFF in (ord("q"), 27): break except KeyboardInterrupt: diff --git a/pi05/camera_tilt.py b/pi05/camera_tilt.py new file mode 100644 index 0000000..a7483e0 --- /dev/null +++ b/pi05/camera_tilt.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +"""camera_tilt.py — 上カメラの傾き (回転) を数字で出す (2026-11-10 依頼 142・Log28). + +2026-11-10 の deep1 の実機推論 (Log28) で位置 2 が 4 回ともつかめなかった. 上カメラのマットの下の辺の傾きは +学習データ deep_even10_v1 の位置 1 (11/09 午前の 9 本) だけ 右下がり 約 +0.5°, 位置 2〜5 と 11/10 の 1 回目 +(111034・成功) は 右上がり 約 −0.5〜−1.6°, 11/10 の 112346 以降 (iPhone を付け直したあと) は 右下がり 約 +0.7° だった. +camera_align.py の写り方の確かめ (phaseCorrelate の dx/dy) は平行移動しか見ないので, 1.7° の回転は OK のまま通る. + +傾きの測り方 (tilt_deg): + count_by_position.detect_corners (mat_corner_detection) でマットの四隅を取る (左上・右上・右下・左下の順). + 上の 2 つは画面の上 (y=0) で切れるので使わず, 下の 2 つ (左下 BL・右下 BR) を結ぶ線の角度を傾きとする. + 度で, プラス = 右下がり (画像の y は下向きなので atan2(BR.y − BL.y, BR.x − BL.x)). + 四隅が取れないときは None (= 測れない). 1920×1080 の生の画像でも学習データの小さい画像 (長辺 384) でも測れる. + 小さい画像では角が 1px 単位なので, 1 枚の値は ±0.2° ほど揺れる. + +使い方 (読むだけ. 何も書き出さない. datasets/ にも書かない): + python3 pi05/camera_tilt.py --dataset mecharm_redblock_box_deep_even10_v1 + 各エピソードの 1 枚目の傾きを, 位置 (count_by_position.classify) ごとに 中央値・最小〜最大 で表にする + python3 pi05/camera_tilt.py --live --dataset mecharm_redblock_box_deep_even10_v1 + 上カメラから 1 枚取り, 今の傾きと --dataset の全体の中央値との差を出す + python3 pi05/camera_tilt.py --video runs/pi05_policy/pi05_20261110_111034/recording/top.mp4 ... + 動画の 1 コマ目の傾き (記録の見直し用. いくつでも) + +camera_align.py (位置合わせの画面) と run_policy_client.py ([CAMERA] の行) も reference_tilt / tilt_deg で +今の傾きとお手本の中央値との差を出す (表示だけ. 止めない). +""" +import argparse +import math +import statistics +import sys +from pathlib import Path + +import cv2 + +_HERE = Path(__file__).resolve() +sys.path.insert(0, str(_HERE.parent)) +sys.path.insert(0, str(_HERE.parent.parent)) + +TILT_WARN_DEG = 0.5 # 今の傾きとお手本の中央値の差がこれを超えたら目立たせる (表示だけ. 止めない) + + +def tilt_deg(frame, corners_fn=None): + """マットの下の辺 (左下 → 右下) の傾き [度]. プラス = 右下がり. 四隅が取れなければ None.""" + if frame is None: + return None + if corners_fn is None: + from count_by_position import detect_corners as corners_fn + try: + c = corners_fn(frame) + except Exception: # 見つけ方で落ちても「測れない」にする + return None + if c is None or len(c) != 4: + return None + (blx, bly), (brx, bry) = c[3], c[2] + dx, dy = float(brx) - float(blx), float(bry) - float(bly) + if dx <= 0: # 左下が右下より右 = 四隅の取り違え + return None + return math.degrees(math.atan2(dy, dx)) + + +def first_frame(ep_dir): + """エピソードの 1 枚目 (frames/ の最初の jpg). 読めなければ None.""" + fs = sorted((Path(ep_dir) / "frames").glob("*.jpg")) + return cv2.imread(str(fs[0])) if fs else None + + +def video_first_frame(path): + cap = cv2.VideoCapture(str(path)) + try: + ok, f = cap.read() + finally: + cap.release() + return f if ok else None + + +def episode_tilts(raw_dir, corners_fn=None): + """raw_episodes の各エピソードの 1 枚目の傾き. 戻り値 [(エピソードのフォルダ, 傾き or None)].""" + eps = sorted(p.parent for p in Path(raw_dir).glob("[!.]*/episode.npz")) + return [(ep, tilt_deg(first_frame(ep), corners_fn)) for ep in eps] + + +def reference_tilt(raw_dir, corners_fn=None): + """お手本の傾きの中央値 (各エピソードの 1 枚目). 戻り値 (中央値 or None, 測れた本数, 全本数).""" + ts = [t for _ep, t in episode_tilts(raw_dir, corners_fn)] + ok = [t for t in ts if t is not None] + return (statistics.median(ok) if ok else None), len(ok), len(ts) + + +def tilt_text(cur, ref): + """端末用の 1 行の中身 (日本語). 戻り値 (文字, 差が線を超えたか).""" + if cur is None: + return "傾き 測れない (マットの四隅が取れない)", False + s = f"傾き {cur:+.2f}° (プラス=右下がり)" + if ref is None: + return s + " / お手本の中央値 測れない", False + d = cur - ref + warn = abs(d) > TILT_WARN_DEG + s += f" / お手本の中央値 {ref:+.2f}° / 差 {d:+.2f}°" + s += f" → 傾きがお手本と違う (±{TILT_WARN_DEG:g}° を超える)" if warn else f" (±{TILT_WARN_DEG:g}° 以内)" + return s, warn + + +def _fmt(t): + return "測れない" if t is None else f"{t:+.2f}" + + +def dataset_table(raw_dir, data_root, corners_fn=None, log=print): + """位置ごとの 中央値・最小〜最大 の表 (Markdown の行) と全体の中央値.""" + import json + + import count_by_position as cbp + runs_root = Path(data_root) / "runs" + by_pos = {} + for ep, t in episode_tilts(raw_dir, corners_fn): + meta = json.loads((ep / "meta.json").read_text()) if (ep / "meta.json").exists() else {} + stem = meta.get("run_stem", ep.name) + pos = cbp.classify(stem, meta, runs_root)["position"] + by_pos.setdefault(pos, []).append(t) + log(f"[TILT] {ep.name} 位置 {pos} {_fmt(t)}°") + lines = ["| 位置 | 本数 | 測れた | 中央値 (°) | 最小〜最大 (°) |", "| --- | --- | --- | --- | --- |"] + keys = [k for k in list(cbp.POSITIONS) + [cbp.UNKNOWN] if k in by_pos] + alls = [] + for k in keys: + ok = [t for t in by_pos[k] if t is not None] + alls += ok + name = f"位置 {k}" if k != cbp.UNKNOWN else "分からない" + if ok: + lines.append(f"| {name} | {len(by_pos[k])} | {len(ok)} | {statistics.median(ok):+.2f} | " + f"{min(ok):+.2f} 〜 {max(ok):+.2f} |") + else: + lines.append(f"| {name} | {len(by_pos[k])} | 0 | — | — |") + n = sum(len(v) for v in by_pos.values()) + med = statistics.median(alls) if alls else None + if alls: + lines.append(f"| 全体 | {n} | {len(alls)} | {med:+.2f} | {min(alls):+.2f} 〜 {max(alls):+.2f} |") + else: + lines.append(f"| 全体 | {n} | 0 | — | — |") + return lines, med + + +def live_tilt(): + """上カメラ (camera_resolver) から 1 枚取って傾きを測る (生の大きさのまま).""" + import camera_align + cap = camera_align.open_top_camera() + try: + for _ in range(3): # バッファに溜まった古いフレームを捨てる + cap.grab() + ok, f = cap.read() + finally: + cap.release() + if not ok or f is None: + sys.exit("[ERROR] 上カメラの 1 枚を撮れない") + return tilt_deg(f), f.shape + + +def main(): + import count_by_position as cbp + ap = argparse.ArgumentParser(description="上カメラの傾き (マットの下の辺. プラス=右下がり) を出す. 読むだけ") + ap.add_argument("--dataset", help="データセット名 (datasets/ の下) か raw_episodes のフォルダ") + ap.add_argument("--live", action="store_true", help="上カメラから 1 枚取って今の傾きを出す (--dataset があれば差も)") + ap.add_argument("--video", nargs="*", default=[], help="動画 (top.mp4) の 1 コマ目の傾きを出す") + ap.add_argument("--data-root", default=str(cbp.DATA_ROOT), help=argparse.SUPPRESS) + args = ap.parse_args() + if not (args.dataset or args.live or args.video): + ap.error("--dataset / --live / --video のどれかを付ける") + + ref = None + if args.dataset: + raw, _ds = cbp.resolve_input(args.dataset, args.data_root) + if not raw.is_dir(): + sys.exit(f"[ABORT] raw_episodes がない: {raw}") + if args.live: # 今の傾きとの差だけなら位置分けは要らない (速い) + ref, n_ok, n = reference_tilt(raw) + print(f"[REF ] {raw} 全体の中央値 {_fmt(ref)}° (測れた {n_ok}/{n} 本)") + else: + lines, ref = dataset_table(raw, args.data_root) + print() + print("\n".join(lines)) + for v in args.video: + t = tilt_deg(video_first_frame(v)) + s, _w = tilt_text(t, ref) if ref is not None else (f"傾き {_fmt(t)}°", False) + print(f"[VIDEO] {v} {s}") + if args.live: + t, shape = live_tilt() + s, warn = tilt_text(t, ref) + print(f"[LIVE] 上カメラ ({shape[1]}×{shape[0]}) {s}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pi05/run_policy_client.py b/pi05/run_policy_client.py index 8ac31ec..2b879ca 100644 --- a/pi05/run_policy_client.py +++ b/pi05/run_policy_client.py @@ -135,6 +135,7 @@ import waypoint_replay as wr # noqa: E402 from camera_align import ( # noqa: E402 FRAMING_MAX_DX, FRAMING_MAX_DY, FRAMING_MIN_RESPONSE, framing_verdict, load_reference, measure_framing, ) +from camera_tilt import reference_tilt, tilt_deg, tilt_text # noqa: E402 依頼 142 from fk_lookup import J1_SAFE_MAX # noqa: E402 88.0. 本線と二重定義しない from j0_sweep_scan import SEND_WAIT_SEC, SPEED, _send_and_wait_arrival # noqa: E402 本線と同じ 20 / 1.0 (待ちは下の SEND_WAIT で上書き) from policy_recorder import PolicyRecorder, daemon_read_servo, install_daemon_lock # noqa: E402 @@ -1082,6 +1083,15 @@ def apply_wait_args(args): f"{GRIP_OPEN_DAEMON_WAIT:.1f}s) / 止まるのを待つ目安 {SETTLE_WAIT_SEC:.1f}s") +def camera_tilt_line(bgr, raw_dir: Path) -> str: + """上カメラの傾きと, 安全範囲と同じお手本 (--raw-dir) の傾きの中央値との差 (依頼 142. 表示だけ. 落ちない).""" + try: + ref, _n_ok, _n = reference_tilt(raw_dir) + return tilt_text(tilt_deg(bgr), ref)[0] + f" (お手本: {raw_dir})" + except Exception as e: + return f"傾き 測れない ({type(e).__name__}: {e})" + + def framing_raw_dir(args) -> Path: """写り方の確かめの基準画像を取るお手本. --framing-raw-dir がなければ --raw-dir (依頼 137).""" return Path(args.framing_raw_dir or args.raw_dir) @@ -1205,6 +1215,7 @@ def main(): verdict, why = framing_verdict(dx, dy, resp) print(f"[CAMERA] 写り方 dy={dy:+.1f} dx={dx:+.1f} response={resp:.3f} → {verdict} ({why})") print(f"[CAMERA] 基準: {ref_path}") + print(f"[CAMERA] {camera_tilt_line(bgr0, Path(args.raw_dir))}") # 依頼 142. 表示だけ (判定・停止は変えない) log_f.write(json.dumps({"type": "framing", "dy": round(dy, 2), "dx": round(dx, 2), "response": round(resp, 3), "verdict": verdict, "why": why, "max_dy": FRAMING_MAX_DY, "max_dx": FRAMING_MAX_DX, diff --git a/pi05/test_camera_tilt.py b/pi05/test_camera_tilt.py new file mode 100644 index 0000000..6882772 --- /dev/null +++ b/pi05/test_camera_tilt.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 +"""test_camera_tilt.py — 上カメラの傾き (camera_tilt.py・依頼 142). + + - マット (黒い四角. 上は画面の上で切れる) を ±1° 回した合成画像で, 向き (プラス=右下がり) と値が正しく出る + (1920×1080 の生の大きさと, 学習画像の大きさ 384×216 の両方) + - 四隅が取れない (真っ白・画像なし・見つけ方が落ちる・左右の取り違え) ときは None で, 落ちない + - お手本の中央値 (reference_tilt) と差の文字 (tilt_text) の ±0.5° の線 + - camera_align.draw_overlay が傾きあり・なし・差が大きいときに落ちない + - camera_align.main のループで 数値 → 測れない → 数値 のとき [ALIGN ] 行に傾きが出る (カメラは偽物) + - run_policy_client.camera_tilt_line がお手本のフォルダが無くても・測り方が落ちても 1 行を返す + +アーム・カメラには触らない. + + python3 pi05/test_camera_tilt.py +""" +import math +import sys +import tempfile +import unittest +from pathlib import Path + +import cv2 +import numpy as np + +_HERE = Path(__file__).resolve().parent +sys.path.insert(0, str(_HERE)) + +import camera_tilt as ct # noqa: E402 + + +def mat_image(deg, w=1920, h=1080): + """白い机に黒いマット. マットの上は画面の上で切れる. 画面の中心で deg 度回す (プラス=右下がり).""" + img = np.full((h, w, 3), 230, np.uint8) + cx, cy = w / 2, h * 0.45 + hw, hh = w * 0.32, h * 0.75 # 上は画面の外まで伸ばす + pts = np.array([[-hw, -hh], [hw, -hh], [hw, hh * 0.55], [-hw, hh * 0.55]]) + r = math.radians(deg) + rot = np.array([[math.cos(r), -math.sin(r)], [math.sin(r), math.cos(r)]]) # y は下向き + p = (pts @ rot.T) + [cx, cy] + cv2.fillPoly(img, [np.round(p).astype(np.int32)], (20, 20, 20), cv2.LINE_AA) + return img + + +def small(img): + return cv2.resize(img, (384, 216), interpolation=cv2.INTER_AREA) + + +class TestTilt(unittest.TestCase): + def test_sign_and_value_full(self): + for deg in (+1.0, -1.0, 0.0, +1.7, -0.5): + t = ct.tilt_deg(mat_image(deg)) + self.assertIsNotNone(t, deg) + self.assertAlmostEqual(t, deg, delta=0.1, msg=f"{deg}: {t}") + + def test_sign_and_value_small(self): + for deg in (+1.0, -1.0, 0.0): + t = ct.tilt_deg(small(mat_image(deg))) + self.assertIsNotNone(t, deg) + self.assertAlmostEqual(t, deg, delta=0.35, msg=f"{deg}: {t}") + self.assertGreater(ct.tilt_deg(small(mat_image(+1.0))), 0.5) # 右下がり = プラス + self.assertLess(ct.tilt_deg(small(mat_image(-1.0))), -0.5) # 右上がり = マイナス + + def test_unmeasurable(self): + self.assertIsNone(ct.tilt_deg(np.full((216, 384, 3), 255, np.uint8))) # マットが無い + self.assertIsNone(ct.tilt_deg(None)) + self.assertIsNone(ct.tilt_deg(mat_image(1.0), corners_fn=lambda f: None)) + + def boom(_f): + raise RuntimeError("x") + self.assertIsNone(ct.tilt_deg(mat_image(1.0), corners_fn=boom)) + swapped = lambda f: [(0, 0), (10, 0), (5, 10), (50, 10)] # 左下が右下より右 + self.assertIsNone(ct.tilt_deg(mat_image(1.0), corners_fn=swapped)) + self.assertIsNone(ct.tilt_deg(mat_image(1.0), corners_fn=lambda f: [(0, 0), (1, 1)])) + + def test_reference_and_text(self): + with tempfile.TemporaryDirectory() as d: + self.assertEqual(ct.reference_tilt(d), (None, 0, 0)) # 空のフォルダ + for i, deg in enumerate((-1.0, -0.5, 0.0)): + ep = Path(d) / f"ep{i}" + (ep / "frames").mkdir(parents=True) + np.savez(ep / "episode.npz", state=np.zeros((2, 7))) + cv2.imwrite(str(ep / "frames" / "000000.jpg"), small(mat_image(deg))) + bad = Path(d) / "ep9" # 画像が無い本は測れない + (bad / "frames").mkdir(parents=True) + np.savez(bad / "episode.npz", state=np.zeros((2, 7))) + ref, n_ok, n = ct.reference_tilt(d) + self.assertEqual((n_ok, n), (3, 4)) + self.assertAlmostEqual(ref, -0.5, delta=0.35) + s, warn = ct.tilt_text(0.7, -0.6) + self.assertTrue(warn) + self.assertIn("差 +1.30°", s) + self.assertIn("傾きがお手本と違う", s) + self.assertFalse(ct.tilt_text(-0.2, -0.6)[1]) # 0.4° は線の内 + self.assertFalse(ct.tilt_text(None, -0.6)[1]) + self.assertIn("測れない", ct.tilt_text(None, -0.6)[0]) + self.assertIn("お手本の中央値 測れない", ct.tilt_text(0.1, None)[0]) + + def test_camera_align_overlay(self): + import camera_align as ca + img = small(mat_image(1.0)) + for tilt, ref in ((None, None), (0.9, None), (0.9, 0.8), (0.9, -0.6)): + big = ca.draw_overlay(img, img, 0.0, 0.0, 1.0, 0.5, 2.0, "ep", tilt, ref) + self.assertEqual(big.shape[:2], (432, 768)) + ca.draw_overlay(img, img, 0.0, 0.0, 1.0, 0.5, 2.0, "ep") # 今までの呼び方も通る + + def test_camera_align_loop(self): + """位置合わせのループ: 数値 → 測れない → 数値 のとき [ALIGN ] 行に傾きが出て, 落ちない (codex R1).""" + import contextlib + import io + from unittest import mock + + import camera_align as ca + frames = [mat_image(1.0), np.full((1080, 1920, 3), 255, np.uint8), mat_image(-1.0)] + + class Cap: + def __init__(self): + self.i = 0 + + def grab(self): + return True + + def read(self): + if self.i >= len(frames): + return False, None + self.i += 1 + return True, frames[self.i - 1] + + def release(self): + pass + + keys = iter([0, 0, ord("q")]) + ref = small(mat_image(0.0)) + out = io.StringIO() + with mock.patch.object(ca, "open_top_camera", return_value=Cap()), \ + mock.patch.object(ca, "load_reference", return_value=(ref, Path("/x/ep/frames/000000.jpg"))), \ + mock.patch.object(ca, "reference_tilt", return_value=(-0.6, 50, 50)), \ + mock.patch.object(ca, "grasp_j0", return_value=None), \ + mock.patch.object(ca.cv2, "imshow"), mock.patch.object(ca.cv2, "destroyAllWindows"), \ + mock.patch.object(ca.cv2, "waitKey", side_effect=lambda _t: next(keys)), \ + mock.patch.object(sys, "argv", ["camera_align.py", "--raw-dir", "/x"]), \ + contextlib.redirect_stdout(out): + ca.main() + al = [l for l in out.getvalue().splitlines() if l.startswith("[ALIGN ] dy=")] + self.assertEqual(len(al), 3, out.getvalue()) + self.assertIn("差 +1.5", al[0]) + self.assertIn("傾きがお手本と違う", al[0]) + self.assertIn("傾き 測れない", al[1]) + self.assertIn("差 -0.3", al[2]) + self.assertIn("お手本の中央値 -0.60°", out.getvalue()) + + def test_run_policy_client_line_exception(self): + """傾きの測り方が落ちても 1 行の文字を返し, 推論は止めない (codex R1).""" + from unittest import mock + + import run_policy_client as rpc + with mock.patch.object(rpc, "reference_tilt", side_effect=OSError("disk")): + self.assertIn("測れない", rpc.camera_tilt_line(small(mat_image(1.0)), Path("/x"))) + with mock.patch.object(rpc, "tilt_deg", side_effect=ValueError("x")), \ + mock.patch.object(rpc, "reference_tilt", return_value=(0.0, 1, 1)): + self.assertIn("測れない", rpc.camera_tilt_line(small(mat_image(1.0)), Path("/x"))) + + def test_run_policy_client_line(self): + import run_policy_client as rpc + s = rpc.camera_tilt_line(small(mat_image(1.0)), Path("/nonexistent/raw_episodes")) + self.assertIn("お手本の中央値 測れない", s) + self.assertIn("傾き +", s) + + +if __name__ == "__main__": + unittest.main() diff --git a/test_lab_start.sh b/test_lab_start.sh index c98ed67..d64080c 100755 --- a/test_lab_start.sh +++ b/test_lab_start.sh @@ -424,6 +424,7 @@ check "deep1 の設定とフォルダでサーバーを起動" 'has ssh.log "--p check "クライアントに --model deep1 を渡す" 'has py.log "--model deep1 (PixelstoMotors/pi05-mecharm-deep-even10-lora-v1)"' check "deep1 の安全範囲 (run_policy_client.py --raw-dir) は deep_even10_v1 の raw_episodes・trim2 は使わない (依頼 135)" 'has py.log "run_policy_client.py --raw-dir $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes --steps-per-chunk" && ! has py.log "--raw-dir $T/data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes" && has out.txt "[RANGE ] 安全範囲 (学習データ ±10°) は $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes から (--model deep1)"' check "deep1 の写り方の基準画像 (--framing-raw-dir) は trim2・deep_even10_v1 ではない (依頼 137)" 'has py.log "--ignore-framing --model deep1 (PixelstoMotors/pi05-mecharm-deep-even10-lora-v1) --framing-raw-dir $T/data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes" && ! has py.log "--framing-raw-dir $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes" && has out.txt "[FRAME ] 写り方の確かめの基準画像は $T/data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes の最初のエピソードから" && has out.txt "[RANGE ] 安全範囲 (学習データ ±10°) は $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes から (--model deep1)"' +check "deep1 の傾きのお手本は安全範囲と同じ deep_even10_v1 (依頼 142)" 'has out.txt "[TILT ] 上カメラの傾きのお手本 (各エピソードの 1 枚目の中央値) は $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes から"' check "記録に model=deep1" '[ "$(jq -r .model "$T/data/lab/LogT/pod/lab_pod.json")" = deep1 ]' rm -rf "$T" setup; rmdir "$T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes"; run --model deep1 @@ -562,6 +563,9 @@ rm -rf "$d/params" "$d/_CHECKPOINT_METADATA"; mkdir -p "$d/assets/local" check "済んだ印と assets だけ (前の確かめなら ckpt=1 だった形) → ckpt=0" '[[ "$(pr)" == *" ckpt=0 mark=1 "* ]]' rm -rf "$d" +# 位置合わせ (camera_align.py) はドライランでは流れないので, 書き方を lab_start.sh から確かめる (依頼 142) +check "camera_align.py に傾きのお手本 --tilt-raw-dir \"\$RAW_RANGE\" を渡す (依頼 142)" 'grep -qF "python3 pi05/camera_align.py --raw-dir \"\$RAW\" --episode \"\$EPISODE\" --scale 5 --alpha 0.7 --tilt-raw-dir \"\$RAW_RANGE\"" "$HERE/lab_start.sh"' + echo echo "OK $OK / NG $NG" [ $NG = 0 ]