98 lines
5.0 KiB
Python
98 lines
5.0 KiB
Python
"""手眼标定·触碰记录器(只读)。臂掉使能、手引导食指尖依次碰板上已知角点,本脚本 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()
|