LOG197R2R2R2026/5/14

mechArm 270 Pi 動的追従の限界を探る Log#12 | なぜアームが動かないのかカメラとdaemonの干渉を追った

デザイナーがロボットを触ると、見えてくるものがある。LOG197で見えたのはこれ。

横カメラのpixel座標とUnity座標の対応関係を実データで取ることがこのセッションの目標だった。横カメラのキャリブレーションデータがあれば、「横カメラのpixelがどの実座標に対応するか」という変換式（B）が作れる。変換式ができれば、視覚サーボループに横カメラのZ・Y方向観測値を組み込める（C）。これがデュアルカメラ統合の最後のインフラ工事だ。

そのためにFKスキャンで60姿勢を回し、各姿勢で上カメラのpixel・Unity座標と横カメラのpixelを同時記録するスクリプト fk_scan_calibration.py を新規作成して実行に臨んだ。

スクリプトは動いた。でもアームが動かなかった。

コードはエラーなく通っている。daemonは {"ok": true} を返している。スキャンログには top=OK が流れている。それでもアームは1姿勢目から最後まで同じ場所に固まったままだった。

「なぜ動かないのか」を1つずつ切り分けた。pymycobot直接では動く。daemon経由でも動く。カメラなしのループでも動く。カメラとdaemonを組み合わせたときだけ止まる。

原因はロボット制御の問題ではなかった。cv2.VideoCapture() の実行中にmacOSのAVFoundationが処理を占有して、daemonとのTCPソケットが応答できなくなっていた。カメラがdaemonを殺していた。

キャリブCSVは1行も取れなかった。副産物として auto_detect_cameras.py の安定化と横カメラのTIP検出改善が進んだ。原因は確定した。解決策は次セッションに持ち越す。

💡 Key Takeaway

キャリブデータ収集スクリプトでアームが動かなかった原因は、cv2.VideoCapture() の実行中にdaemonとのTCP接続が切れることだった。pymycobot直接では動く、daemon直接では動く、カメラなしループでも動く——カメラとdaemonの共存だけが止まる。この切り分けで問題の輪郭が確定した。

発生した問題	現場判断	結果
スキャン中にアームが全く動かない	カメラなし→動く、daemon直接→動く、pymycobot直接→動くの順で切り分け	カメラ取得処理がdaemonのTCP接続を殺していることが確定
Continuity Camera keep-openで同じフレームが返り続ける	mean値テストで133.25が5回連続一致。macOS AVFoundationとOpenCVの相性問題と判断	都度open/closeに戻すが、根本のdaemon干渉は解消せず
`auto_detect_cameras.py` 連続実行でRuntimeError頻発	warmup_framesを30→120、cap.release()後にtime.sleep(0.5)追加	3回連続実行で全て安定。D完了

キャリブデータ収集の設計意図

前セッションまでで横カメラのTIP検出インフラは揃っていた。auto_detect_cameras.py でカメラindexを自動判定し、detect_tip_side() で緑シール2点の中点からTIPを取る。

残っていたのはAの作業——横カメラのpixel座標とUnity座標の対応関係を実データで取ること。FKスキャンで60姿勢を回し、各姿勢で上カメラのpixel・Unity座標と横カメラのpixelを同時記録したCSVを作る。このCSVがあれば横カメラのpixel→Unity座標変換式（Bの作業）に進める。

新規スクリプト fk_scan_calibration.py を作成した。

D: auto_detect_cameras.py の安定化

スキャンを走らせる前に、まずDから片付けた。auto_detect_cameras.py の連続実行が約2/3の頻度でRuntimeErrorを出す問題だ。

原因はIriun + macOSの自動露出リセット + warmupフレーム不足。修正点は3箇所。

warmup_frames のデフォルト値を30から120に変更。cap.release() 直後に time.sleep(0.5) を2箇所追加。各インデックスの試行間に time.sleep(0.3) を追加。

修正後の関数定義と該当ループ：

