把当前控制栈收成一份可克隆的源码：腕部看不见时用顶视一次定到基座，二阶段对孔和手外标定跟着进同一份树。

现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。

Co-authored-by: Cursor <cursoragent@cursor.com>

scripts/sense_tool_length.py

@@ -0,0 +1,309 @@
+#!/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())