Files

98 lines
5.0 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""手眼标定·触碰记录器(只读)。臂掉使能、手引导食指尖依次碰板上已知角点,本脚本 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()