def auto_detect_cameras(
    candidate_indices: list = None,
    warmup_frames: int = 120,          # 30 → 120 に変更
    save_debug_frames: bool = True,
    debug_dir: str = "logs/debug_frames",
) -> dict:
    ...
    for idx in candidate_indices:
        ...
        cap = cv2.VideoCapture(idx)
        if not cap.isOpened():
            all_results.append(entry)
            cap.release()
            time.sleep(0.5)            # 追加
            continue

        entry["opened"] = True

        for _ in range(warmup_frames):
            cap.read()

        ret, frame = cap.read()
        cap.release()
        time.sleep(0.5)                # 追加

        ...
        all_results.append(entry)
        time.sleep(0.3)                # 追加

3回連続実行の結果：

上カメラ index: 2
横カメラ index: 1

  index 0: exclude  (blue=0, green=0, opened=True)
  index 1: side     (blue=2, green=2, opened=True)
  index 2: top      (blue=5, green=0, opened=True)
  index 3: skipped  (blue=0, green=0, opened=False)

3回とも同じ結果。Dはここで片付いた。

横カメラ: 緑シール検出の改善

最初のスキャン実行で横カメラのside=MISSが全姿勢で続いた。logs/calib_frames/ のannotated画像を確認すると、緑の検出点がモニターのベゼル付近にあり、爪の緑シールは検出されていなかった。

爪の角度で緑シールが影になり、HSV閾値から漏れていた。光が当たる確率を単純に上げるため、シールを縦2枚に増やした。

さらに detect_tip_side() に設計上の問題が見つかった。元の実装は青シール（爪上面）が2点以上見えない場合にTIPをNoneで返す設計だった。HOME姿勢では爪が真上を向くため横カメラから青シールが見えない。青シールチェックを削除した。

# 修正前
if not blue_centers or len(blue_centers) < 2:
    return None, blue_for_return, None
if not green_centers or len(green_centers) < 2:
    return None, blue_for_return, None

# 修正後: 青シールチェックを削除
if not green_centers or len(green_centers) < 2:
    return None, blue_for_return, None

修正後の detect_tip_side() フルコード：

def detect_tip_side(frame):
    """
    横カメラのフレームから TIP（爪先）の pixel 位置を計算する。

    TIP は緑シール2点（爪前面）の中点として返す。
    座標系変換（pixel → mm）は別タスクのため、tip_yz_mm は常に None。

    Args:
        frame: BGR フォーマットの ndarray (H, W, 3)

    Returns:
        tuple (tip_pixel, blue_markers, tip_yz_mm)
    """
    blue_centers, _bmask = _count_blue_markers(frame)
    green_centers, _gmask = _count_green_markers(frame)

    blue_for_return = blue_centers if blue_centers else None

    if not green_centers or len(green_centers) < 2:
        return None, blue_for_return, None

    # 横カメラの背景ノイズ対策：3点以上なら画面下方の2点を採用
    green_pair = _pick_bottom_two(green_centers)

    # X座標昇順で左右を識別
    green_sorted = sorted(green_pair, key=lambda p: p[0])
    green_left, green_right = green_sorted[0], green_sorted[1]

    tip = (
        (green_left[0] + green_right[0]) // 2,
        (green_left[1] + green_right[1]) // 2,
    )
    return tip, blue_centers, None

修正後のテスト：

python3 -c "
import cv2, time
from auto_detect_cameras import _count_green_markers
from side_camera_utils import detect_tip_side

cap = cv2.VideoCapture(1)
for _ in range(120):
    cap.read()
ret, frame = cap.read()
cap.release()
time.sleep(0.5)

tip, blue, _ = detect_tip_side(frame)
green, _ = _count_green_markers(frame)
print(f'tip_pixel: {tip}')
print(f'green_centers: {green}')
"

結果：

tip_pixel: (962, 61)
green_centers: [(1085, 70), (840, 53)]

HOME姿勢で横カメラのTIP検出が成立した。

fk_scan_calibration.py のフルコード

