arm_dev
7d22f777c8
现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。 Co-authored-by: Cursor <cursoragent@cursor.com>
310 lines
13 KiB
Python
310 lines
13 KiB
Python
#!/usr/bin/env python3
|
||
"""真机标定 `tool_tip_below_flange_m`：**规尖**在法兰下方多远 —— 视觉感知。
|
||
|
||
为什么需要单独标定
|
||
------------------
|
||
规是**夹在爪里**的，露出多少随装夹变化（仿真上料脚本实测见过 243 / 254mm 两种）。
|
||
而真机上**没有"规尖"的 TF**：规只是被夹住的一个零件，TF 树里查不到它。所以只能
|
||
**感知**：腕部相机看到规，用 `T_base = T_base_flange · T_flange_cam` 把手眼链
|
||
反投影回基座，量「法兰 z − 规尖 z」。
|
||
|
||
这个值一旦偏小，下游几何就会把规按进工件（site.yaml 记着 2026-09-29 的事故：
|
||
悬停这一步就把规插进工件，S05 横向对准变成拿规尖拨工件，工件被推走 19mm）。
|
||
|
||
口径（必须显式）
|
||
----------------
|
||
* 量的是**竖直**距离（基座 z 之差），不是相机光轴方向的深度 —— 两者因相机
|
||
29.7° 的倾角不同，混用会差 20mm 量级。
|
||
* 只在法兰近似**竖直朝下**时成立（悬停/跟踪都是该姿态）。
|
||
|
||
自证（不许"看起来差不多"）
|
||
--------------------------
|
||
先把**当场**反投影出来的**工件上表面** z 与 site.yaml 记录的
|
||
`workpiece_surface_z_m` 比：差 > `--tol-mm`（默认 5mm）就判链路/手眼/深度有
|
||
系统性问题并**拒绝输出**。这一步用的是独立已知量，能真的失败。
|
||
|
||
用法
|
||
----
|
||
bash ~/ros2/scripts/run_real.sh --arm 89 # 另开终端：起控制栈
|
||
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 # 起相机
|
||
# 把臂开到**悬停高**（stage2 的 hole_hover_z_m，竖直朝下、规在视野里）
|
||
python3 ~/ros2/scripts/sense_tool_length.py --sn Rizon4-063989
|
||
|
||
输出一行 `**tool_tip_below_flange_m: 0.xxx**` 与它是否通过自证。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import sys
|
||
import time
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
|
||
ROOT = Path(__file__).resolve().parents[1]
|
||
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
|
||
|
||
|
||
def _load_frame(sn: str, handeye_file: Path):
|
||
"""读手眼 T_flange_cam（eye-in-hand）。缺文件直接失败。"""
|
||
import yaml
|
||
|
||
doc = yaml.safe_load(handeye_file.read_text(encoding="utf-8")) or {}
|
||
if "T_4x4" not in doc:
|
||
raise SystemExit(f"手眼文件 {handeye_file} 里没有 T_4x4（键：{sorted(doc)}）")
|
||
return np.array(doc["T_4x4"], dtype=float)
|
||
|
||
|
||
class _Grabber:
|
||
"""抓彩色/对齐深度/内参 + 查 TF。放在类里是为了让订阅和 TF 监听一起 spin。"""
|
||
|
||
def __init__(self, cam_ns: str):
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from rclpy.qos import QoSProfile, ReliabilityPolicy
|
||
from sensor_msgs.msg import CameraInfo, Image
|
||
import tf2_ros
|
||
|
||
self.rclpy = rclpy
|
||
self.node = Node("sense_tool_length")
|
||
self.color = self.depth = self.info = None
|
||
qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.BEST_EFFORT)
|
||
ns = cam_ns.strip("/")
|
||
self.node.create_subscription(Image, f"/{ns}/color/image_raw",
|
||
lambda m: setattr(self, "color", m), qos)
|
||
self.node.create_subscription(Image, f"/{ns}/aligned_depth_to_color/image_raw",
|
||
lambda m: setattr(self, "depth", m), qos)
|
||
self.node.create_subscription(CameraInfo, f"/{ns}/color/camera_info",
|
||
lambda m: setattr(self, "info", m), qos)
|
||
self.buf = tf2_ros.Buffer()
|
||
self.lis = tf2_ros.TransformListener(self.buf, self.node)
|
||
|
||
def spin(self, dt: float = 0.05) -> None:
|
||
self.rclpy.spin_once(self.node, timeout_sec=dt)
|
||
|
||
def wait_frames(self, timeout_s: float = 8.0) -> bool:
|
||
t0 = time.monotonic()
|
||
while time.monotonic() - t0 < timeout_s:
|
||
self.spin()
|
||
if self.color is not None and self.depth is not None and self.info is not None:
|
||
return True
|
||
return False
|
||
|
||
def need_fresh(self, timeout_s: float = 4.0) -> bool:
|
||
t0 = time.monotonic()
|
||
while time.monotonic() - t0 < timeout_s:
|
||
self.spin()
|
||
if self.color is None or self.depth is None:
|
||
continue
|
||
now = self.node.get_clock().now().nanoseconds * 1e-9
|
||
|
||
def age(m):
|
||
return now - m.header.stamp.sec - m.header.stamp.nanosec * 1e-9
|
||
|
||
if age(self.color) < 1.0 and age(self.depth) < 1.0:
|
||
return True
|
||
return False
|
||
|
||
def T(self, parent: str, child: str, timeout_s: float = 3.0):
|
||
t0 = time.monotonic()
|
||
while time.monotonic() - t0 < timeout_s:
|
||
self.spin()
|
||
try:
|
||
tr = self.buf.lookup_transform(parent, child, self.rclpy.time.Time())
|
||
except Exception:
|
||
continue
|
||
t, r = tr.transform.translation, tr.transform.rotation
|
||
x, y, z, w = r.x, r.y, r.z, r.w
|
||
M = np.eye(4)
|
||
M[:3, :3] = [
|
||
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
||
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
||
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
||
]
|
||
M[:3, 3] = [t.x, t.y, t.z]
|
||
return M
|
||
return None
|
||
|
||
|
||
def _depth_m(msg) -> np.ndarray:
|
||
if (msg.encoding or "").lower() != "16uc1":
|
||
raise SystemExit(f"深度编码应为 16UC1(mm)，实际 {msg.encoding!r}")
|
||
return (np.frombuffer(msg.data, dtype=np.uint16)
|
||
.reshape(int(msg.height), int(msg.width)).astype(np.float32) * 1e-3)
|
||
|
||
|
||
def _near_tip(depth: np.ndarray, near_m: float, min_m: float, max_m: float):
|
||
"""近端连通域里"最深"的像素 = 规尖候选。
|
||
|
||
为什么用连通域而不是全图最深处：工件面/台面比规更远，全图最深处会落到工件上；
|
||
规是**唯一**贴着相机（< near_m）的物体，所以先在近端找块，再取块内最深点。
|
||
"""
|
||
import cv2
|
||
|
||
valid = (depth > min_m) & (depth < max_m)
|
||
near = (valid & (depth < near_m)).astype(np.uint8)
|
||
if int(near.sum()) < 150:
|
||
return None
|
||
n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8)
|
||
if n <= 1:
|
||
return None
|
||
big = max(range(1, n), key=lambda i: stats[i, 4])
|
||
ys, xs = np.where(lab == big)
|
||
dv = depth[ys, xs]
|
||
k = np.percentile(dv, 97) # 块内最深 3% ≈ 规尖
|
||
sel = dv >= k - 1e-6
|
||
ty = int(round(float(np.median(ys[sel]))))
|
||
tx = int(round(float(np.median(xs[sel]))))
|
||
return tx, ty
|
||
|
||
|
||
def main(argv=None) -> int:
|
||
ap = argparse.ArgumentParser(description="真机视觉标定 tool_tip_below_flange_m")
|
||
ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml robot.sn）")
|
||
ap.add_argument("--site", default=str(ROOT / "config" / "site.yaml"))
|
||
ap.add_argument("--frames", type=int, default=8, help="取多少帧求中值")
|
||
ap.add_argument("--near-m", type=float, default=0.15, help="近端阈值：规比这更近 [m]")
|
||
ap.add_argument("--min-m", type=float, default=0.04)
|
||
ap.add_argument("--max-m", type=float, default=0.60)
|
||
ap.add_argument("--tol-mm", type=float, default=5.0,
|
||
help="反投影出的工件面 z 与 site.yaml 记录值的最大允许差 [mm]")
|
||
ap.add_argument("--save", default="", help="把带标记的彩图写到这个路径（可选）")
|
||
args = ap.parse_args(argv)
|
||
|
||
import export_site_env as ex
|
||
|
||
data = ex.load_site(Path(args.site))
|
||
sn = (args.sn or "").strip() or str((data.get("robot") or {}).get("sn") or "").strip()
|
||
if not sn:
|
||
raise SystemExit("没给 --sn，且 site.yaml 的 robot.sn 为空")
|
||
merged, robot = ex.apply_active_arm(data, sn)
|
||
cam_ns = ex.camera_ns(merged)
|
||
base = str(merged.get("servo", {}).get("base_frame") or f"{sn}_base_link")
|
||
flange = f"{sn}_flange"
|
||
he_file = Path(str((merged.get("paths") or {}).get("handeye_file") or ""))
|
||
if not he_file.is_file():
|
||
raise SystemExit(f"手眼文件不存在：{he_file}")
|
||
gp = (merged.get("gauge_assembly") or {})
|
||
surface_ref = float(gp.get("workpiece_surface_z_m") or 0.0)
|
||
cur = float(gp.get("tool_tip_below_flange_m") or 0.0)
|
||
|
||
print(f"=== 真机工具长标定 SN={sn} cam=/{cam_ns.strip('/')} ===")
|
||
print(f" 基座帧={base} 法兰帧={flange}")
|
||
print(f" 手眼 ={he_file}")
|
||
print(f" site 现值 tool_tip_below_flange_m={cur:.4f} 工件面={surface_ref:.4f}")
|
||
|
||
import rclpy
|
||
|
||
rclpy.init()
|
||
try:
|
||
g = _Grabber(cam_ns)
|
||
except Exception as exc: # noqa: BLE001
|
||
print(f"节点初始化失败：{exc}", file=sys.stderr)
|
||
return 2
|
||
|
||
try:
|
||
if not g.wait_frames(10.0):
|
||
print("错误：没收到相机帧。先起相机：bash scripts/run_camera_d405.sh --arm <臂>",
|
||
file=sys.stderr)
|
||
return 1
|
||
T_flange_cam = _load_frame(sn, he_file)
|
||
|
||
tips, flange_zs, surf = [], [], []
|
||
for i in range(max(1, args.frames)):
|
||
if not g.need_fresh():
|
||
continue
|
||
dep = _depth_m(g.depth)
|
||
k = g.info.k
|
||
fx, fy, cx, cy = float(k[0]), float(k[4]), float(k[2]), float(k[5])
|
||
T_bf = g.T(base, flange)
|
||
if T_bf is None:
|
||
print("错误：TF 查不到 base→flange（控制栈起了吗？）", file=sys.stderr)
|
||
return 1
|
||
T_bc = T_bf @ T_flange_cam
|
||
got = _near_tip(dep, args.near_m, args.min_m, args.max_m)
|
||
if got is None:
|
||
continue
|
||
ux, vy = got
|
||
# 尖端深度取 5x5 中值抑噪；再用整条链反投到基座
|
||
y0, y1 = max(0, vy - 2), min(dep.shape[0], vy + 3)
|
||
x0, x1 = max(0, ux - 2), min(dep.shape[1], ux + 3)
|
||
patch = dep[y0:y1, x0:x1]
|
||
pv = patch[(patch > args.min_m) & (patch < args.max_m)]
|
||
if pv.size == 0:
|
||
continue
|
||
zt = float(np.median(pv))
|
||
p_cam = np.array([(ux - cx) * zt / fx, (vy - cy) * zt / fy, zt, 1.0])
|
||
tip_base = (T_bc @ p_cam)[:3]
|
||
tips.append(float(T_bf[2, 3]) - float(tip_base[2]))
|
||
flange_zs.append(float(T_bf[2, 3]))
|
||
# 工件面：取画面下部中央一块（工件/台面），用同一链反投影
|
||
h, w = dep.shape
|
||
band = dep[int(h * 0.80):int(h * 0.95), int(w * 0.30):int(w * 0.70)]
|
||
bv = band[(band > args.min_m) & (band < args.max_m)]
|
||
if bv.size > 50:
|
||
zb = float(np.median(bv))
|
||
bu, bvv = int(w * 0.50), int(h * 0.88)
|
||
pb = (T_bc @ np.array([(bu - cx) * zb / fx, (bvv - cy) * zb / fy, zb, 1.0]))[:3]
|
||
surf.append(float(pb[2]))
|
||
if args.save and args.frames == 1:
|
||
try:
|
||
import cv2
|
||
bgr = np.frombuffer(g.color.data, np.uint8).reshape(
|
||
int(g.color.height), int(g.color.width), 3).copy()
|
||
cv2.circle(bgr, (ux, vy), 8, (0, 0, 255), 2)
|
||
cv2.imwrite(args.save, bgr)
|
||
except Exception:
|
||
pass
|
||
g.spin(0.05)
|
||
|
||
if not tips:
|
||
print("错误：没找到规（近端 < %.0fmm 的物体）。确认规夹在爪里、且臂在竖直朝下姿态。"
|
||
% (args.near_m * 1000), file=sys.stderr)
|
||
return 1
|
||
|
||
tl = float(np.median(tips))
|
||
spread = (max(tips) - min(tips)) * 1000.0
|
||
fz = float(np.median(flange_zs))
|
||
print(f"\n 帧数={len(tips)} 法兰 z={fz:.4f}")
|
||
print(f" 感知工具长 = **{tl:.4f} m** (逐帧散布 {spread:.1f}mm)")
|
||
|
||
ok = True
|
||
tip_z = fz - tl
|
||
if surf:
|
||
zs = float(np.median(surf))
|
||
# 画面下部那块可能是**工件面**（site 记录 surface_ref）也可能是**台面**
|
||
# （真机基座与台面同高 ⇒ ≈0）。两者都是**独立已知量**，用它们卡链路：
|
||
# 只要反投影出的 z 落在允许窗内，就说明手眼/深度/姿态没有系统性问题。
|
||
lo = min(0.0, surface_ref) - 0.03
|
||
hi = max(0.0, surface_ref) + 0.04
|
||
tag = "OK" if lo <= zs <= hi else "FAIL"
|
||
print(f" 自证 反投影支撑面 z={zs:.4f}"
|
||
f"（site 工件面 {surface_ref:.4f}、台面≈0）允许 [{lo:+.3f},{hi:+.3f}] → [{tag}]")
|
||
if not (lo <= zs <= hi):
|
||
ok = False
|
||
if not (tip_z > zs + 0.003):
|
||
print(f" 自证：规尖 z={tip_z:.4f} 不在支撑面 z={zs:.4f} 之上 → FAIL")
|
||
ok = False
|
||
else:
|
||
print(" 自证：画面下部没采到有效深度，跳过（不敢保证链路）")
|
||
if not (0.10 <= tl <= 0.35):
|
||
print(f" 自证：工具长 {tl:.3f} 不在合理区间 [0.10, 0.35] → FAIL")
|
||
ok = False
|
||
|
||
if not ok:
|
||
print("\n ✗ 自证未过 —— **不要**把这个值写进 site.yaml，先查手眼/深度/姿态。")
|
||
return 4
|
||
print(f"\n ✓ 可写：gauge_assembly.tool_tip_below_flange_m: {tl:.3f}"
|
||
f"（现值 {cur:.4f}，差 {(tl - cur) * 1000:+.1f}mm）")
|
||
return 0
|
||
finally:
|
||
try:
|
||
g.rclpy.shutdown()
|
||
except Exception:
|
||
pass
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|