120 lines
6.6 KiB
Python
120 lines
6.6 KiB
Python
"""手眼标定·触碰记录器(只读)。掉使能手动摆位,上使能且确认反馈有效后记录;固定法兰参考点对准锚点右下格子中心,本脚本 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()
|