#!/usr/bin/env python3
"""
fk_scan_calibration.py

横カメラの pixel ↔ Unity (z, y) 変換式を作るためのキャリブレーションデータ収集。
FK scan から ee_y/ee_x/ee_z 制約で 60 姿勢を均等サンプリングし、
各姿勢で「上カメラ pixel」「横カメラ pixel」「Unity TIP 座標」を同時記録する。

出力: ~/r2r2r-scripts/fk_scan_calibration.csv
ヘッダ: j0,j1,j2,j3,j4,j5,top_px_x,top_px_y,top_unity_x,top_unity_z,side_px_x,side_px_y
"""

import os
import sys
import csv
import time
import cv2
import numpy as np

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))

import visual_servo_pick
from visual_servo_pick import (
    detect_tip_from_frame,
    load_fk_table,
    get_daemon_socket,
    _daemon_request,
    close_daemon_socket,
)
from side_camera_utils import detect_tip_side

HOMOGRAPHY_PATH = os.path.expanduser("~/r2r2r-scripts/homography.npy")
OUT_CSV_PATH    = os.path.expanduser("~/r2r2r-scripts/fk_scan_calibration.csv")

HOME_ANGLES = [0, 0, 0, 0, 0, -30]
MOVE_SPEED  = 15
MOVE_WAIT   = 4
POST_MOVE_SETTLE = 1.0

EE_Y_MIN, EE_Y_MAX = -0.080, -0.010
EE_X_MIN, EE_X_MAX = -0.200,  0.200
EE_Z_MIN, EE_Z_MAX = -0.050,  0.350
N_SAMPLES = 60

CAM_WARMUP_FRAMES = 10
CAM_RELEASE_SLEEP = 0.5

CSV_HEADER = [
    "j0", "j1", "j2", "j3", "j4", "j5",
    "top_px_x", "top_px_y",
    "top_unity_x", "top_unity_z",
    "side_px_x", "side_px_y",
]


def load_homography_into_module():
    H = np.load(HOMOGRAPHY_PATH)
    visual_servo_pick._homography = H
    print(f"[Mac] homography をロード: shape={H.shape}")


def pick_calibration_poses():
    table = visual_servo_pick._fk_table
    filtered = [
        (ee_x, ee_y, ee_z, joints) for (ee_x, ee_y, ee_z, joints) in table
        if EE_Y_MIN <= ee_y <= EE_Y_MAX
        and EE_X_MIN <= ee_x <= EE_X_MAX
        and EE_Z_MIN <= ee_z <= EE_Z_MAX
    ]
    print(f"[Mac] FK制約合致: {len(filtered)} / {len(table)} 点")
    if len(filtered) == 0:
        raise RuntimeError("制約を満たす姿勢が0件")
    n = min(N_SAMPLES, len(filtered))
    indices = np.linspace(0, len(filtered) - 1, n).round().astype(int)
    return [filtered[i] for i in indices]


def capture_one_frame(cam_idx):
    cap = cv2.VideoCapture(cam_idx)
    if not cap.isOpened():
        cap.release()
        time.sleep(CAM_RELEASE_SLEEP)
        return None
    for _ in range(CAM_WARMUP_FRAMES):
        cap.read()
    ret, frame = cap.read()
    cap.release()
    time.sleep(CAM_RELEASE_SLEEP)
    if not ret or frame is None:
        return None
    return frame


def send_pose(joints, speed=MOVE_SPEED, wait=MOVE_WAIT):
    _daemon_request({
        "cmd": "send_angles",
        "angles": list(joints),
        "speed": speed,
        "wait": wait,
    })


def go_home():
    print(f"[Mac] HOME へ移動: {HOME_ANGLES}")
    send_pose(HOME_ANGLES)
    time.sleep(POST_MOVE_SETTLE)


def ensure_daemon_connected():
    try:
        _daemon_request({"cmd": "ping"})
    except Exception:
        print("[Mac] daemon 再接続中...")
        close_daemon_socket()
        get_daemon_socket()
        _daemon_request({"cmd": "ping"})
        print("[Mac] ✓ daemon 再接続完了")


