#!/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())