Files

120 lines
6.6 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 读所选臂的 arm_state(默认 /left/arm_state,末端 pose 在 base_link 系)。全程只订阅、不发任何指令。
健壮性:每次记录都【强制等一条新消息】再 snapshot(避免 input() 阻塞期间读到陈旧位姿);
并检测与上一条是否雷同(臂没动/位姿流卡住会立刻报警)。
用法:.venv/bin/python scripts/record_touch.py --capture data/real_calib
交互:固定法兰参考点对准第 k 号格子中心(见 out/align_lines.png)、手腕朝向每次都变一变 → 输入 k 回车记一条;
s=看实时位姿, u=撤销上一条, done=结束存盘。要 ≥5 个角点、朝向各不相同。
"""
import argparse, json, time
from storage import save_json, stamp
from arm_contract import check_sample_arm
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="清空已有样本重新开始")
ap.add_argument("--arm", choices=["left", "right"], default="left")
a = ap.parse_args()
cap = ROOT / a.capture
cam = json.loads((cap / 'camera.json').read_text()); cols, rows = cam['board_grid']
guide = cap / f'align_lines_{a.arm}.png'
if not guide.exists():
raise SystemExit('缺少所选臂对准图,请先重绘:'+str(guide))
print(f"请使用{a.arm}臂对准图:{guide}")
samples = []
f = cap / "touch_samples.json"
if f.exists() and not a.fresh:
data = json.loads(f.read_text())
check_sample_arm(data, a.arm)
samples = data.get("samples", [])
print(f"已有 {len(samples)} 条 {a.arm} 臂样本,将在其后追加")
ros = roslibpy.Ros(host=a.host, port=a.port); ros.run(timeout=8)
print("connected:", ros.is_connected)
st = {"m": None, "n": 0}
topic = f"/{a.arm}/arm_state"
print(f"采集 {a.arm} 臂:{topic}")
tp = roslibpy.Topic(ros, topic, "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:
tp.unsubscribe(); ros.terminate()
raise SystemExit(f"收不到 {topic} 新消息——确认 web 控制器在线、rosbridge 通")
def pose_of(m):
return np.array([m["pose"]["position"][k] for k in "xyz"])
print("== 固定法兰参考点对准锚点右下格子中心、上使能确认反馈后,每次换个手腕朝向 → 输入 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']}")
save_json(f, {'samples':samples,'host':a.host,'arm':a.arm,'target_definition':'anchor_lower_right_cell_center'})
continue
try:
k = int(s)
except ValueError:
print(" 请输入角点编号(整数) / s / u / done"); continue
if not (0 <= k < cols*rows and k % cols < cols-1 and k // cols < rows-1):
print('编号无右下格子,拒绝记录'); continue
m = fresh()
if m is None:
print(" ⚠ 2 秒内没收到新位姿消息——位姿流卡住了,没记录。检查 web 控制器/网络"); continue
p = m["pose"]["position"]; q = m["pose"]["orientation"]
angles=np.array(m['joint_state']['position'],float)
if len(angles)!=7 or not np.isfinite(angles).all() or np.max(np.abs(angles)) < np.deg2rad(.02):
print('拒绝记录:全零附近固定值或无效角度,请确认实际反馈恢复;收到新消息不代表编码器更新。'); continue
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({"arm":a.arm,"recorded_at_local":stamp(),"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"])})
save_json(f, {'samples':samples,'host':a.host,'arm':a.arm,'target_definition':'anchor_lower_right_cell_center'})
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)
save_json(f, {"samples": samples, "host": a.host,"arm":a.arm,"target_definition":"anchor_lower_right_cell_center"})
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,无法求解。") +
f" 下一步(在标定工具目录):.venv/bin/python scripts/solve_base_board.py --capture {cap.resolve()} --arm {a.arm}")
print(f"当前会话也可运行:bash run.sh solve --arm {a.arm}")
if __name__ == "__main__":
main()