def main():
    print("=" * 70)
    print("fk_scan_calibration.py  ── 横カメラキャリブ用データ収集")
    print("=" * 70)

    CALIB_FRAME_DIR = os.path.expanduser("~/r2r2r-scripts/logs/calib_frames")
    os.makedirs(CALIB_FRAME_DIR, exist_ok=True)

    load_fk_table()
    load_homography_into_module()

    poses = pick_calibration_poses()
    print(f"[Mac] スキャン姿勢数: {len(poses)}")

    top_idx  = 2   # Continuity Camera (iPhone 14 Pro Max)
    side_idx = 1   # Iriun Webcam (iPhone 11)
    print(f"[Mac] カメラindex固定: 上={top_idx}, 横={side_idx}")

    print("[Mac] デーモンソケット接続 (TCP 5555)...")
    get_daemon_socket()
    print("[Mac] ✓ daemon ready")

    out_f = open(OUT_CSV_PATH, "w", newline="")
    writer = csv.writer(out_f)
    writer.writerow(CSV_HEADER)
    out_f.flush()

    recorded = 0
    skipped  = 0

    try:
        go_home()

        print("[Mac] グリッパーを開く...")
        _daemon_request({"cmd": "set_gripper", "state": 0, "speed": 80, "wait": 2})
        time.sleep(1.0)

        for i, (ee_x_target, ee_y_target, ee_z_target, joints) in enumerate(poses, start=1):
            ensure_daemon_connected()
            send_pose(joints)
            time.sleep(POST_MOVE_SETTLE)

            frame_top = capture_one_frame(top_idx)
            tip_pixel_top = None
            tip_unity = None
            top_blue_markers = None
            if frame_top is not None:
                tip_pixel_top, top_blue_markers, tip_unity = detect_tip_from_frame(frame_top)

            frame_side = capture_one_frame(side_idx)
            tip_pixel_side = None
            side_blue_markers = None
            if frame_side is not None:
                tip_pixel_side, side_blue_markers, _ = detect_tip_side(frame_side)

            top_ok = (tip_pixel_top is not None) and (tip_unity is not None)
            side_ok = tip_pixel_side is not None

            joints_str = " ".join(f"j{k}={joints[k]:+.1f}" for k in range(6))
            top_desc = (
                f"OK(px={tip_pixel_top[0]},{tip_pixel_top[1]} "
                f"unity={tip_unity[0]:+.3f},{tip_unity[1]:+.3f})"
                if top_ok else "MISS"
            )
            side_desc = (
                f"OK(px={tip_pixel_side[0]},{tip_pixel_side[1]})"
                if side_ok else "MISS"
            )

            if top_ok:
                side_x = tip_pixel_side[0] if side_ok else -1
                side_y = tip_pixel_side[1] if side_ok else -1
                writer.writerow([
                    f"{joints[0]:.3f}", f"{joints[1]:.3f}", f"{joints[2]:.3f}",
                    f"{joints[3]:.3f}", f"{joints[4]:.3f}", f"{joints[5]:.3f}",
                    tip_pixel_top[0], tip_pixel_top[1],
                    f"{tip_unity[0]:.6f}", f"{tip_unity[1]:.6f}",
                    side_x, side_y,
                ])
                out_f.flush()
                recorded += 1
                status = "RECORDED"
            else:
                skipped += 1
                status = "SKIPPED"

            if frame_top is not None:
                annotated_top = frame_top.copy()
                if top_blue_markers:
                    for pt in top_blue_markers:
                        cv2.circle(annotated_top, tuple(map(int, pt)), 18, (0, 255, 0), 2)
                if tip_pixel_top is not None:
                    cv2.circle(annotated_top, tuple(map(int, tip_pixel_top)), 25, (0, 165, 255), 3)
                cv2.putText(
                    annotated_top,
                    f"blue={len(top_blue_markers) if top_blue_markers else 0} tip={'OK' if tip_pixel_top else 'MISS'}",
                    (20, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 165, 255), 2,
                )
                cv2.imwrite(os.path.join(CALIB_FRAME_DIR, f"pose_{i:03d}_top.png"), annotated_top)

            if frame_side is not None:
                annotated_side = frame_side.copy()
                if side_blue_markers:
                    for pt in side_blue_markers:
                        cv2.circle(annotated_side, tuple(map(int, pt)), 18, (0, 255, 0), 2)
                if tip_pixel_side is not None:
                    cv2.circle(annotated_side, tuple(map(int, tip_pixel_side)), 25, (0, 165, 255), 3)
                cv2.putText(
                    annotated_side,
                    f"side tip={'OK' if tip_pixel_side else 'MISS'}",
                    (20, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 165, 255), 2,
                )
                cv2.imwrite(os.path.join(CALIB_FRAME_DIR, f"pose_{i:03d}_side.png"), annotated_side)

            print(
                f"pose {i}/{len(poses)}: {joints_str} "
                f"top={top_desc} side={side_desc} → {status}"
            )

    finally:
        try:
            go_home()
        except Exception as e:
            print(f"[Mac] [WARN] HOME 復帰失敗: {e}")
        try:
            close_daemon_socket()
        except Exception as e:
            print(f"[Mac] [WARN] daemon close 失敗: {e}")
        out_f.close()

    print()
    print("=" * 70)
    print(f"完了: recorded={recorded}, skipped={skipped}")
    print(f"出力: {OUT_CSV_PATH}")
    print("=" * 70)


