"""手眼标定·触碰记录器(只读)。臂掉使能、手引导食指尖依次碰板上已知角点,本脚本 snapshot 每次的末端位姿。 连控制器 rosbridge 读 /right/arm_state(末端 pose 在 base_link 系)。全程只订阅、不发任何指令。 健壮性:每次记录都【强制等一条新消息】再 snapshot(避免 input() 阻塞期间读到陈旧位姿); 并检测与上一条是否雷同(臂没动/位姿流卡住会立刻报警)。 用法:.venv/bin/python scripts/record_touch.py --capture data/real_calib 交互:食指尖精确碰板上第 k 号角点(见 out/corners_labeled.png)、手腕朝向每次都变一变 → 输入 k 回车记一条; s=看实时位姿, u=撤销上一条, done=结束存盘。要 ≥5 个角点、朝向各不相同。 """ import argparse, json, time from pathlib import Path import numpy as np import roslibpy ROOT = Path(__file__).resolve().parents[1] def main(): ap = argparse.ArgumentParser() ap.add_argument("--capture", default="data/real_calib") ap.add_argument("--host", default="192.168.10.21"); ap.add_argument("--port", type=int, default=9090) ap.add_argument("--fresh", action="store_true", help="清空已有样本重新开始") a = ap.parse_args() cap = ROOT / a.capture ros = roslibpy.Ros(host=a.host, port=a.port); ros.run(timeout=8) print("connected:", ros.is_connected) st = {"m": None, "n": 0} tp = roslibpy.Topic(ros, "/right/arm_state", "lbot_arm_interfaces/msg/ArmState") tp.subscribe(lambda m: (st.__setitem__("m", m), st.__setitem__("n", st["n"] + 1))) def fresh(timeout=2.0): """等一条【新】消息再返回,保证不是陈旧帧。返回 m 或 None(流卡住)。""" n0 = st["n"]; t0 = time.time() while st["n"] == n0 and time.time() - t0 < timeout: time.sleep(0.02) return st["m"] if st["n"] != n0 else None if fresh() is None: raise SystemExit("收不到 /right/arm_state 新消息——确认 web 控制器在线、rosbridge 通") samples = [] f = cap / "touch_samples.json" if f.exists() and not a.fresh: samples = json.loads(f.read_text()).get("samples", []) print(f"已有 {len(samples)} 条历史样本,将在其后追加(--fresh 可清空重来)") def pose_of(m): return np.array([m["pose"]["position"][k] for k in "xyz"]) print("== 食指尖精确压第 k 号角点、每次换个手腕朝向 → 输入 k 回车。s=实时位姿, u=撤销, done=结束 ==") while True: s = input(f"[{len(samples)} 条] 角点编号 k / s / u / done: ").strip().lower() if s == "done": break if s == "s": m = fresh() if m is None: print(" ⚠ 位姿流卡住,没收到新消息"); continue p = m["pose"]["position"]; q = m["pose"]["orientation"] print(f" 实时末端 pos=({p['x']:.4f},{p['y']:.4f},{p['z']:.4f}) quat=({q['x']:.3f},{q['y']:.3f},{q['z']:.3f},{q['w']:.3f})") continue if s == "u": if samples: print(f" 撤销角点 {samples.pop()['corner']}") continue try: k = int(s) except ValueError: print(" 请输入角点编号(整数) / s / u / done"); continue m = fresh() if m is None: print(" ⚠ 2 秒内没收到新位姿消息——位姿流卡住了,没记录。检查 web 控制器/网络"); continue p = m["pose"]["position"]; q = m["pose"]["orientation"] pos = pose_of(m) # 雷同检测:与上一条几乎相同 → 忘了上使能/位姿没刷新 if samples and np.linalg.norm(pos - np.array(samples[-1]["pos"])) < 0.003: c = input(f" ⚠ 与上一条角点 {samples[-1]['corner']} 位姿几乎相同(<3mm)——移动后【上使能】了吗?仍要记录输 y: ").strip().lower() if c != "y": print(" 已跳过"); continue samples.append({"corner": k, "pos": [p["x"], p["y"], p["z"]], "quat_xyzw": [q["x"], q["y"], q["z"], q["w"]], "joints": list(m["joint_state"]["position"])}) print(f" 记下角点 {k}: pos=({p['x']:.4f},{p['y']:.4f},{p['z']:.4f}) (共 {len(samples)} 条)") tp.unsubscribe(); ros.terminate() cap.mkdir(parents=True, exist_ok=True) f.write_text(json.dumps({"samples": samples, "host": a.host}, indent=2)) nc = len({s["corner"] for s in samples}) # 检查位姿/朝向是否真的有变化 if samples: P = np.array([s["pos"] for s in samples]); Q = np.array([s["quat_xyzw"] for s in samples]) print(f"\n存盘 {f}:{len(samples)} 条 / {nc} 个角点;位置跨度 {(np.ptp(P,0)*1000).round(0).tolist()} mm;四元数跨度 {np.ptp(Q,0).round(3).tolist()}") print(("" if nc >= 4 else "⚠ 不同角点 <4,无法求解。") + " 下一步:.venv/bin/python scripts/solve_base_board.py --capture " + a.capture) if __name__ == "__main__": main()