if __name__ == "__main__":
    main()

スキャン開始直後の出力：

======================================================================
fk_scan_calibration.py  ── 横カメラキャリブ用データ収集
======================================================================
[Mac] FK scan CSV読み込み中...
[Mac] ✓ 512087点ロード
[Mac] homography をロード: shape=(3, 3)
[Mac] FK制約合致: 2200 / 512087 点
[Mac] スキャン姿勢数: 60
[Mac] カメラindex固定: 上=2, 横=1
[Mac] デーモンソケット接続 (TCP 5555)...
[Mac] ✓ daemon ready
[Mac] HOME へ移動: [0, 0, 0, 0, 0, -30]
[Mac] グリッパーを開く...
pose 1/60: j0=-37.0 j1=+120.0 j2=-39.0 j3=+13.5 j4=-75.0 j5=-30.5 top=OK(px=657,696 unity=+0.163,+0.223) side=MISS → RECORDED
pose 2/60: j0=-32.0 j1=+120.0 j2=-29.0 j3=-11.5 j4=-60.0 j5=-30.5 top=OK(px=655,695 unity=+0.164,+0.222) side=MISS → RECORDED
pose 3/60: j0=-61.9 j1=+107.7 j2=-33.3 j3=-11.4 j4=-50.1 j5=-30.0 top=OK(px=606,660 unity=+0.187,+0.205) side=MISS → RECORDED
pose 4/60: j0=-36.9 j1=+117.7 j2=-33.3 j3=-6.4 j4=-75.1 j5=-30.0 top=OK(px=584,744 unity=+0.198,+0.245) side=MISS → RECORDED

top=OKは安定しているが、アームが動いていないことに気づいた。unity座標が2パターンに固定されていた。

アームが動かない問題の切り分け

ステップ1: Pi側でpymycobotを直接実行

from pymycobot.mycobot import MyCobot
mc = MyCobot('/dev/ttyAMA0', 1000000)
print('current:', mc.get_angles())
mc.send_angles([0, 60, -30, 0, -30, -30], 15)
import time; time.sleep(4)
print('after:', mc.get_angles())

結果：

current: [-0.17, 1.49, 0.61, -0.26, 42.97, -29.7]
after: []

動いた。after: [] はアームが移動中にget_angles()を呼ぶpymycobotの既知挙動。問題はpymycobot自体にはない。

ステップ2: Mac側からdaemonに直接send_anglesを送る

最初に、Pi側のdaemonファイルがMacの修正版と同期していないことが判明した。

# Pi側ファイルの確認
ssh ubuntu@192.168.2.1 -i ~/.ssh/id_rsa_pi \
  "grep -n 'send_angles 呼び出し前' ~/r2r2r-scripts/mecharm_daemon.py"
# 何も返らない → 古いバージョン

# MacからPiにコピー
scp -i ~/.ssh/id_rsa_pi ~/r2r2r-scripts/mecharm_daemon.py \
  ubuntu@192.168.2.1:~/r2r2r-scripts/mecharm_daemon.py
# mecharm_daemon.py    100% 4846     1.7MB/s   00:00

コピー後にdaemonを再起動して実行：

import socket, json
s = socket.socket()
s.settimeout(15)
s.connect(('192.168.2.1', 5555))
s.sendall(json.dumps({'cmd': 'ping'}).encode() + b'\n')
print('ping:', s.recv(4096))
cmd = {'cmd': 'send_angles', 'angles': [0, 60, -30, 0, -30, -30], 'speed': 15, 'wait': 4}
s.sendall(json.dumps(cmd).encode() + b'\n')
print('send_angles:', s.recv(4096))
s.close()

結果：

ping: b'{"ok": true}\n'
send_angles: b'{"ok": true}\n'

Pi側のdaemonログ：

[DAEMON 16:24:18] 接続: ('192.168.2.2', 63245)
[DAEMON 16:24:18] send_angles 呼び出し前: angles=[0, 60, -30, 0, -30, -30], speed=15, wait=4.0
[DAEMON 16:24:18] send_angles 呼び出し後: result=None
[DAEMON 16:24:18] send_angles 後の現在角度: [-0.17, 61.17, -28.21, -0.26, -30.32, -29.79]
[DAEMON 16:24:22] 切断: ('192.168.2.2', 63245)

daemon経由でも動く。角度が目標値に変化している。

ステップ3: カメラなしのスキャンループで動かす

import sys, os, time
sys.path.insert(0, os.path.expanduser('~/r2r2r-scripts'))
import visual_servo_pick as vsp
from visual_servo_pick import load_fk_table, get_daemon_socket, _daemon_request, close_daemon_socket
import numpy as np

vsp._homography = np.load(os.path.expanduser('~/r2r2r-scripts/homography.npy'))
load_fk_table()
get_daemon_socket()

poses = [
    [0, 0, 0, 0, 0, -30],
    [0, 60, -30, 0, -30, -30],
    [30, 80, -40, 0, -40, -30],
    [0, 0, 0, 0, 0, -30],
]

for i, p in enumerate(poses):
    print(f'pose {i}: {p}')
    _daemon_request({'cmd': 'send_angles', 'angles': p, 'speed': 15, 'wait': 4})
    time.sleep(1.0)
    print(f'pose {i}: done')

close_daemon_socket()

Pi側のdaemonログ：

[DAEMON 16:38:07] 接続: ('192.168.2.2', 63909)
[DAEMON 16:38:07] send_angles 呼び出し前: angles=[0, 0, 0, 0, 0, -30], speed=15, wait=4.0
[DAEMON 16:38:07] send_angles 呼び出し後: result=None
[DAEMON 16:38:07] send_angles 後の現在角度: [-27.15, 90.26, -29.35, 10.37, -56.25, -30.05]
[DAEMON 16:38:12] send_angles 呼び出し前: angles=[0, 60, -30, 0, -30, -30], speed=15, wait=4.0
[DAEMON 16:38:12] send_angles 呼び出し後: result=None
[DAEMON 16:38:12] send_angles 後の現在角度: []
[DAEMON 16:38:17] send_angles 呼び出し前: angles=[30, 80, -40, 0, -40, -30], speed=15, wait=4.0
[DAEMON 16:38:17] send_angles 呼び出し後: result=None
[DAEMON 16:38:17] send_angles 後の現在角度: []
[DAEMON 16:38:22] send_angles 呼び出し前: angles=[0, 0, 0, 0, 0, -30], speed=15, wait=4.0
[DAEMON 16:38:22] send_angles 呼び出し後: result=None
[DAEMON 16:38:22] send_angles 後の現在角度: []

アームが動いた。目視でも確認。

カメラ取得処理がdaemon接続を殺していた。問題はアームでも通信設計でもなく、それだった。

なぜカメラがdaemonを殺すのか

capture_one_frame() は cv2.VideoCapture(idx) をopen → warmup(10フレーム) → read → releaseする処理だ。

def capture_one_frame(cam_idx):
    cap = cv2.VideoCapture(cam_idx)      # AVFoundation経由でopen
    if not cap.isOpened():
        cap.release()
        time.sleep(CAM_RELEASE_SLEEP)
        return None
    for _ in range(CAM_WARMUP_FRAMES):   # 10フレームwarmup
        cap.read()
    ret, frame = cap.read()
    cap.release()
    time.sleep(CAM_RELEASE_SLEEP)        # 0.5秒sleep
    if not ret or frame is None:
        return None
    return frame

この一連の実行中に、daemonとのTCPソケット接続が切れる。

これはロボット制御の問題ではなく、「Mac上で複数のリアルタイム処理を共存させる」問題だった。OpenCVのAVFoundationバックエンドがGILを掴んで、socketを窒息させている——そんな仮説を立てた。macOS上のContinuity CameraはAVFoundationを経由するため特に影響が大きい。

スキャン中にdaemonのsend_anglesログが一切出ていなかったことが決定的な証拠だ。daemonがコマンドを受け取る前に接続が切れている。

副次: Continuity Cameraのkeep-openでフレームが更新されない

上カメラをスキャン全体でopen保持したまま姿勢ごとにフレームを取れば高速化できると考えた。mean値テストで検証した。

import cv2, time
cap = cv2.VideoCapture(2)
for _ in range(120):
    cap.read()

for i in range(5):
    for _ in range(5):
        cap.read()
    ret, frame = cap.read()
    if ret:
        print(f'frame {i}: mean={frame.mean():.2f}')
    time.sleep(1.0)

cap.release()

結果：

frame 0: mean=133.25
frame 1: mean=133.27
frame 2: mean=133.26
frame 3: mean=133.25
frame 4: mean=133.25

5回連続でほぼ同一値。keep-openではフレームが更新されない。

都度open/closeの場合：

for i in range(5):
    cap = cv2.VideoCapture(2)
    for _ in range(10):
        cap.read()
    ret, frame = cap.read()
    cap.release()
    time.sleep(0.5)
    if ret:
        print(f'frame {i}: mean={frame.mean():.2f}')
    time.sleep(1.0)

結果：

frame 0: mean=133.40
frame 1: mean=133.19
frame 2: mean=133.32
frame 3: mean=133.32
frame 4: mean=133.36

mean値が変化している。都度open/closeでフレームは更新される。ただしこれがdaemon干渉の原因でもある。

mecharm_daemon.py のログ追加（デバッグ用修正）

問題調査のためにdaemonのsend_anglesブロックにログを追加した。修正後のフルコード：

#!/usr/bin/env python3
"""
mecharm_daemon.py
Pi 側で常駐する MyCobot 制御デーモン。
TCP socket で Mac からの JSON コマンドを受け取り、永続化された MyCobot 経由で実機に転送する。
"""

import json
import socket
import sys
import time
import signal
import traceback
from pymycobot.mycobot import MyCobot

DAEMON_HOST = "0.0.0.0"
DAEMON_PORT = 5555
SERIAL_PORT = "/dev/ttyAMA0"
BAUDRATE = 1000000

_mc = None
_server_sock = None
_running = True


def log(msg):
    print(f"[DAEMON {time.strftime('%H:%M:%S')}] {msg}", flush=True)


def init_mycobot():
    global _mc
    log(f"MyCobot 初期化中 ({SERIAL_PORT}, baud={BAUDRATE})...")
    _mc = MyCobot(SERIAL_PORT, BAUDRATE)
    log("✓ MyCobot 初期化完了")


def handle_command(cmd_obj):
    cmd = cmd_obj.get("cmd")

    if cmd == "ping":
        return {"ok": True}

    elif cmd == "send_angles":
        try:
            angles = cmd_obj["angles"]
            speed = int(cmd_obj.get("speed", 20))
            wait = float(cmd_obj.get("wait", 0))

            log(f"send_angles 呼び出し前: angles={angles}, speed={speed}, wait={wait}")
            result = _mc.send_angles(angles, speed)
            log(f"send_angles 呼び出し後: result={result}")

            current_angles = _mc.get_angles()
            log(f"send_angles 後の現在角度: {current_angles}")

            if wait > 0:
                time.sleep(wait)
            return {"ok": True}
        except Exception as e:
            log(f"send_angles 例外: {type(e).__name__}: {e}")
            log(traceback.format_exc().rstrip())
            return {"ok": False, "error": f"{type(e).__name__}: {e}"}

    elif cmd == "get_angles":
        a = None
        for attempt in range(3):
            a = _mc.get_angles()
            if a and len(a) == 6:
                break
            time.sleep(0.1)
        if not a or len(a) != 6:
            return {"ok": False, "error": "get_angles returned empty/invalid after 3 retries"}
        return {"ok": True, "angles": a}

    elif cmd == "set_gripper":
        state = int(cmd_obj["state"])
        speed = int(cmd_obj.get("speed", 100))
        wait = float(cmd_obj.get("wait", 0))
        time.sleep(2)
        result = _mc.set_gripper_state(state, speed)
        if wait > 0:
            time.sleep(wait)
        return {"ok": True, "result": result}

    else:
        return {"ok": False, "error": f"unknown cmd: {cmd}"}


def serve_client(client_sock, addr):
    log(f"接続: {addr}")
    buf = b""
    try:
        while _running:
            data = client_sock.recv(4096)
            if not data:
                break
            buf += data
            while b"\n" in buf:
                line, buf = buf.split(b"\n", 1)
                line = line.strip()
                if not line:
                    continue
                try:
                    cmd_obj = json.loads(line.decode("utf-8"))
                    response = handle_command(cmd_obj)
                except Exception as e:
                    response = {"ok": False, "error": f"{type(e).__name__}: {e}"}
                    log(f"コマンド処理エラー: {e}")
                resp_line = (json.dumps(response) + "\n").encode("utf-8")
                client_sock.sendall(resp_line)
    except ConnectionResetError:
        log(f"接続リセット: {addr}")
    except Exception as e:
        log(f"クライアントエラー: {e}")
    finally:
        client_sock.close()
        log(f"切断: {addr}")


def shutdown_handler(signum, frame):
    global _running
    log(f"シグナル受信 ({signum}), シャットダウン開始")
    _running = False
    if _server_sock:
        try:
            _server_sock.close()
        except Exception:
            pass
    sys.exit(0)


def main():
    global _server_sock

    signal.signal(signal.SIGTERM, shutdown_handler)
    signal.signal(signal.SIGINT, shutdown_handler)

    init_mycobot()

    _server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    _server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    _server_sock.bind((DAEMON_HOST, DAEMON_PORT))
    _server_sock.listen(1)
    log(f"ポート {DAEMON_PORT} で待ち受け中...")

    try:
        while _running:
            try:
                client_sock, addr = _server_sock.accept()
            except OSError:
                break
            serve_client(client_sock, addr)
    finally:
        _server_sock.close()
        log("シャットダウン完了")


if __name__ == "__main__":
    main()

このセッションで確定したこと・していないこと

確定したこと:

D完了: auto_detect_cameras.py warmup_frames 30→120、cap.release()後にtime.sleep追加で3回連続安定
detect_tip_side() 青シールチェック削除 → HOME姿勢でのTIP検出成立: tip_pixel=(962, 61)
緑シール縦2枚配置でHOME姿勢でも検出可能なことを確認
daemon経由のsend_anglesはカメラなしの場合は正常動作する
カメラ取得がdaemonのTCP接続を切断することを切り分けで確定
Pi側へのファイル配布はscpが必要（gitでは自動同期されない）
Continuity CameraはOpenCVのkeep-openでフレームが更新されない（AVFoundationの仕様）

未解決:

capture_one_frame() 実行中のdaemon接続切れ → send_pose wait=0化 + Mac側sleep設計を次セッションで実装
横カメラ（side）の全姿勢MISS → 緑シール縦2枚で改善したが、スキャン実行中の検証は未完
fk_scan_calibration.csv 未取得