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

现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。 Co-authored-by: Cursor <cursoragent@cursor.com>
2026-10-09 14:51:03 +08:00
parent 16d2912a9a
commit 7d22f777c8
67 changed files with 8597 additions and 409 deletions
@@ -67,6 +67,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
 | [docs/FRAMEWORK_RULES.md](docs/FRAMEWORK_RULES.md) | **框架规则**（改代码前先读；含历史反例的症状表） |
 | [docs/WORKSPACE.md](docs/WORKSPACE.md) | 每个目录 / 脚本 / 包 |
 | [docs/QUICKSTART.md](docs/QUICKSTART.md) | 环境与第一次跑通 |
 | [docs/ENVIRONMENT.md](docs/ENVIRONMENT.md) | 在另一台电脑上装出和本机相同的 ROS / conda / RDK 环境 |
 | [docs/ARCHITECTURE.md](docs/ARCHITECTURE.md) | 进程与几何 |
 | [docs/SITE_CONFIG.md](docs/SITE_CONFIG.md) | `site.yaml` 字段 |
 | [docs/HANDEYE.md](docs/HANDEYE.md) | 手眼标定 |
@@ -26,14 +26,14 @@ robot:
  # >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
  sn: "Rizon4-XXXXXX"
  # Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
-  tool_name: ""
+  tool_name: "tool1"
  # 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
  robots:
    - sn: "Rizon4-XXXXXX"
      arm: left
      ip: "192.168.2.XXX"
      pc_ip: "192.168.2.XXX"
-      iface: "enp4s0"
+      iface: "enxXXXXXXXXXXXX"
      label: "左臂89"
      # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
      # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
@@ -67,7 +67,11 @@ robot:
      # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
      # 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
      servo: &servo_left
-        ee_z_m: 0.35           # 跟踪锁高：法兰相对基座 Z [m]
+        # 2026-09-29 底座加高后由 0.35 → 0.36：工件上表面实测抬高 10mm
        # （0.012 → 0.022），锁高同步 +0.011 ⇒ 相机到工件的视距**与加高前一致**
        # （≈0.213m，跟踪那一档已实拍验证过检出稳定），不是随手抬的。
        # 规矩：ee_z_m 要高于 servo.hole_hover_z_m（二阶段得往**下**走，不能往上返）。
        ee_z_m: 0.36           # 跟踪锁高：法兰相对基座 Z [m]
        min_ee_z_m: 0.20
        safe_z_m: 0.45
        z_min_m: 0.22
@@ -77,13 +81,93 @@ robot:
        # >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<<
        # 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
        # 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
-        hole_hover_z_m: 0.23   # ← 二阶段悬停高度调这一行
+        # = 工件上表面 0.022（本臂 gauge_assembly.workpiece_surface_z_m，加底座后重测）
-        # 腕部 FT 话题**不在这里配**：默认按 sn 推导 = /<sn>/ft_sensor_wrench，
+        #   + 工具长 0.245 + 间隙 0.002
-        # 由真机 flexiv_robot_states_broadcaster 无条件发布，仿真侧也是同名同类型
+        # 2026-09-29 实测：按 0.23 悬停时工具尖端落在工件面**下** 5~10mm，
-        # （见 devices/sim/gazebo/config/wrist_ft.yaml）。
+        # 悬停这一步就把通止规插进了工件（见本臂 gauge_assembly 段的完整说明）。
-        # 只有换 FT 来源时才需要显式写 servo.ft_wrench_topic，例如改用
+        #
-        #   /<sn>/external_wrench_in_tcp（已做重力/负载补偿，基线≈0，
+        # 2026-09-29 二次修正 —— 上面那条"工具几何"只保证**不碰工件**，它保证不了
-        #   对"轻碰"更灵敏，但对负载变化更敏感）。
+        # **相机看得见孔**，而二阶段的第一步就是拍 k1/k2。旧值 0.259 的后果实测：
        #   相机光心在法兰下方 0.136m（手眼 T_cam_to_gripper + 29.7° 倾角）
        #   法兰 0.259 → 相机离工件面 0.123m → 检出 0/16，conf 峰值 0.012（噪声）
        #   法兰 0.309 → 相机离工件面 0.173m → 检出 16/16，conf 0.87，
        #                k0/k1/k2 置信度全 0.999~1.000，k1-k2 间距 156px 稳定
        # gauge_fsm.hover_z_guard 的设计注释也写着"腕部相机要离工件面 ~0.17m
        # 才看得清孔"，所以 0.309 才是这条同时满足"相机看得见"和"规尖不碰"的值
        # （规尖此时在面上 64mm，比旧值更安全）。
        # 最后 2mm 的贴近由 stage2 的**深度闭环轻降**（final_gap_mm）完成 ——
        # 它按实测间隙往下走，与绝对悬停高无关，所以抬高悬停不会让尖撞工件。
        #
        # 2026-09-29 三次修正（工件加了底座，工作平面重新感知）：
        #   拿 gongjian0929 在**实时图**上出 k0/k1/k2，再取各点邻域深度投到基座系：
        #     k1 = +0.0220 / +0.0221 / +0.0223 m（多帧重复性 0.4mm 内）
        #     k2 = +0.0223 / +0.0224 m
        #     k0 = +0.0165 / +0.0186 m（偏低约 6mm 且抖，局部读数噪声大）
        #   三点"中心 vs 外环"深度差 ≤1mm ⇒ 三点都落在**上表面**上，不是孔底 ⇒
        #   上表面取 k1/k2 的一致值 **0.022**（原 0.012 是加底座前的旧值）。
        #   悬停随之 0.309 → 0.320（同样 +0.011），让"相机到工件面"的**视距保持
        #   ≈0.17m 不变** —— 0.309 那一档是实拍验证过"16/16 检出、conf 0.87、
        #   k1-k2 间距 156px 稳定"的工作点，改高度就是在改视距，不能随手抬。
        #   几何兜底仍然满足：0.320 − 工具长 0.245 = 尖端 0.075，工件面 0.022
        #   ⇒ 尖端在面上方 53mm（要求 ≥ final_gap 2mm）。
        hole_hover_z_m: 0.320  # ← 二阶段悬停高度调这一行
        # FT 数据源：默认按 sn 推导 = /<sn>/ft_sensor_wrench，由真机
        # flexiv_robot_states_broadcaster 发布。但 **2026-10-08 实测该话题恒为
        # 全零（0,0,0）** —— RDK 那侧的原始 FT 传感没在出数（`ft_sensor_raw` 为 0），
        # 于是应用里"abs(fz−基线)>2.5N 判接触"的兜底**永远判不到接触**（静默失效）。
        # 同一时刻 /<sn>/external_wrench_in_tcp 是有数的（≈负载 54N，且随受力变化），
        # 所以真机改用它。判接触只看"相对轻降起始基线的增量"，与 54N 这个偏置无关，
        # 用这个源可直接恢复接触兜底，不需要改代码。
        # 仿真**别跟着用**（仿真没有这个话题）—— 见本文件 sim 段的显式覆盖。
        # 注意：这不是话题名写错，而是"哪个数据源有数"的取舍（真机 RDK 未启用原始 FT）。
        ft_wrench_topic: "/Rizon4_XXXXXX/external_wrench_in_tcp"
      # ---- 工件几何（2026-09-29 现场实测标定）----
      # 真机段的坐标都是**基座系**；本机基座与台面同高 ⇒ 台面 z ≈ 0。
      gauge_assembly:
        # 工件**上表面**的世界 z [m]。悬停高度的几何判据（gauge_fsm.hover_z_guard）
        # 与 S07 下探预算都拿它当基准面。
        #
        # ⚠️ 为什么不能留全局段的默认 0.0：0.0 = "未知"，运行期退回"用 stage2 实测
        #    孔位 z 兜底" —— 但兜底发生在**悬停之后**，悬停本身还是按 0.0 算的。
        #    后果（2026-09-29 实测）：
        #      悬停 0.247 − 工具长 0.245 = 工具尖端 z≈0.002，而工件面在 0.0074
        #      ⇒ 尖端在工件面**下** 5~10mm。悬停这一步就把通止规插进工件，接着
        #      S05 的横向对准等于拿规尖**拨工件**：两孔中点两拍之间被推走 19.1mm，
        #      残差 27.3 → 42.7mm 反向发散（日志看着像手眼标定坏了），最后被
        #      drag_verdict 看门狗拦下（exit=6）。
        #
        # 实测依据（旧，加底座前）：stage2 解出的两孔基座系 z = 0.00743 / 0.00715 m
        #   （/tmp/stage2_result.json 的 k1_base_m / k2_base_m）。孔是通孔，
        #   深度相机在孔口附近读数会偏深 ⇒ 上表面取工件板厚 **0.012**。
        #
        # 实测依据（2026-09-29 加底座后**重新感知**，现行值 = 0.022）：
        #   不再靠"板厚"推，而是直接量上表面 —— 让 gongjian0929 在实时图上出
        #   k0/k1/k2，对每个点取 (±6px) 邻域深度的中位数，用同一帧 TF
        #   (base←flange · flange←cam) 投到基座系：
        #     k1 = +0.0220 / +0.0221 / +0.0223，k2 = +0.0223 / +0.0224（重复性 0.4mm）
        #     k0 = +0.0165 / +0.0186（偏低 6mm 且抖 ⇒ 该点邻域读数不可靠，不参与定面）
        #   判"是不是孔底"：各点中心 (±6px) 与外环 (12~20px) 深度差 ≤1mm ⇒ 齐平
        #   ⇒ 三点都在上表面。ROI 高度直方图同为单峰平台，p75≈0.0228、p95≈0.0247
        #   ⇒ 上表面 0.022（比加底座前的 0.012 高 10mm）✓ 与"工件加了底座变高"一致。
        #   法兰 Z=0.356 时该点光学视距实测 0.204m（模型仍检出 conf 0.862）⇒
        #   模型对 0.17~0.20m 有一档余量，但两侧都不要顶到边界。
        # 复核（会同时打印实测工件面 + 悬停几何自检）：
        #   bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
        #   python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
        workpiece_surface_z_m: 0.022
        # 工具长（规尖在法兰下方的距离）—— **2026-10-08 真机感知标定**：0.223m。
        #   方法：hover(法兰 0.320) 下腕部相机抓一帧，取规杆最深像素 + 深度，用
        #   手眼 T_base = T_base_flange·T_flange_cam 投到基座 ⇒ 规尖 z=0.0971，
        #   故 0.320 − 0.0971 = 0.2229m。同帧反投影链的独立校验：k1/k2 基座 z
        #   精确复现上次实测 0.0219/0.0204（误差 <0.1mm），链路可信。
        #   工具长 = tool1 夹爪 TCP 0.147 + 规露出 0.076。
        #   ⚠️ 旧值 0.245 是照抄仿真的（TCP 0.200 + 规 0.043），比真机长 22mm：
        #   它让几何误以为悬停时尖端只在面上方 53mm（实际 75mm），轻降预算因此
        #   少 22mm，**永远到不了孔口**（现象就是"机械臂不下臂"）。
        #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
        #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
        #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
        tool_tip_below_flange_m: 0.223
    # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
    # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
    #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -153,6 +237,10 @@ robot:
      #    夹好规后读到的 TF 值写进全局段的 tool_tip_below_flange_m。
      servo:
        <<: *servo_left
        # 仿真 FT：由 sim_wrist_ft 发布，与真机 /<sn>/ft_sensor_wrench 同名同类型同单位。
        # 必须显式覆盖 —— 上面 *servo_left 带下来的 ft_wrench_topic 是**真机**的
        # external_wrench_in_tcp，仿真里没有这个话题（不覆盖 ⇒ 仿真 FT 收不到）。
        ft_wrench_topic: "/sim/ft_sensor_wrench"
        ee_z_m: 0.72              # 一阶段跟踪锁高（法兰，世界系）
                                  # 不必随悬停一起抬：跟踪时工具尖端离面 ~60mm，
                                  # 只要**高于悬停**就不碰工件（0.72 > 0.657）。
@@ -188,7 +276,7 @@ robot:
      arm: right
      ip: "192.168.2.XXX"
      pc_ip: "192.168.2.XXX"
-      iface: "enp4s0"
+      iface: "enxXXXXXXXXXXXX"
      label: "右臂91"
      camera:
        name: d405_right         # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
@@ -221,6 +309,9 @@ robot:
        hover_offset_ee_xy_m: [0.0, 0.0]  # RGI 与法兰同轴；爪心偏了再改这两项 [m]
        # 二阶段孔定位悬停锁高（91 装了传感器后同样可用）
        hole_hover_z_m: 0.22
        # FT 数据源同 89：真机 ft_sensor_wrench 恒零，改用有数的 external_wrench_in_tcp
        # （见 89 段 ft_wrench_topic 的完整说明）。
        ft_wrench_topic: "/Rizon4_XXXXXX/external_wrench_in_tcp"
        # --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方
 paths:
@@ -228,19 +319,24 @@ paths:
  #   1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
  #   2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
  #   3) backend=tensorrt 时必须先导出引擎（只需做一次）：
-  #        bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640
+  #        bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
  #   4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640）
  #   5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control]
  #   6) 换完确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines
-  # 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入）
+  # 当前：gongjian0929（2026-09-29 从 ~/下载 转入）。
-  yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx"
+  # ⚠️ 这是个 **pose 模型**（YOLO26s-pose）：一个工件框 + 3 个关键点
  #    k0=跟踪孔 / k1,k2=对孔。它同时替代了原来的跟踪模型和入孔模型，
  #    所以下面 hole_weights 也指向同一份。
  #    ultralytics 解不了 pose 的 TensorRT engine，运行时走
  #    arm_application/pose_detector.py 的自解码（见该模块头）。
  #    仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
  yolo_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
  # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
  # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
-  # 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
+  # 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
-  # 原先指向 hole_insert/best.pt —— 那个文件不存在（悬空引用）。这里改成实际存在的
+  # pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
-  # detect_best_1.onnx，这样 backend: yolo 至少是能跑的。查悬空引用：
+  # 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
-  #   bash ~/ros2/scripts/weights.sh doctor
+  hole_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
  hole_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
  # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
  handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
  handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
@@ -251,11 +347,23 @@ paths:
 handeye:
  mode: "eye_in_hand"
-  # >>> 标定板尺寸（内角点数 + 方格边长 m）；改这里即可 <<<
+  # >>> 标定板尺寸：**按模式各一份**（内角点数 + 方格边长 m）<<<
-  # 例：A5 板 12×17 方格 → 内角点 11×16，方格 11 mm
+  #
-  board_cols: 8          # 内角点列数 (方格列数 - 1)
+  # 为什么必须按模式分开写（而不是两个模式共用一组键）：
-  board_rows: 11          # 内角点行数 (方格行数 - 1)
+  # 眼在手上与眼在手外通常用**两块不同的物理板**，方格边长不一样。共用一组键
  # 的后果不是报错，而是"跑另一种模式时静默用错方格边长"—— 标定照样跑完、
  # 结果只是系统性偏，现场几乎联想不到板尺寸（历史反例见 handeye/README.md 第 3 节）。
  # 内角点 = 方格数 − 1；列/行顺序不用纠结，检测端两种顺序都会试
  #（见 handeye_lib.detect_chessboard）。
  boards:
    eye_in_hand:            # A5 板：9×12 方格
      cols: 8               # 内角点列数
      rows: 11              # 内角点行数
      square_size_m: 0.015  # 方格边长 [m]
    eye_to_hand:            # A4 板：12×9 方格
      cols: 11
      rows: 8
      square_size_m: 0.020
  # 运动更顺滑：慢速 + 到位等静止 + 零速 pin 后再拍照
  velocity_scale: 0.05
  move_time_sec: 8.0
@@ -358,6 +466,18 @@ yolo:
  backend: "tensorrt"        # 强制 .engine；没有则启动失败。导出：bash ~/ros2/scripts/export_yolo_trt.sh
  mp_infer: true             # YOLO 独立进程，不和 ROS 抢 GIL
  min_conf: 0.12             # 新锁门槛；已锁定的框可略低于此
  # 重新捕获门槛：**跳到新位置**（离上一帧框中心 >max_track_px）必须达到的置信度。
  # 为什么必须比 min_conf 高得多 —— 2026-10-08 真机实测（一阶段 servo_node 日志）：
  #   0659.551  死区保持 |e_xy|=3.0mm              ← 已经收敛到 3mm
  #   0659.598  k0 置信度 0.00 < 0.30，本帧不用它
  #   0659.6xx  重新捕获 conf=0.15 dpx=503         ← 503px 外、conf 0.15 的假框
  #             feat_base=(0.231,-0.484) z=-0.021  ← 跟踪点瞬间横移 427mm
  #   0670.985  === 一阶段已锁定 ===               ← "锁定"在离工件 500mm 的位置
  # 那串假框实测 conf 0.13~0.47，而真 k0 是 0.87。旧写法把这里接成 min_conf=0.12
  # ⇒ 0.15 的假框被原样采信、perception 的 kf.reset() 直接跳过去，伺服再拦也只是
  # 拦一帧（假框帧间稳定，3 帧后 FeatureJumpGate 也会认）。
  # 取 0.55：高于假框上沿 0.47，低于真 k0 0.87。宁可原地不动，也不追假点。
  reacquire_conf: 0.55
  vision_hz: 30.0
  infer_max_width: 640       # 进 GPU 进程前缩到 640，减小进程间拷图，避免队列堵
  box_ema_alpha: 1.00          # 绿框用预测补偿后的框，不再 EMA
@@ -370,6 +490,17 @@ yolo:
  z_ref_m: 0.22
  min_depth_m: 0.04
  max_depth_m: 0.55
  # 特征点的**相机量程下限**：比这更近的深度读数一定来自机械臂自身（爪上夹的
  # 通止规 / 相机座），不是工件。**判据是实测的两条带，不是几何推导**：
  #   机械臂自身（随臂一起动）：0.078 ~ 0.087 m   ← 2026-10-08 被误锁的假目标
  #   工件面：                0.19  ~ 0.29  m     ← 锁高下正常跟踪时的 p_cam.z
  # 取 0.14：距近带 53mm、距远带 50mm，两边都不顶边界。
  # 几何上为什么近带被占满：规尖在法兰下 0.223m 而相机只在法兰下 0.124m（handeye
  # cam_in_flange z=0.1236）⇒ 规杆整段都在相机近场；且相机光轴与法兰 Z 差 29.7°，
  # 所以"光轴视距"不等于"竖直距离"，只能靠实测划带。
  # 撞了会怎样：假目标在相机前 0.08m 且**随臂一起动**，伺服误差恒为 0 ——
  # "一阶段已锁定"会锁在自己的工具上（见上面 reacquire_conf 的记录）。
  feature_min_range_m: 0.14
  work_z_max_m: 0.50
  meas_blend: 0.85             # 3D 跟手；抖则略降
  z_alpha_min: 0.10
@@ -378,7 +509,26 @@ yolo:
  camera_frame: "d405_color_optical_frame"
  object_frame: "tracked_object"
  depth_topic: "/d405/aligned_depth_to_color/image_raw"
-  feature_mode: "plane"    # plane=框内近表面立体点（工件本体）；box=单目射线
+  feature_mode: "keypoint"  # keypoint=pose 模型的 k0（跟踪孔）；plane=框内近表面立体点；box=单目射线
  # feature_mode=keypoint 时用：跟踪用哪个关键点、它的置信度低于多少就不信它。
  # k0 = 跟踪孔（见 pose_detector 的约定）。k0 不可用时**借 k0 相对检测框的归一化
  # 偏移**在框上重建 k0 —— 位置连续、工件动时还跟着走。
  # 刻意**不**回落"框中心"（那是另一个点，离 k0 有 ~89mm，真机上就是一次不该有的
  # 横向快移，见 2026-10-08 一阶段 150s 没锁上的记录）；只有在**从未**见过可信 k0
  # 的冷启动帧才回落到框中心/框内深度。
  keypoint_index: 0
  keypoint_min_conf: 0.30
  keypoint_win_px: 6        # k0 处取多大窗口的中位深度
  # ---- 跟踪点可信性的三道守卫（实现：feature_guard.py，单一数据源）----
  # 为什么需要：2026-10-08 真机 arm 89，一阶段真 k0（conf 0.87 uv=(410,246)）被一串
  # **高置信度假 k0** 顶掉，伺服 1s 内把臂从 home 带到工作盒角落 (0.083,-0.515)，
  # 之后 148s 全是"检测漏检"、150s 超时。假 k0 的 conf 是 0.53~0.73（真值 0.87）
  # ⇒ 置信度没有可分区间；但它们**贴在画面边上**、且**借深度**：
  #   766.96 uv=(76,75)  768.16 uv=(143,92)  768.78 uv=(12,30)  770.07 uv=(431,226)
  keypoint_border_px: 6     # k0 离画面边缘 < 此值即判位置不可信（孔心不在画面边上）
  track_max_step_m: 0.04    # 已发布跟踪点的每帧位移上限 [m]（实测真实运动 ≤15mm/帧）
  reacquire_far_px: 120     # 远于此像素距离的"重新捕获"要连续确认（dpx 那个门限）
  reacquire_confirm_frames: 3  # 远框要连续几帧指向同一处才采信
  plane_inlier_m: 0.008    # 拟合平面内点阈值 [m]
  plane_band_m: 0.06       # 近表面深度带宽 [m]
@@ -409,7 +559,16 @@ servo:
  predict_enable: true        # 基座系恒速预测，补视觉延迟
  predict_sec: 0.16           # 额外前瞻 [s]
  predict_v_max: 0.80         # 工件速度上限 [m/s]，防乱飞
-  predict_jump_m: 0.08        # 基座 XY 跳变大于此则重锁速度
+  predict_jump_m: 0.08        # 基座 XY 跳变大于此则视为"疑似假跳"，转入确认流程
  # 大跳变要连续多少帧指向同一新位置才采信（见 feature_jump_guard.py 的注释）。
  # 30Hz 下 3 帧 ≈ 0.1s：真的重定位 0.1s 就被认；假点（乱跳）永远认不了。
  # 为什么不能是 1：2026-10-08 真机，一阶段已收敛到 3mm 后 k0 掉信、跟踪点瞬间
  # 横移 89mm，旧逻辑单帧就 reset 采信 → 伺服以 0.45m/s 去追假点 → 彻底丢失。
  jump_confirm_frames: 3
  # 特征点位置合理性：必须落在工作盒 XY 内（±此余量 [m]），否则判误检、当"没有目标"。
  # 为什么需要它：2026-10-08 真机，模型在运动模糊下把特征点丢到基座后方
  # (-0.10,-0.65)（工件其实在 (0.5,-0.12)），伺服一路追到工作盒边缘卡住。
  feat_ws_margin_m: 0.05
  kp_xy: 4.20                # 很跟手：v = kp × 误差，再经加速度整形；抖则降回 3.5
  ki_xy: 0.00
  kd_xy: 0.20
@@ -419,10 +578,20 @@ servo:
  i_sep_pos_m: 0.008
  deadzone_m: 0.003          # 3 mm；收紧提高对准精度。若抖动调回 0.006
  deadzone_z_m: 0.012
-  lock_hold_sec: 0.7        # 死区驻留多久才发 /visual_servo/locked（串联 0.6s 已够）
+  lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
  lock_still_sec: 3.0
  lock_still_move_m: 0.015
  e_xy_clip_m: 0.200         # 大误差仍按 v_max 走，靠 a_max 加速
  v_max_xy: 0.45             # 法兰 XY 上限 450 mm/s
  v_max_z: 0.04
  # 远处限速：目标离得比 slow_far_m 远时 XY 限速到 v_slow_xy（**与"目标真不真"无关**
  # 的一道防线）。为什么需要：2026-10-08 真机一阶段被一串高置信度假 k0（conf 0.53~0.73）
  # 顶掉，伺服 0.7s 内以 0.45m/s 把臂从 home (0.686,-0.113) 带到工作盒角落
  # (0.083,-0.515)，之后 148s 全是"检测漏检"、150s 超时。加了这道之后：真 k0（离
  # 145mm）只是晚 ~1.8s 到位，假目标最多把臂带走几厘米。
  slow_far_m: 0.05           # 目标离得超过此距离即限速 [m]
  v_slow_xy: 0.08            # 限速值 [m/s]（真目标到位后仍回到 v_max_xy）
  min_ee_z_m: 0.20
  workspace_xyz_min: [0.08, -0.55, 0.18]   # 法兰工作盒；盒外只允许往回走，不再卡死 no traj
  workspace_xyz_max: [0.85, 0.55, 0.65]
@@ -490,10 +659,18 @@ gripper:
 #   bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
 hole_detect:
  backend: "two_stage"   # two_stage | yolo | hough
-  # backend=yolo 时用。原为 hole_insert/best.pt（文件不存在），已改为实际存在的模型。
+  # backend=yolo 时用。
-  weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
+  weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
-  # 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔）
+  # 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
-  workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
+  # 所以和上面的跟踪权重是同一份。
  workpiece_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
  # pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
  # 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
  # 亮环 + Hough 那套老路径。
  keypoint_enable: true
  keypoint_min_conf: 0.30
  keypoint_min_dist_px: 15.0
  keypoint_hole_radius_px: 12.0   # 关键点没有半径；反投影时按这个半径取孔周深度环
  config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
  conf: 0.25
  iou: 0.7
@@ -532,16 +709,66 @@ hole_detect:
  # bash ~/ros2/scripts/run_stage2_locate.sh --arm 89            # 完整流程
  # bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only  # 只悬停调高度
  stage2:
-    target_hole: "k1"        # 对准目标孔：k1=图像左孔 / k2=右孔（见 k1_label）
+    # 对准目标孔。pose 模型下 k1/k2 是**模型学出来的固定语义**（不是图像左右）：
-    k1_label: "left"         # k1 是画面里的左孔；现场相反就改 right
+    #   hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2
-    align_tol_m: 0.0007      # 对准到位判据 [m]
+    # 下游是 `k1_b = pts[0] if k1_label=="left" else pts[1]`，所以：
-    align_max_iters: 8       # 拍照→修正→复拍 迭代上限
+    #   k1_label: "left"  → target_hole=k1 打的是**模型的 k1**（当前设置，推荐）
    #   k1_label: "right" → target_hole=k1 打的是模型的 k2（等价于老 Hough 的"图像左孔"）
    # 实测这台臂的视角下模型给的是 k1 在画面右、k2 在画面左；对孔顺序要反过来就改这里。
    target_hole: "k1"
    k1_label: "left"
    # ---- 对准判据：工具**用视觉量到的规尖**，不是法兰轴线（2026-10-08 真机定稿）----
    # 实测（同一帧、同一条手眼链，n=8）：
    #   · 规杆轴线相对法兰轴线 = (+10.7, −0.1)mm（σ≈0.5mm）；最深像素法 (+8.8,−1.8)mm
    #   · 而 site 原来把 hover_offset_ee_xy_m 写成 [0,0]（"平行爪同轴"），于是精对准
    #     把**法兰**对到孔上，规尖实际离孔心 ~10mm ⇒ 轻降时 φ6 规尖骑在 φ7 孔沿上，
    #     腕力从 −1N 直接蹦到 −31N→−63N 卡死（现象是"轻降在离面 4mm 停住"）。
    #   · 孔检测本身的重复性极好：k1 基座坐标 σ=(0.17,0.04)mm —— 精度不是瓶颈。
    # 所以：在同一帧里同时解「规尖基座 XY」和「孔基座 XY」，两者一起走手眼链，
    # 链路误差与装夹变动一起抵消。规尖量取规则见 _tip_offset_from_depth（四条判据）。
    align_tol_m: 0.00025     # 对准到位判据 [m]。检测重复性 0.04mm 撑得住 0.25mm。
                             # ⚠️ 不要回到 0.7mm：那会让"规偏 10mm"这种系统偏差
                             #    在容差里被放过去（原来残差 0.69mm 看着"通过"，其实
                             #    是拿法兰对的孔）。收到 0.25mm 才逼它必须真的对上规尖。
    align_max_iters: 12      # 拍照→修正→复拍 迭代上限（收紧容差后需要更多拍）
    align_tip_vision: true   # true=精对准用「视觉规尖」；false=退回旧的「法兰+偏置」
    tip_near_m: 0.15         # 规尖量取：近端阈值，规比这更近 [m]
    tip_min_m: 0.04          # 有效深度下限 [m]
    tip_max_m: 0.60          # 有效深度上限 [m]
    tip_search_band_frac: 0.25  # 中央竖带半宽 = 该比例 × 图宽（规夹在爪里，不会靠边）
    tip_min_px: 150          # 近端连通域最小像素数（少了就是没看到规）
    tip_calib_n: 4           # 定偏移需要的一致拍数（够了就冻结）
    tip_offset_tol_m: 0.003  # 样本相对中位偏移的最大允许偏差 [m]（剔偶发外点 + 冻结判据）
    tip_len_min_m: 0.15      # 反投影出的「法兰z − 规尖z」必须落在此区间 ——
    tip_len_max_m: 0.32      # 这条同时挡住"深度读到台面/工件"（主干守卫）
    coarse_max_moves: 2      # 工件粗居中（bbox 中心）移动上限
    # —— 粗居中的两条误检守卫（单源：代码读这两个键，见 _coarse_target_reject）——
    # 为什么需要：2026-10-08 真机 arm 89，粗居中拿被画面左边缘切掉的半框
    # bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
    # **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
    # 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
    coarse_max_offset_m: 0.12   # 目标离夹爪轴线超过此值就不像粗居中量 [m]
    coarse_border_px: 2         # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
    # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
    align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
    move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
    final_gap_mm: 2.0        # 对准后轻降目标：规尖悬在孔面上方这么高 [mm]（不接触；安全下限1.2mm自动停）
    final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
    # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
    #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
    #   · 2026-09-29（多降→撞工件）：环形深度中值报 gap0=141.0mm，几何真值只有 53mm
    #     （probe 0.245 版），多降 ~86mm 把通止规撞进孔，工件被 +X 推走 5mm。
    #     ⇒ 读数往**大**错一次就撞。
    #   · 2026-10-08（少降→到不了 2mm）：同一个读数悬停时报 144.0mm（几何真值 75mm），
    #     只降 24mm 后又翻成 −7.0mm，触发 SAFE_FLOOR 把轻降停在离面 ~50mm 处，
    #     最终日志却打印"悬在孔面上方 ~2.0mm"（其实停在 ~18mm）。
    #     ⇒ 读数往**小**错一次就永远到不了目标。
    #   同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
    #   和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
    # 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。
    #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
    #   + 任一步 _goto 走不到位就停。
    #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
    final_descent_enable: true  # 对准后是否轻降到 final_gap_mm（false=停在悬停高）
    verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
    settle_sec: 0.45         # 到位后静止多久再拍照
@@ -615,6 +842,22 @@ hole_detect:
 # =============================================================================
 perception:
  source: "yolo"
  # 一阶段：wrist=D405 主跟踪，画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0。
  # overhead=整段只用顶视（实验）。非法值启动即报错。
  track_camera: "wrist"
  wrist_handoff_conf: 0.5
  overhead:
    min_depth_m: 0.25
    max_depth_m: 1.80
    feature_min_range_m: 0.30
    work_z_max_m: 1.50
    z_ref_m: 0.70
    feature_mode: "plane"
    hold_point_on_miss: true
    oneshot_conf: 0.5
    cooldown_s: 1.5
    frame_max_age_s: 0.4
    oneshot_hold_s: 8.0
  truth:
    # 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空：
    # "哪个是工件"是场景的知识，换场景不该改这里）。
@@ -845,8 +1088,11 @@ go_home:
  speed: 0.25
 network:
-  # User1=enp4s0  本机 <PC_IP>  ↔  <左臂SN>=.101  /  <右臂SN>=.100
+  # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
-  prefer_iface: "enp4s0"
+  #   enxXXXXXXXXXXXX = 该 USB 网卡（名字由网卡 MAC 派生，换网卡会变）
  #   本机 <PC_IP>  ↔  <左臂SN>(左89)=.101  /  <右臂SN>(右91)=.100
  # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
  prefer_iface: "enxXXXXXXXXXXXX"
 # 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
 # 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
@@ -29,8 +29,9 @@ cameras/
 | `wrist_eye_in_hand` | D405（左腕 / 右腕各一台） | 视觉跟踪 k0、拍照推理 k1/k2 | 是（眼在手上） |
 | `overhead_eye_to_hand` | D435（双臂中间） | 粗定位兜底（S03） | 是（眼在手外） |
-**注意**：D435 当前**还没有做手眼标定**，所以 S03 粗定位链路暂时不可用。
+**注意**：真机 D435 的眼在手外标定按 `docs/HANDEYE.md` 的「眼在手外」一节做；
-但这不影响 S04 跟踪链路，两条链路用的是同一套权重、同一个跟踪目标 k0。
+产物是 `handeye/results/<臂号>_d435_eye_to_hand.yaml`。没有那份文件时 S03
 会明确报缺文件并回 S04 跟踪，不做假装成功的反投影。这不影响 S04 跟踪链路。
 ## 已知的坑（迁移时必须保留的处理）
@@ -5,8 +5,8 @@
 #    （实测见过 243mm / 254mm 两种）。写成固定值迟早会算错悬停高度。
 #    真机没有这个文件（现场装夹后把实测值写进 site.yaml）。
 flange_z_m: 0.763
-measured_at: '2026-09-24T19:40:08'
+measured_at: '2026-09-28T14:06:51'
 model: gauge
-tip_z_m: 0.5198
+tip_z_m: 0.5197
-tool_tip_below_flange_m: 0.2432
+tool_tip_below_flange_m: 0.2433
 world: assembly
@@ -0,0 +1,317 @@
 # 环境安装（按本机快照复现）
 本文是 **2026-10-09** 在这台工控机上核对过的软件栈，用来在另一台电脑上装出同一套能跑本仓库的环境。
 核对来源：`/etc/os-release`、`conda-meta/history`、`conda list`、`~/flexiv_install` 的 cmake 版本文件、`nvidia-smi`。
 日常怎么用已经装好的环境，看 [QUICKSTART.md](QUICKSTART.md)。本文只回答「从一台空的 Ubuntu 装到能 `source scripts/env.sh`」。
 ---
 ## 1. 本机现在是什么
 | 项 | 本机实测 |
 |----|----------|
 | 系统 | Ubuntu 22.04.5 LTS，x86_64，内核 `6.8.0-138-generic` |
 | GPU | NVIDIA GeForce RTX 3090 24GB，驱动 **595.91.07**（DKMS）。`nvidia-smi` 报告的 CUDA 是驱动上限 **13.2** |
 | 推理用的 CUDA | **不装系统 CUDA toolkit**。PyTorch / TensorRT 用 pip wheel 自带的 **CUDA 12.4** 用户态库 |
 | ROS | **Humble**。本机 **没有** `/opt/ros/humble`，ROS 在 conda 里（RoboStack） |
 | Conda | Anaconda3 装在 `~/anaconda3`，conda **25.11.1** |
 | 真机环境 | conda 环境 `ros_flexiv`，Python **3.11.15**，约 15GB |
 | 仿真环境 | conda 环境 `ros_gz_sim`，Python **3.11.6**，Gazebo Sim **6.16.0**（Fortress / `ign gazebo`），约 5.1GB |
 | 工作区 | 仓库在 `~/ros2`（ASCII）。`~/文档/ros2` 只是指向它的符号链接 |
 | Flexiv C++ | `~/flexiv_install`，`flexiv_rdk` **1.9.1**（spdlog 1.9.2，Boost 1.83.0），约 194MB |
 | Flexiv Python | `~/rdk_py310/bin/python` → 系统 `/usr/bin/python3.10`（3.10.12）+ `flexivrdk==1.9.1` + `numpy==1.26.4` |
 | DDS 域 | 真机 `ROS_DOMAIN_ID=0`，仿真强制 `77` |
 真机和仿真是两套 conda、两套 colcon 产物，互不覆盖：
 | | 真机 | 仿真 |
 |--|------|------|
 | 激活 | `source ~/ros2/scripts/env.sh` | `source ~/ros2/scripts/sim_env.sh` |
 | conda | `ros_flexiv` | `ros_gz_sim` |
 | 产物 | `build/` `install/` | `build_sim/` `install_sim/` |
 | 域 | 0 | 77 |
 同一只 shell 里来回 `source` 会残留 `PATH` / `PYTHONPATH`。换环境时开新终端。
 ---
 ## 2. 新机器先对齐的路径
 下面这些路径被脚本写死为用户 `cc` 的家目录。新机器用**同一个用户名** `cc`（家目录 `/home/cc`）时，后面的命令可以原样复制。
 | 路径 | 谁在用 |
 |------|--------|
 | `/home/cc/anaconda3` | `scripts/run_real.sh`、`scripts/run_mock_demo.sh` 直接 `source` 这个 `conda.sh`。`scripts/env.sh` 用的是 `$HOME/anaconda3`，两边要是同一个目录 |
 | `/home/cc/rdk_py310/bin/python` | `run_real.sh`、`test_rdk_python.sh`、`rdk_switch_tool.sh`、`scripts/lib/real_preflight.py` 的默认 `RDK_PYTHON` |
 | `/home/cc/flexiv_install` | `run_real.sh` 用 `$HOME/flexiv_install`（随用户走） |
 | `/home/cc/ros2` | 工作区。`colcon build` 必须在这条 ASCII 路径下，不要在中文目录里编 |
 用户名不是 `cc` 时，至少要改 `scripts/run_real.sh` 里的 conda 路径，并在启动前 `export RDK_PYTHON=$HOME/rdk_py310/bin/python`。否则真机入口会去找 `/home/cc`。
 建议磁盘空闲 **40GB 以上**（两个 conda 环境加下载缓存）。
 ---
 ## 3. 系统包与显卡
 ```bash
 sudo apt update
 sudo apt install -y \
  build-essential git curl wget \
  python3.10 python3-pip \
  v4l-utils usbutils \
  dkms linux-headers-$(uname -r)
 sudo usermod -aG dialout "$USER"
 ```
 `dialout` 给夹爪的 USB–RS485（`/dev/ttyUSB*`）。加完组要重新登录。
 显卡驱动装到能让 `nvidia-smi` 跑起来即可。本机是 **595.91.07**。驱动只要够跑 CUDA 12.4（远低于 595 的驱动也可以），PyTorch 的 12.4 wheel 就能用。不要再装一套系统级 CUDA toolkit，避免和 wheel 里的 `libcudart` 抢。
 Orisys 视触觉相机会被 USB 自动休眠卡死。本机规则文件是 `/etc/udev/rules.d/99-orisys-uvc.rules`：
 ```
 # Orisys 视触觉相机 (1bcf:2281)：禁止 USB 自动休眠（autosuspend 会把 UVC 相机搞到内核态卡死）
 ACTION=="add|bind", SUBSYSTEM=="usb", ATTR{idVendor}=="1bcf", ATTR{idProduct}=="2281", ATTR{power/control}="on", ATTR{power/autosuspend}="-1"
 ```
 写好后 `sudo udevadm control --reload-rules && sudo udevadm trigger`。
 ---
 ## 4. Anaconda
 装到 **`$HOME/anaconda3`**（安装器里选 Just Me、路径填这个）。不要装成 `~/miniconda3` 或其他目录：`run_real.sh` 写死了 `/home/cc/anaconda3/etc/profile.d/conda.sh`。
 本机 conda 版本是 25.11.1。装完新开一个终端：
 ```bash
 source "$HOME/anaconda3/etc/profile.d/conda.sh"
 conda --version
 ```
 频道不要写进 `~/.condarc` 当成全局默认。本机 `~/.condarc` 只有 `defaults`，RoboStack 的频道是**每次安装命令显式带上**的：`-c robostack-staging -c conda-forge`。
 ---
 ## 5. 真机环境 `ros_flexiv`
 下面三条 `conda` 命令来自本机 `~/anaconda3/envs/ros_flexiv/conda-meta/history`（2026-08-21 创建，2026-08-25 装 ROS）。pip 那一段是按同日 `conda list` 里的 pypi 版本还原的；当初的 `pip install` 原文没有留在 conda 历史里。
 ```bash
 source "$HOME/anaconda3/etc/profile.d/conda.sh"
 conda create -n ros_flexiv python=3.11 pip -y
 conda install -y -n ros_flexiv -c robostack-staging -c conda-forge \
  ros-humble-ros-base \
  ros-humble-robot-state-publisher \
  ros-humble-xacro \
  ros-humble-tf2-ros \
  ros-humble-joint-state-publisher \
  compilers cmake make pkg-config catkin_pkg empy lark-parser numpy \
  colcon-common-extensions
 conda install -y -n ros_flexiv -c robostack-staging -c conda-forge \
  ros-humble-rviz2
 conda install -y -n ros_flexiv -c robostack-staging -c conda-forge \
  ros-humble-moveit \
  ros-humble-moveit-configs-utils \
  ros-humble-control-msgs \
  ros-humble-controller-manager \
  ros-humble-joint-trajectory-controller \
  ros-humble-ros2-control \
  ros-humble-ros2-controllers
 ```
 然后钉住本机正在用的 pip 包。Humble 的 `rosidl` 要 **empy 3.x**（conda 有时会给 4.x，构建消息包时会报 `AttributeError: NoneType object has no attribute shutdown`）。TensorRT 装 `tensorrt-cu12`，不要装元包 `tensorrt`。
 ```bash
 conda activate ros_flexiv
 python -m pip install \
  'empy==3.3.4' \
  'lark==1.3.1' \
  'numpy==2.4.6' \
  'opencv-python-headless==4.10.0.84' \
  'pyrealsense2==2.58.3.10794' \
  'pyserial==3.5' \
  'minimalmodbus==2.1.1' \
  'onnx==1.22.0' \
  'onnxruntime==1.30.0' \
  'onnxruntime-gpu==1.29.0' \
  'tensorrt-cu12==10.13.3.9' \
  'ultralytics==8.4.123'
 python -m pip install \
  'torch==2.6.0+cu124' \
  'torchvision==0.21.0+cu124' \
  --index-url https://download.pytorch.org/whl/cu124
 ```
 相机走 **`pyrealsense2`**，本机没有装 `ros-humble-realsense2-camera`。夹爪 Modbus 走 `pyserial` + `minimalmodbus`。
 Orisys SDK 不在 git 里。把本机这个 wheel 拷过去再装（`--no-deps`，避免它改写上面的 torch / numpy）：
 ```bash
 # 源文件：~/sensor_ws/0.4.2/orisys-0.4.2-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_28_x86_64.whl
 conda activate ros_flexiv
 python -m pip install --no-deps \
  "$HOME/sensor_ws/0.4.2/orisys-0.4.2-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_28_x86_64.whl"
 ```
 本机这个环境里还有 Jupyter、PySide6、`flexiv-assembly` 等包，**本仓库脚本不用它们**，新机器不必装。
 激活后应看到：
 ```bash
 conda activate ros_flexiv
 python -c 'import sys; print(sys.version)'          # 3.11.x
 echo "ROS_DISTRO=$ROS_DISTRO"                        # humble
 python -c 'import rclpy, cv2, pyrealsense2, torch; import tensorrt as trt; print("torch", torch.__version__); print("tensorrt", trt.__version__); print("cuda", torch.cuda.is_available())'
 ```
 本机对应输出：`torch 2.6.0+cu124`，`tensorrt 10.13.3.9`，`torch.cuda.is_available()` 为真。
 ---
 ## 6. Flexiv RDK（与 conda 分开）
 运行中的库是 **1.9.1**。仓库里的 `src/flexiv_rdk` 工程版本是 **2.1**（`project(flexiv_rdk VERSION 2.1 ...)`）。`scripts/build_flexiv_rdk.sh` 编的是这份当前源码，产物和本机 `~/flexiv_install` 不是同一个版本。
 新机器要对齐**正在跑的**这套，把本机这两个目录原样拷过去：
 ```bash
 # 在新机器上，从本机拉（把 user@old-host 换成这台工控机）
 rsync -a user@old-host:~/flexiv_install/  ~/flexiv_install/
 rsync -a user@old-host:~/rdk_py310/       ~/rdk_py310/
 ```
 拷完核对：
 ```bash
 grep 'set(PACKAGE_VERSION' ~/flexiv_install/lib/cmake/flexiv_rdk/flexiv_rdk-config-version.cmake
 # 应有 set(PACKAGE_VERSION "1.9.1")
 ~/rdk_py310/bin/python -c 'import flexivrdk; print(flexivrdk.__version__)'
 # 1.9.1
 ```
 `~/rdk_py310/bin/python` 是一个壳：清掉 `PYTHONHOME`（避免 conda 污染），再用系统 Python 3.10 加上自己的 `site-packages`。`flexivrdk` 1.9.1 的 ABI 是 cp310，**不要**装进 `ros_flexiv`（那是 Python 3.11）。
 壳脚本内容（拷目录时已经带上；若要手写，注意把路径改成新机器的家目录）：
 ```bash
 #!/bin/bash
 export PYTHONPATH="${HOME}/rdk_py310/lib/python3.10/site-packages${PYTHONPATH:+:$PYTHONPATH}"
 unset PYTHONHOME
 exec /usr/bin/python3.10 "$@"
 ```
 ---
 ## 7. 仿真环境 `ros_gz_sim`
 用仓库脚本，可重复执行：
 ```bash
 bash ~/ros2/scripts/install_sim_env.sh
 bash ~/ros2/scripts/install_sim_env.sh --verify
 ```
 脚本会创建 `ros_gz_sim`（Python 3.11），从 `robostack-staging` + `conda-forge` 安装 `ros-humble-ros-base`、`ros-humble-ros-gz`、`ros-humble-gz-ros2-control`、`ros2_control` / `ros2_controllers`、`controller_manager`、`xacro`、`tf2_ros`、`camera-info-manager`、`image-transport-plugins`，并把 **empy 钉在 3.3.4**。
 本机 `--verify` 能通过时的关键版本：
 | 包 | 版本 |
 |----|------|
 | Gazebo Sim（`ign gazebo --version`） | 6.16.0 |
 | `libignition-gazebo6` | 6.16.0 |
 | `ros-humble-ros-gz` | 0.244.20 |
 | `ros-humble-gz-ros2-control` | 0.7.15 |
 | `empy` | 3.3.4（conda-forge） |
 | `numpy` | 1.26.4（conda-forge） |
 Humble 配的是 Gazebo Fortress，命令是 `ign gazebo`，不是 `gz sim`。
 仿真环境里**没有**真机那套 `torch` / `tensorrt` / `pyrealsense2` / `onnx`。本机仿真环境额外有一个 pip 的 `onnxruntime==1.30.0`。缺 `onnx` 时部分单测会红，这是现况；不要把真机的 pip 依赖灌进 `ros_gz_sim`，两个环境是故意隔开的。
 ---
 ## 8. 工作区、现场配置、编译
 ```bash
 git clone <本仓库> ~/ros2
 cd ~/ros2
 # 若习惯从中文路径进： ln -s ~/ros2 ~/文档/ros2
 ```
 `config/site.yaml`、YOLO 权重（`.pt` / `.onnx` / `.engine`）不在 git 里。
 ```bash
 cp ~/ros2/config/site.example.yaml ~/ros2/config/site.yaml
 # 按新机器填写 robot.sn、robots[].ip、相机序列号、权重路径
 # 权重放到 models/<分组>/ ，见 models/README.md
 ```
 TensorRT 的 `.engine` 跟 GPU 绑定。换了显卡（哪怕仍是 3090 以外的型号）要在新机器的 `ros_flexiv` 里重新 `bash ~/ros2/scripts/weights.sh convert --to engine`。直接拷本机的 `.engine` 经常加载失败。
 手眼结果 `handeye/results/` 在 git 里，但是**这台机器、这个相机安装位**的矩阵。新机器克隆后要重新标定，见 [HANDEYE.md](HANDEYE.md)。
 在 **`ros_flexiv`** 里编真机（先确认 `conda info --envs` 里星号在 `ros_flexiv`，python 指向 `~/anaconda3/envs/ros_flexiv/bin/python`）：
 ```bash
 cd ~/ros2
 conda activate ros_flexiv
 bash scripts/build_all.sh
 source scripts/env.sh
 echo "ROS_DISTRO=$ROS_DISTRO ROS_DOMAIN_ID=$ROS_DOMAIN_ID"
 # humble 与 0
 ```
 编完查 shebang 没有被仿真环境污染（正常应是 0）：
 ```bash
 grep -l ros_gz_sim install/*/lib/*/* | wc -l
 ```
 仿真另开一个终端：
 ```bash
 cd ~/ros2
 bash scripts/build_sim.sh
 source scripts/sim_env.sh
 # 打印里应有 CONDA=ros_gz_sim  ROS_DISTRO=humble  ROS_DOMAIN_ID=77
 ```
 ---
 ## 9. 装完怎么确认
 ```bash
 # 真机环境
 source ~/ros2/scripts/env.sh
 python -c 'import rclpy, torch, tensorrt, pyrealsense2, cv2, serial, minimalmodbus, onnx'
 echo "$ROS_DISTRO $ROS_DOMAIN_ID"          # humble 0
 ~/rdk_py310/bin/python -c 'import flexivrdk; print(flexivrdk.__version__)'   # 1.9.1
 # 仿真环境（新终端）
 bash ~/ros2/scripts/install_sim_env.sh --verify
 ```
 有机器人、示教器已切到 AUTO + Remote Ethernet、并且 `config/site.yaml` 已填好之后，再跑：
 ```bash
 bash ~/ros2/scripts/test_rdk_python.sh
 bash ~/ros2/scripts/run_real_preflight.sh --arm 89
 ```
 这两条会访问真机网络，空机器上失败不代表环境没装上。
 ---
 ## 10. 本机还有、但本仓库不用的 conda 环境
 `conda env list` 里还有 `flexiv`、`flexiv_rdk_py312`、`orisys`、`py311`、`ros2`。`scripts/env.sh` 和 `scripts/sim_env.sh` 都不会激活它们。新机器只建 `ros_flexiv` 和 `ros_gz_sim` 即可。
@@ -68,6 +68,21 @@ bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，
 bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）
 ```
 一阶段默认 **D405 主跟踪**：画面里没有 conf≥`perception.wrist_handoff_conf`（0.5）的目标，
 马上用顶视 D435 **拍一张** 把工件 / k0 定到基座系（眼在手外 T，不再乘法兰），把臂迁过去
 让 D405 重新看见。D405 跟住后，左臂法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过
 `lock_still_move_m`（15 mm）→ `/visual_servo/locked` → 二阶段对孔。
 ```bash
 bash ~/ros2/scripts/run_real.sh --arm 89
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
 bash ~/ros2/scripts/run_camera_d435.sh --preview            # 混合跟踪必须两台都开
 bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89         # 先只看框
 bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
 ```
 `track_camera: overhead` 仍是整段只用 D435 的实验开关。非法值启动即报错。
 细节、参数、退出码见 [`HOLE_DETECT.md`](HOLE_DETECT.md)。
 ### 1.4 现在到哪一步了（如实标注，别信印象）
@@ -78,7 +93,7 @@ bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（
 | 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
 | Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
 | Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
-| D435 手外相机（S03 粗定位兜底） | ❌ **还没做手眼标定**，链路不可用 | 见 `devices/cameras/README.md` |
+| D435 手外相机（一阶段一次性 + S03） | ⚠️ **89 已标定**；一阶段 D405 看不见时拍一张定 K0（混合跟踪）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
 | 自动启动全部服务（一条命令拉起所有进程） | ❌ 还没做，目前靠三个终端手工起 | 见第 6 节待办 |
 | 「规有没有被夹住」自检 | ❌ 还没做 | 见第 6 节待办 |
 | 装配合格判定（通/止） | ⚠️ **通止规装配流程**能按力矩判据拧到底/拧出来，但"通/止合格"结论还没做 | `run_gauge_assembly.sh --arm 89 --dry-run` |
@@ -357,7 +372,7 @@ bash scripts/weights.sh doctor --check-engines
 ### 4.3 单元测试怎么跑
-**每个功能包都有自己的测试**，改完对应包就跑一下。当前 pytest 收集 **1128 项**
+**每个功能包都有自己的测试**，改完对应包就跑一下。当前 pytest 收集 **1301 项**
 （另有 1 项按环境 skip）。
 ```bash
@@ -486,7 +501,7 @@ bash scripts/run_unit_tests.sh -k gauge              # 只跑名字带 gauge 的
   且顺序错了会出"控制器没起来但脚本继续跑"这类问题。
 2. **夹爪持规自检** —— 启动后先用触觉判断"规真的被夹着"，没有就报错停机
   （现在没有这一步，规掉了会一路跑到撞工件才发现）。
-3. **D435 手眼标定** —— 标完 S03 粗定位链路才可用（工件不在 D405 视野里时靠它先粗居中）。
+3. **D435 手眼标定（91）** —— 89 已写入 `handeye/results/89_d435_eye_to_hand.yaml`（18 视角）。91 还没标。S03 粗定位要用这份 TF 在真机上对一次工件才算链路验过。
 4. **装配合格判定** —— 把力曲线转成"通/止"结论。当前只输出原始几何与力，
   判定逻辑（含螺纹旋入圈数）还没写。
@@ -1,50 +1,82 @@
-# 手眼标定（眼在手上）
+# 手眼标定
-每台机械臂一份结果，**禁止混用**：
+两种装法，两块板，两台相机，两份结果。**禁止混用**：
-| 臂 | 结果 | 示教关节 | 工作目录 |
+| 模式 | 相机 | 板 | 板怎么摆 | 结果文件 | `T_4x4` 含义 |
-|----|------|----------|----------|
+|------|------|----|----------|----------|--------------|
-| 89 左 | `handeye/results/89_eye_in_hand.yaml` | `handeye/poses/89_eye_in_hand.yaml` | `handeye/tmp/89/` |
+| `eye_in_hand` | 腕部 D405 | A5，内角点 8×11，15 mm | 板放台面不动 | `handeye/results/<臂号>_eye_in_hand.yaml` | 相机 → 法兰 |
-| 91 右 | `handeye/results/91_eye_in_hand.yaml` | `handeye/poses/91_eye_in_hand.yaml` | `handeye/tmp/91/` |
+| `eye_to_hand` | 顶置 D435 | A4，内角点 11×8，20 mm | **板装到法兰上** | `handeye/results/<臂号>_d435_eye_to_hand.yaml` | 相机 → 这条臂的基座 |
-文件名统一为 `<臂号>_<模式>.yaml`（模式 = `eye_in_hand` | `eye_to_hand`），
+| 臂 | 眼在手上结果 | 眼在手外结果 | 示教关节 | 工作目录 |
-全部集中在仓库根 `handeye/`（`poses/` 输入、`results/` 输出、`tmp/` 临时产物）。
+|----|--------------|--------------|----------|----------|
-目录结构、板尺寸三处一致性与全部选项见 `handeye/README.md`。
+| 89 左 | `89_eye_in_hand.yaml` | `89_d435_eye_to_hand.yaml` | `poses/89_<模式>.yaml` | `tmp/89/` |
 | 91 右 | `91_eye_in_hand.yaml` | `91_d435_eye_to_hand.yaml` | `poses/91_<模式>.yaml` | `tmp/91/` |
-`T_4x4` = **相机 → 法兰**。跟踪和静态 TF 都读当前 `--arm` 对应的文件。
+目录结构、按键、稳定性参数见 `handeye/README.md`。棋盘尺寸只改
 `config/site.yaml` → `handeye.boards.<模式>`（内角点 = 方格数 − 1）。
 没配过的模式会被 `run_handeye.sh` 当场拦下，不会静默用另一块板的尺寸。
-棋盘尺寸只改 `site.yaml` → `handeye.board_*` / `square_size_m`（内角点 = 方格数 − 1）。两臂可共用同一块板。
+## 眼在手上（腕部 D405 + A5 板）
 ## 流程
 `teach`（RDK 软拖拽）与 `run_real.sh` **互斥**。全程带同一 `--arm`。
 `--mode eye_in_hand|eye_to_hand` 选模式（读写对应 `<臂号>_<模式>.yaml`，两种模式互不覆盖）；
 `teach --points N` 设定目标点数；`teach --ros --auto-solve` 采完自动 merge 并接着标定。
 ```bash
 # 1) 停掉 run_real.sh 和视觉伺服
 # 2) 示教 8–12 个姿态（棋盘清晰、占画面 30–70%，姿态拉开）
 bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
-#    窗口：d=拖拽  Enter=拍照  q=结束
+#    窗口：d=拖拽  Enter=拍照  q=结束（窗口只显示画面，不做棋盘检测）
 #    接着采、不清空旧点：  teach --append
-# 3) 写入该臂 handeye/poses/<臂号>_<模式>.yaml
+# 3) 写入该臂 handeye/poses/89_eye_in_hand.yaml
 bash ~/ros2/scripts/run_handeye.sh --arm 89 merge
-# 4) 再开实机 + 相机，棋盘不要动
+# 4) 再开实机 + 腕部相机，棋盘不要动
 bash ~/ros2/scripts/run_real.sh --arm 89
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
 bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate
 ```
-91 把 `--arm 89` 改成 `--arm 91`。91 的结果文件初始是 `uncalibrated: true`，标定成功后才会写出 `T_4x4`。
+91 把 `--arm 89` 改成 `--arm 91`。
-查看路径与板参数：`bash ~/ros2/scripts/run_handeye.sh --arm 89 show`
+## 眼在手外（顶置 D435 + A4 板）
 物理摆法与上面**反过来**：相机固定，板跟着臂动。板若还摆在台面上，
 `target→camera` 不变，方程退化（解不出来或解出垃圾）。
 ```bash
 # 1) 把 A4 板刚性固定在法兰 / 夹爪上（夹在正中间，标定过程不要松）
 # 2) 起臂 + 顶置相机（两个终端，常驻）
 bash ~/ros2/scripts/run_real.sh --arm 89
 bash ~/ros2/scripts/run_camera_d435.sh --preview
 # 3) 采点：推着臂走，让板在 D435 画面里出现在不同位置、不同角度
 #    teach 与 run_real.sh 互斥 —— 用 RDK 拖拽前先停掉 run_real.sh
 bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
 # 4) 合并 + 自动走点求解（此时再开 run_real.sh + D435）
 bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
 bash ~/ros2/scripts/run_real.sh --arm 89
 bash ~/ros2/scripts/run_camera_d435.sh --preview
 bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand
 ```
 查看当前模式选中的板 / 相机 / 文件：
 ```bash
 bash ~/ros2/scripts/run_handeye.sh --arm 89 show --mode eye_to_hand
 ```
 应看到 `camera=d435`、`board 11x8 inner, square=0.02 m`、
 结果路径 `handeye/results/89_d435_eye_to_hand.yaml`。不是这三样就停，
 不要带着错配置开标定。
 ## 结果怎么用
-`visual_servo.launch.py` 读当前臂 `T_4x4`，发布 `{ee_frame}` → `{camera_frame}` 的 static TF。
+- **眼在手上**：`visual_servo.launch.py` / `stage2_locate.launch.py` /
-
+  `gauge_assembly.launch.py` 读 `paths.handeye_file`，发布
-`servo_node` 用同一份 T 做 `hover` / `lookat`。换结果后**重启** `run_visual_servo_rt.sh --arm …`。
+  `{法兰}` → `{腕部相机}_color_optical_frame`。`servo_node` 用同一份 T
-
+  做 `hover` / `lookat`。换结果后**重启**对应节点。
-装配 HMI（`ros_flexiv`）选左/右臂后，读同一份 `handeye/results/89_eye_in_hand.yaml` / `91_eye_in_hand.yaml`。
+- **眼在手外**：`gauge_assembly.launch.py` 按
  `handeye/results/<臂号>_<相机>_eye_to_hand.yaml` 自己找，发布
  `{基座}` → `{手外相机}_color_optical_frame`。S03 粗定位查的就是这条 TF。
  **不要**改 `paths.handeye_file` 去指向它 —— 那会把腕部精拍定位的 TF 换掉。
@@ -99,15 +99,26 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
 ## 二阶段一次性拍照定位（stage2，与一阶段跟踪分开跑）
 先悬停垂直朝下 → 拍一张照 → `detect_best_1.onnx` 框工件 → Hough 解 k1/k2 孔心 →
-深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动夹爪/通止规轴线到目标孔正上方 →
+深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动**规尖**到目标孔正上方 →
 复拍验证（≤`align_max_iters` 次，收敛判据 `align_tol_m`）→ 锁定保持。
 > **对的是"规尖"，不是"法兰轴线"**（2026-10-08 定稿）。通止规夹在爪里，相对法兰轴线
 > 可以整体偏出若干毫米；旧写法把**法兰**对到孔上，规尖就偏着压在孔沿上，轻降一压
 > 腕力蹦到 63N。现在每一帧同时解「规尖基座 XY」和「孔基座 XY」——两者走同一条手眼
 > 链，链路误差与装夹变动一起抵消。规尖偏移是**刚体量**，量准一份（`tip_calib_n` 拍
 > 一致）就冻结。
 > 真机验证（arm 89，独立重测那一帧）：规尖到 k1 **0.34mm**、到 k2 61.15mm、
 > 反投影工具长 222mm（与 site 的 223mm 一致），偏移冻结值 (2.6, −1.0)mm。
 > 相关开关在 `site.yaml → hole_detect.stage2:`（`align_tip_vision` / `tip_*`）。
 > 量不到规尖偏移时二阶段**判失败**（exit 8），不会拿"法兰对上了"冒充成功。
 ```bash
 # 前置：T1 run_real.sh --arm 89；T3 run_camera_d405.sh --arm 89 --preview
 bash ~/ros2/scripts/run_stage2_locate.sh --arm 89                # 完整流程（自动拉起触觉服务，仅左臂）
 bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only   # 只垂直+悬停锁定，用来调高度
 bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2      # 对准 k2（默认 k1）
 bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高，不动 yaml
 bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent             # 只验证精对准、不轻降
 ```
 悬停锁高接口：`site.yaml → robots[89].servo.hole_hover_z_m`（91 在同名段）。
@@ -4,7 +4,9 @@
 ## 依赖
- Ubuntu 22.04 + ROS 2 Humble
+从一台空机器装到和本机一样：见 **[ENVIRONMENT.md](ENVIRONMENT.md)**（2026-10-09 快照：RoboStack Humble、`ros_flexiv` / `ros_gz_sim`、RDK 1.9.1）。
 - Ubuntu 22.04 + ROS 2 Humble（本机 ROS 在 conda 里，没有 `/opt/ros`）
 - Conda 环境 `ros_flexiv`（本仓库脚本默认这个名字）
 - Flexiv RDK Python：`/home/cc/rdk_py310/bin/python`（flexivrdk 1.9.x）
 - 工控机与机器人在同一网段（例：`192.168.2.10` ↔ `192.168.2.101`）
@@ -84,7 +84,7 @@ devices/<kind>/<brand>/
 | `run_camera_d405.sh --arm … --preview` | 该臂腕部 D405。89→`/d405_left/`，91→`/d405_right/` |
 | `run_camera_d435.sh --preview` | 顶视 D435（手外，可选） |
 | `list_realsense.sh` | 列出已接 RealSense 序列号 |
-| `run_visual_servo_rt.sh --arm … [--control]` | YOLO + 伺服。无 `--control` 只看框；有则动臂。从当前姿态开始，不回 home |
+| `run_visual_servo_rt.sh --arm … [--control]` | YOLO + 伺服。无 `--control` 只看框；有则动臂。`wrist`=D405 主跟踪，看不见则 D435 一次性定 K0；`overhead`=整段顶视 |
 | `go_home.sh --arm …` | 回 named pose `home`（会先停伺服） |
 | `show_site_config.sh` | 打印当前臂解析出的路径/锁高/相机 |
@@ -183,6 +183,7 @@ devices/<kind>/<brand>/
 | `FRAMEWORK_RULES.md` | **框架规则**（硬规矩，改代码前先读；第 8 节是按症状查的反例表） |
 | `WORKSPACE.md` | 本页，目录与职责 |
 | `QUICKSTART.md` | 环境、编译、第一次实机 |
 | `ENVIRONMENT.md` | **按本机快照在另一台电脑上装同一套环境**（conda / RDK / 显卡 / 路径） |
 | `ARCHITECTURE.md` | 进程与数据流 |
 | `SITE_CONFIG.md` | `site.yaml` 字段 |
 | `HANDEYE.md` | 示教 / merge / 标定 |
@@ -16,7 +16,7 @@
 | 模式 | 相机装在哪 | 解出的 X 是什么 | 本项目的用法 |
 |------|-----------|----------------|-------------|
 | `eye_in_hand` | **相机装在机械臂末端（法兰/手腕）上** | 相机 → 法兰 | **D405 腕部相机（当前使用）** |
-| `eye_to_hand` | **相机固定在旁边不动** | 相机 → 机械臂基座 | D435 顶置相机（预留） |
+| `eye_to_hand` | **相机固定在旁边不动** | 相机 → 机械臂基座 | D435 顶置相机 |
 **怎么选**：看相机是不是跟着机械臂动。
 - 跟着动 → `eye_in_hand`
@@ -50,40 +50,36 @@ handeye/
   `handeye_workdir`。两条臂若共用一个工作目录，先标定的那份采点会被后
   标定的覆盖，而症状是"标定结果突然变得很差"，不容易联想到是目录冲突。
-**命名约定**：`<臂号>_<模式>.yaml`
+**命名约定**：
- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`，`Rizon4-063991` → `91`）
+- 眼在手上：`<臂号>_eye_in_hand.yaml`（`paths.handeye_file` 指向它）
- 模式 = `eye_in_hand` | `eye_to_hand`
+- 眼在手外：`<臂号>_<相机>_eye_to_hand.yaml`（S03 按这个名字自己找，
-
+  不要改 `paths.handeye_file`）
-运行时谁读结果：`config/site.yaml` 里 `robots[89].paths.handeye_file` 指向
+- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`）
-`handeye/results/89_eye_in_hand.yaml`，再由各 launch 文件发布成静态 TF。
+- 示教点：`<臂号>_<模式>.yaml`（两种模式的关节角完全不同，分开存）
 **换模式 = 把 site.yaml 里那个路径改成 `_eye_to_hand` 那份**，不需要改代码。
 ---
 ## 3. 标定板（最容易出错的地方）
-当前使用的板：
+两种装法用**两块不同的物理板**，尺寸按模式分开写，不能共用一组键：
-```
+| 模式 | 物理板 | 内角点（列 × 行） | 方格边长 | `site.yaml` 里的位置 |
-内角点  8 列 × 11 行      (注意是"内角点"，不是方格数！)
+|------|--------|-------------------|----------|----------------------|
-方格边长 15 mm (0.015 m)
+| `eye_in_hand` | A5（腕部相机看台面） | 8 × 11 | 15 mm (`0.015`) | `handeye.boards.eye_in_hand` |
-```
+| `eye_to_hand` | A4（板装在法兰上） | 11 × 8 | 20 mm (`0.020`) | `handeye.boards.eye_to_hand` |
-**这个尺寸必须三处一致**，否则标定结果会系统性偏错：
+权威只有 `config/site.yaml` 的 `handeye.boards.<模式>`。`run_handeye.sh` 按
-
+`--mode` 取对应那一组，用 `-p` 传给节点。`handeye/calibrate.yaml` 和节点默认值
-| 位置 | 内容 |
+只是兜底（眼在手上那块板），**没配过的模式会被脚本当场拦下**，不会静默用兜底
-|------|------|
+尺寸去标（那会让结果系统性偏，还不报错）。
 | `config/site.yaml` → `handeye.board_cols/rows/square_size_m` | 8 / 11 / 0.015 |
 | `handeye/calibrate.yaml` | `board_cols/rows/square_size_m` |
 | `src/.../handeye_calibrate.py` 的参数默认值 | 代码兜底 |
 > ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、
 > `8×11/0.015`，谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错，
 > 但结果明显不对"，非常难查。现已统一为 `8×11/0.015`。
 > **换板子时，请把上面三处一起改。**
 怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**，
-是「方格数 − 1」。比如 A5 板上画了 9×12 个格子 → 内角点是 8×11。
+是「方格数 − 1」。A5 板上画了 9×12 个格子 → 内角点 8×11；
 A4 板上画了 12×9 个格子 → 内角点 11×8。列/行顺序不用纠结，检测端两种顺序都会试。
 > ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、
 > `8×11/0.015`，谁生效取决于覆盖顺序。后来又把两块板合成一组键，于是标眼在手外
 > 时静默用了 A5 的 15 mm。症状都是"标定流程跑得很顺、不报错，但结果明显不对"。
 ---
@@ -97,7 +93,8 @@ handeye/
 bash ~/ros2/scripts/run_handeye.sh teach --arm 89
 ```
-会弹出实时预览窗口，里面能看到标定板有没有被识别到。操作：
+会弹出实时预览窗口（**只显示画面，不做棋盘检测**——真正的角点稳定判定在
 `calibrate` 那一步）。操作：
 | 按键 | 作用 |
 |------|------|
@@ -190,20 +187,25 @@ bash ~/ros2/scripts/run_handeye.sh teach --ros --auto-solve --arm 89
 ### 切换 eye-in-hand / eye-to-hand
 ```bash
-# 用腕部相机（默认）
+# 用腕部相机 + A5 板（默认，板放台面不动）
 bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
-# 用固定相机
+# 用固定相机 + A4 板（板必须装到法兰上，相机不动）
-bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand
+# 前置：另开终端 bash ~/ros2/scripts/run_camera_d435.sh --preview
 bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
 bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
 bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand
 ```
-`--mode` 会把读写的文件切到 `<臂号>_<模式>.yaml`，所以：
+`--mode` 会同时切：标定板、相机、示教点文件、结果文件。所以：
 - 两种模式**互不覆盖**，可以分别标定、共存
- 跑完想切换使用，改 `config/site.yaml` 里 `robots[89].paths.handeye_file`
+- 眼在手上结果：`handeye/results/<臂号>_eye_in_hand.yaml`（`T` = 相机 → 法兰）
-  指向对应那份即可，代码不用动
+- 眼在手外结果：`handeye/results/<臂号>_<相机>_eye_to_hand.yaml`
  （`T` = 相机 → 这条臂的基座；双臂共用一台 D435 时必须按臂分开）
 - **不要**把 `paths.handeye_file` 改成眼在手外那份 —— 那条路径是腕部相机
  （S05 精拍定位）用的。手外那份由 `gauge_assembly.launch` 按名字自己找，
  并以 **基座 ← 相机** 发布 TF（不是法兰 ← 相机）
 传了非法模式会立刻报错并退出（不会悄悄退回默认值 —— 那会导致"以为标的是
 eye-to-hand、其实算的是 eye-in-hand"这种最难查的错）。
@@ -48,10 +48,10 @@ handeye_calibrate:
    camera_cy: 0.0
    camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0]
-    # 真实使用的标定板：内角点 8x11，方格 15mm。
+    # 板尺寸的权威在 site.yaml → handeye.boards.<模式>。
-    # 依据是 handeye/results/*.yaml 头部记录的实测板子，以及 site.yaml 的 handeye.board_*。
+    # 这里只写眼在手上那块板的兜底（A5 / 8×11 / 15mm）；run_handeye.sh 会按
-    # 这里原先写的是 11x16 / 0.011（另一张早期用过的板）—— 三处配置曾互相矛盾，
+    # --mode 用 -p 覆盖成对应模式的值。没配过的模式会被脚本当场拦下，
-    # 谁生效取决于覆盖顺序，因此存在"标定跑完但结果不对"的风险。
+    # 不会静默用这份兜底去标另一块板。
    board_cols: 8
    board_rows: 11
    square_size_m: 0.015
@@ -0,0 +1,50 @@
 # 眼在手外示教关节（7 关节 × N）。板装在法兰上，顶置 D435 看它。
 #
 # 由 teach → merge 写入，不要手抄眼在手上那份（两种模式的姿态完全不同）。
 #
 #   bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
 #   bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
 /**:
  ros__parameters:
    num_poses: 19
    waypoint_positions: [
      # pose 0
      -1.045969, -0.575935, 0.132913, 2.251024, -0.297265, 1.274437, -0.219817,
      # pose 1
      -1.221272, -0.582225, 0.269777, 2.060852, -0.180051, 1.157081, -0.070498,
      # pose 2
      -1.222672, -0.605722, 0.273858, 2.111021, -0.181634, 1.193047, -0.059758,
      # pose 3
      -1.219127, -0.576923, 0.281368, 2.310613, -0.162188, 1.339573, -0.026065,
      # pose 4
      -1.100853, -0.552692, 0.291246, 2.473326, -0.116403, 1.504852, 0.051283,
      # pose 5
      -1.022050, -0.394038, 0.375788, 2.311711, -0.503617, 1.217768, 1.268088,
      # pose 6
      -1.180556, -0.574370, 0.471095, 2.055400, -0.490664, 1.086456, 1.216527,
      # pose 7
      -2.039707, -0.492556, 1.462952, 2.222064, -0.329990, 0.209789, 0.523258,
      # pose 8
      -1.959748, -0.554596, 1.469405, 2.293419, -0.165008, 0.422761, 0.844081,
      # pose 9
      -1.666347, -0.747468, 1.236192, 2.176000, -0.303646, 0.773895, 1.279040,
      # pose 10
      -1.689100, -0.879982, 1.168013, 2.057620, -0.424417, 0.752234, 1.241290,
      # pose 11
      -1.691875, -0.889908, 1.154014, 2.062930, -0.549154, 0.850985, 1.257704,
      # pose 12
      -1.715420, -0.986765, 1.125361, 1.948461, -0.638012, 0.809355, 1.173290,
      # pose 13
      -1.729057, -1.004106, 1.126932, 1.961482, -0.727920, 0.900058, 1.188069,
      # pose 14
      -2.034166, -0.748356, 1.234076, 2.252718, -1.542249, 0.093972, 0.640260,
      # pose 15
      -2.155450, -0.769613, 1.249491, 2.275528, -1.648348, 0.073256, 0.495109,
      # pose 16
      -1.959788, -0.979802, 1.271261, 2.222882, -1.560991, 0.057204, 0.296625,
      # pose 17
      -1.933247, -0.398944, 1.211885, 2.312756, -0.424981, 0.358678, 0.347203,
      # pose 18
      -1.944530, -0.399220, 1.244635, 2.293245, -0.423596, 0.317991, 0.358565,
    ]
@@ -0,0 +1,13 @@
 # 手眼标定结果
 # mode: eye_to_hand
 # camera_source: d435
 # views: 18
 # utc: 2026-10-09T02:56:24.621687+00:00
 # board: 11x8 square_m=0.02
 # T_cam_to_base (eye_to_hand)
 translation_m: [0.37495201, -0.52156937, 0.72589194]
 T_4x4:
  - [-0.04575158, 0.99819969, -0.03878355, 0.37495201]
  - [0.99893621, 0.04594039, 0.00399073, -0.52156937]
  - [0.00576527, -0.03855971, -0.99923967, 0.72589194]
  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
@@ -0,0 +1,16 @@
 # 模型类别标签（class names）
 #
 # 一个模型对应一份标签，放在权重旁边、同名前缀：
 #   detect_best_0.engine  ← 权重
 #   detect_best_0.labels.yaml  ← 本文件
 #
 # 写法：names 下面一行一个类名，第 1 行对应类别 0，第 2 行对应类别 1，依次类推。
 # 改完保存即生效，不需要重新转换模型、也不用重启环境（重跑节点即可）。
 #
 # 命令行查看/修改（推荐，不用手改本文件）：
 #   bash ~/ros2/scripts/weights.sh labels <模型名>
 #   bash ~/ros2/scripts/weights.sh labels <模型名> --set "工件,通止规"
 model: gongjian0929
 names:
  - workpiece
@@ -0,0 +1,63 @@
 #!/usr/bin/env python3
 """确定性复现「工件被推走 ⇒ 规没进孔」：在 k1 之后把工件挪 25mm。
 为什么需要它：这个故障在自然运行里是**随机**的（实测三跑坏一跑），
 靠碰运气验证判据不可靠。这里改成主动制造：等流程走到 S10（离开 k1、去 k2），
 就把工件沿 +Y 推 25mm —— 而 k2 的孔位是 S05 一次性定下的，于是必然失配。
 判据（要看的就是这个）：S07 必须报「规没有进孔」（HOLE_LOST），
 **不是** 之前那句把人引去查工具系的「转轴偏差」。
 """
 from __future__ import annotations
 import pathlib
 import subprocess
 import sys
 import time
 ROOT = pathlib.Path(__file__).resolve().parents[1]
 sys.path.insert(0, str(ROOT / "src" / "arm_sim_gazebo"))
 LOG = pathlib.Path("/tmp/sim_guard.log")
 WORLD = "assembly"
 SHIFT_M = -0.020  # 与 2026-09-28 自然失效同向同量级（工件 y: 0.000 → -0.0206）
 def _shift() -> None:
    from arm_sim_gazebo.sim_pose_service import set_model_pose
    # 工件当前在 (0.520, 0.000, 0.400)，沿 +Y 挪 25mm（远超孔半径，必然错过）
    ok, out = set_model_pose(WORLD, "workpiece", (0.520, SHIFT_M, 0.400), (0.0, 0.0, 0.0, 1.0))
    print(f"[driver] 推工件 {SHIFT_M * 1000:.0f}mm -> {'成功' if ok else '失败'}: {out[:160]}")
 def main() -> int:
    LOG.write_text("")
    proc = subprocess.Popen(
        ["bash", "scripts/run_gauge_assembly.sh", "--arm", "sim", "--sim"],
        cwd=str(ROOT), stdout=LOG.open("w"), stderr=subprocess.STDOUT,
    )
    print(f"[driver] 流程已起 pid={proc.pid}，等它走到 S10…")
    shifted = False
    t0 = time.monotonic()
    while proc.poll() is None and time.monotonic() - t0 < 1500:
        txt = LOG.read_text(errors="replace")
        if not shifted and "S10_to_k2" in txt:
            print("[driver] 检测到 S10_to_k2 —— 现在推工件")
            _shift()
            shifted = True
        time.sleep(1.0)
    proc.wait(timeout=30)
    txt = LOG.read_text(errors="replace")
    print(f"[driver] 流程退出码 = {proc.returncode}, 是否推过工件 = {shifted}")
    for line in txt.splitlines():
        if "规没有进孔" in line or "孔位失效" in line or "转轴偏差" in line:
            print("  |", line.strip()[:150])
    return 0
 if __name__ == "__main__":
    raise SystemExit(main())
@@ -67,9 +67,11 @@ LITERAL_REPLACEMENTS: list[tuple[str, str]] = [
    # 用户名匿名化。放在其它规则之前，避免路径被二次处理。
    ("/home/cc", "/home/USER"),
    # 注释里裸写的机械臂编号（无引号，正则抓不到）。
    # ⚠️ 这行是**逐字匹配**：site.yaml 里改一个字它就静默失效，SN 就漏进模板了
    #（test_real_devices.test_example_contains_no_live_values 会挡下来）。
    (
-        "# User1=enp4s0  本机 192.168.2.10  ↔  063989=.101  /  063991=.100",
+        "#   本机 192.168.2.10  ↔  063989(左89)=.101  /  063991(右91)=.100",
-        "# User1=enp4s0  本机 <PC_IP>  ↔  <左臂SN>=.101  /  <右臂SN>=.100",
+        "#   本机 <PC_IP>  ↔  <左臂SN>(左89)=.101  /  <右臂SN>(右91)=.100",
    ),
 ]
@@ -77,6 +79,15 @@ LITERAL_REPLACEMENTS: list[tuple[str, str]] = [
 REGEX_REPLACEMENTS: list[tuple[str, str]] = [
    # 机械臂序列号：Rizon4-063989 -> Rizon4-XXXXXX
    (r'"(Rizon4)-\d+"', r'"\1-XXXXXX"'),
    # 同上，但**不带引号**（注释里的裸写）。留一条兜底，免得上面的逐字规则
    # 一旦和 site.yaml 不同步就把 SN 漏出去。
    (r"(Rizon4)-\d+", r"\1-XXXXXX"),
    # sn 里的连字符在下划线形式下（话题名不许含 '-'，broadcaster 用 '_'）：
    # 例 "/Rizon4_063989/external_wrench_in_tcp" -> "/Rizon4_XXXXXX/external_wrench_in_tcp"
    (r"(Rizon4)_\d+", r"\1_XXXXXX"),
    # USB 网卡的 iface 名是 MAC 派生的（enx + 12 位十六进制）= 现场值。
    # enp4s0 这类板载名是通用的，不动。
    (r"enx[0-9a-fA-F]{12}", "enxXXXXXXXXXXXX"),
    # IP 地址（仅带引号的配置值；注释里的裸 IP 由上面的字面替换处理）
    (r'"192\.168\.\d{1,3}\.\d{1,3}"', '"192.168.2.XXX"'),
    # 8 位及以上的纯数字字符串 = 设备序列号（相机 12 位 / 夹爪 8 位）。
@@ -272,7 +272,12 @@ def camera_ns(data: dict) -> str:
 def camera_frames(data: dict) -> tuple[str, str, str]:
-    """当前机械臂的（命名空间, 光学坐标系, 深度话题）。"""
+    """当前机械臂**腕部**相机的（命名空间, 光学坐标系, 深度话题）。
    一阶段跟踪用哪台相机见 ``track_camera_frames`` —— 不许改本函数去跟
    ``perception.track_camera``：``SITE_CAM_NAME`` / 二阶段 / 入孔检测都走这里，
    切跟踪相机不该把精拍那路也切走。
    """
    ns = camera_ns(data)
    yolo = data.get("yolo") or {}
    frame = str(yolo.get("camera_frame") or "").strip()
@@ -284,6 +289,218 @@ def camera_frames(data: dict) -> tuple[str, str, str]:
    return ns, frame, depth
 #: 顶视跟踪必须自己写的深度量程。缺了就借腕部 0.55m 上限 = D435 的点全丢。
 _OVERHEAD_RANGE_REQUIRED = ("min_depth_m", "max_depth_m", "feature_min_range_m")
 #: D405 主跟踪切 D435 一次性时必须显式写的键。缺了就静默不切 = 臂停在半空。
 _ONESHOT_REQUIRED = (
    "oneshot_conf",
    "cooldown_s",
    "frame_max_age_s",
    "oneshot_hold_s",
 )
 def track_camera_frames(data: dict) -> tuple[str, str, str, str]:
    """一阶段跟踪用的（命名空间, 光学坐标系, 深度话题, 手眼模式）。
    ``perception.track_camera``：
      * ``wrist`` → 腕部相机 + ``eye_in_hand``
      * ``overhead`` → 手外相机 + ``eye_to_hand``
    """
    data, robot = apply_active_arm(data)
    sio = _load_site_io()
    role = sio.track_camera_role(data)
    mode = sio.track_handeye_mode(role)
    if role == "overhead":
        over = overhead_camera(data, robot)
        ns = str(over.get("name") or "").strip("/")
        if not ns:
            raise RuntimeError(
                "perception.track_camera=overhead，但 cameras 里没有 "
                "role 以 overhead 开头的相机"
            )
        return ns, f"{ns}_color_optical_frame", f"/{ns}/aligned_depth_to_color/image_raw", mode
    ns, frame, depth = camera_frames(data)
    return ns, frame, depth, mode
 def _track_handeye_file(data: dict, robot: dict, mode: str) -> str:
    """跟踪用的手眼文件。overhead 不许去读 ``paths.handeye_file``（那是腕部的）。"""
    if mode != "eye_to_hand":
        return str((data.get("paths") or {}).get("handeye_file") or "")
    over = overhead_camera(data, robot)
    cam = str(over.get("name") or "").strip("/")
    short = arm_short_id(str(robot.get("sn") or ""))
    name = _load_site_io().eye_to_hand_result_name(short, cam)
    return str(_root() / "handeye" / "results" / name)
 def _apply_overhead_range(data: dict, params: dict) -> None:
    """把 ``perception.overhead`` 的深度量程盖到感知参数上。缺键就红。"""
    oh = (data.get("perception") or {}).get("overhead")
    if not isinstance(oh, dict):
        raise RuntimeError(
            "perception.track_camera=overhead 必须在 perception.overhead 写顶视"
            "深度量程（min_depth_m / max_depth_m / feature_min_range_m）。"
            "腕部那组 0.55m 上限会把 D435 的点全丢掉，看起来像检测失败。"
        )
    missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh]
    if missing:
        raise RuntimeError(
            f"perception.overhead 缺 {missing} —— 不许借腕部量程。"
        )
    params["min_depth_m"] = float(oh["min_depth_m"])
    params["max_depth_m"] = float(oh["max_depth_m"])
    params["feature_min_range_m"] = float(oh["feature_min_range_m"])
    if "work_z_max_m" in oh:
        params["work_z_max_m"] = float(oh["work_z_max_m"])
    if "z_ref_m" in oh:
        params["z_ref_m"] = float(oh["z_ref_m"])
    if "feature_mode" in oh:
        params["feature_mode"] = str(oh["feature_mode"])
    if "hold_point_on_miss" in oh:
        params["hold_point_on_miss"] = bool(oh["hold_point_on_miss"])
 def _apply_wrist_hybrid(data: dict, robot: dict, params: dict) -> None:
    """腕部主跟踪 + D435 一次性：参数接到感知节点，**不许盖掉腕部量程**。
    顶视量程写进 ``overhead_*`` 键。若直接复用 ``min_depth_m``，D405 会按
    0.25–1.80 m 收点，近处工件全丢，看起来像检测失败。
    """
    perc = data.get("perception") or {}
    if "wrist_handoff_conf" not in perc:
        raise RuntimeError(
            "perception.track_camera=wrist 的混合跟踪必须写 "
            "perception.wrist_handoff_conf（画面内最高 conf 低于此才切 D435）。"
            "不许在代码里写死 0.5 再抄一份到 yaml。"
        )
    params["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"])
    oh = perc.get("overhead")
    if not isinstance(oh, dict):
        raise RuntimeError(
            "wrist 混合跟踪必须在 perception.overhead 写顶视量程和一次性参数"
            "（min_depth_m / max_depth_m / feature_min_range_m / oneshot_conf / "
            "cooldown_s / frame_max_age_s / oneshot_hold_s）。"
        )
    missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh]
    if missing:
        raise RuntimeError(
            f"perception.overhead 缺 {missing} —— 不许借腕部量程做 D435 一次性。"
        )
    miss2 = [k for k in _ONESHOT_REQUIRED if k not in oh]
    if miss2:
        raise RuntimeError(f"perception.overhead 缺一次性键 {miss2} —— 不许用代码默认值。")
    over = overhead_camera(data, robot)
    ns = str(over.get("name") or "").strip("/")
    if not ns:
        raise RuntimeError(
            "wrist 混合跟踪需要 cameras 里 role 以 overhead 开头的相机"
        )
    e2h = _track_handeye_file(data, robot, "eye_to_hand")
    params["overhead_oneshot_enable"] = True
    params["overhead_min_depth_m"] = float(oh["min_depth_m"])
    params["overhead_max_depth_m"] = float(oh["max_depth_m"])
    params["overhead_feature_min_range_m"] = float(oh["feature_min_range_m"])
    params["overhead_oneshot_conf"] = float(oh["oneshot_conf"])
    params["overhead_cooldown_s"] = float(oh["cooldown_s"])
    params["overhead_frame_max_age_s"] = float(oh["frame_max_age_s"])
    params["overhead_hold_s"] = float(oh["oneshot_hold_s"])
    params["overhead_handeye_file"] = e2h
    params["overhead_camera_source"] = ns
    params["overhead_camera_frame"] = f"{ns}_color_optical_frame"
    params["overhead_image_topic"] = f"/{ns}/color/image_raw"
    params["overhead_info_topic"] = f"/{ns}/color/camera_info"
    params["overhead_depth_topic"] = f"/{ns}/aligned_depth_to_color/image_raw"
 #: 手眼标定的两种装法。写在这里当"合法值清单"，避免各处各写一遍字符串。
 HANDEYE_MODES = ("eye_in_hand", "eye_to_hand")
 def handeye_board(he: dict, mode: str) -> tuple[dict, str]:
    """取某个模式用的标定板尺寸 → ``({cols, rows, square_size_m}, 来源)``。
    权威只有一处：``handeye.boards.<mode>``。为什么按模式分开——两种装法用的是
    **两块不同的物理板**（本项目：眼在手上 A5 / 眼在手外 A4），方格边长不一样。
    合成一组键的后果不是报错，而是"跑另一种模式时静默用错边长"，结果只是系统性偏，
    现场几乎联想不到板尺寸。
    兼容：没有 ``boards`` 段时，回落到旧的顶层 ``board_cols/rows/square_size_m``。
    返回的第二个值是来源（``boards`` / ``legacy`` / ``default``），调用方据此决定
    要不要拦下——``default`` 意味着**没人配过这块板**，此时开标定等于赌一个尺寸。
    """
    boards = he.get("boards") if isinstance(he.get("boards"), dict) else {}
    node = boards.get(mode) if isinstance(boards, dict) else None
    src = "boards"
    if not isinstance(node, dict):
        node = he
        src = "legacy"
    if mode not in HANDEYE_MODES:
        # 不认识的模式：不猜板子，交给调用方报错（猜错就是静默用错尺寸）。
        node, src = {}, "default"
    def pick(key: str, cast, default):
        v = node.get(key) if isinstance(node, dict) else None
        if v is None:
            return default, False
        try:
            return cast(v), True
        except (TypeError, ValueError):
            return default, False
    # boards 段用 cols/rows；旧的顶层键是 board_cols/board_rows。两种都认，
    # 免得"兼容旧配置"其实读不到旧键，悄悄退回默认 8×11。
    cols, ok_c = pick("cols", int, 8)
    if not ok_c:
        cols, ok_c = pick("board_cols", int, 8)
    rows, ok_r = pick("rows", int, 11)
    if not ok_r:
        rows, ok_r = pick("board_rows", int, 11)
    sq, ok_s = pick("square_size_m", float, 0.015)
    if src == "legacy" and not (ok_c or ok_r or ok_s):
        # 顶层键一个都没有 ⇒ 真的没人配过，不要把"内置默认值"伪装成"配置"。
        src = "default"
    return {"cols": cols, "rows": rows, "square_size_m": sq}, src
 def overhead_camera(data: dict, robot: dict | None = None) -> dict:
    """手外（固定）相机：``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台。
    **规则只有一份**，在 ``arm_application.site_io.overhead_camera``——S03 粗定位
    取命名空间走的也是它。这里若自己再写一遍"按 role 找 + 按 arm 收敛"，两份判据
    必然漂移，症状是"标定用的相机"和"推理用的相机"不是同一台，且错而不报。
    所以这里只做一层薄封装（从源码树 import），拿到配置字典。
    """
    fn = _load_overhead_camera_fn()
    cam, _others = fn(data, robot)
    return cam
 def _load_site_io():
    """从 ``src/arm_application`` 取 ``site_io`` 模块。
    为什么从源码树而不是已安装包：``export_site_env.py`` 在 ``colcon build``
    之前就被调用（``load_site_config.sh`` 的第一件事），那时 install 里可能还没有
    这个包。源码树在本仓库里恒定存在（``scripts/`` 与 ``src/`` 同根）。
    """
    src = _root() / "src" / "arm_application"
    if not (src / "arm_application" / "site_io.py").is_file():
        raise RuntimeError(
            f"找不到 {src}/arm_application/site_io.py —— 手外相机 / 结果文件名的规则"
            "只有那一处，不接受本文件自己再实现一份（两份判据必然漂移）。"
        )
    if str(src) not in sys.path:
        sys.path.insert(0, str(src))
    from arm_application import site_io as _mod  # noqa: PLC0415
    return _mod
 def _load_overhead_camera_fn():
    return _load_site_io().overhead_camera
 def flatten(data: dict) -> dict:
    """供 bash 脚本使用的扁平 ``SITE_*`` 环境变量映射。"""
    data, robot = apply_active_arm(data)
@@ -313,7 +530,59 @@ def flatten(data: dict) -> dict:
    # 两边会共用同一个手眼文件，表现是"标定做了但不生效"。
    short = arm_short_id(str(robot.get("sn") or ""))
    he_mode = str(he.get("mode") or "eye_in_hand")
-    he_stem = f"{short}_{he_mode}" if short else he_mode
+    over = overhead_camera(data, robot)
    over_name = str(over.get("name") or "").strip("/")
    root = _root()
    # 手眼标定的"按模式"派生物：结果文件 / 示教点 / 标定板。
    #
    # 为什么一切都要按模式分开（eye_in_hand / eye_to_hand）：
    #   1) 结果文件：眼在手外的产物是"**这台相机的光心 → 这条臂的基座**"，换条臂
    #      （基座不同）或换台相机都不通用 ⇒ 名字要同时带臂号与相机名
    #      （<臂号>_<相机>_eye_to_hand）。曾经只有 <臂号>_<模式>，于是一个双臂共用
    #      的手外相机只能存一份外参，第二条臂拿到的必然错；
    #   2) 标定板：两种装法用两块不同的物理板（A5 / A4），方格边长不同 ⇒ 见
    #      handeye_board() 的说明；
    #   3) 示教点：两种模式采的关节角完全不同，共用一份会把上一模式的点位当成本
    #      模式的（merge 之后才发现在动错的姿态）。
    he_modes_out: dict[str, str] = {}
    _e2h_name = _load_site_io().eye_to_hand_result_name
    for _m in HANDEYE_MODES:
        _tag = _m.upper()
        _pose_stem = f"{short}_{_m}" if short else _m
        # 结果文件名：眼在手外走 site_io.eye_to_hand_result_name（唯一口径）；
        # 眼在手上仍是 <臂号>_eye_in_hand.yaml。
        if _m == "eye_to_hand":
            _result_name = _e2h_name(short, over_name)
        else:
            _result_name = f"{_pose_stem}.yaml"
        # eye_to_hand 允许用显式路径覆盖（与 eye_in_hand 的 paths.handeye_file 对称）；
        # 不写就用派生的名字。
        if _m == "eye_in_hand":
            _file = str(
                paths.get("handeye_file")
                or root / "handeye" / "results" / _result_name
            )
            _wps = str(
                paths.get("handeye_waypoints")
                or root / "handeye" / "poses" / f"{_pose_stem}.yaml"
            )
        else:
            _file = str(
                paths.get(f"handeye_file_{_m}")
                or root / "handeye" / "results" / _result_name
            )
            _wps = str(
                paths.get(f"handeye_waypoints_{_m}")
                or root / "handeye" / "poses" / f"{_pose_stem}.yaml"
            )
        _board, _src = handeye_board(he, _m)
        he_modes_out[f"SITE_HANDEYE_FILE_{_tag}"] = _file
        he_modes_out[f"SITE_HANDEYE_WAYPOINTS_{_tag}"] = _wps
        he_modes_out[f"SITE_HANDEYE_BOARD_COLS_{_tag}"] = str(_board["cols"])
        he_modes_out[f"SITE_HANDEYE_BOARD_ROWS_{_tag}"] = str(_board["rows"])
        he_modes_out[f"SITE_HANDEYE_SQUARE_{_tag}"] = str(_board["square_size_m"])
        he_modes_out[f"SITE_HANDEYE_BOARD_SRC_{_tag}"] = _src
    out = {
        "SITE_CONFIG": str(site_path()),
@@ -339,10 +608,7 @@ def flatten(data: dict) -> dict:
        ),
        "SITE_YOLO_WEIGHTS": weights,
        "SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
-        "SITE_HANDEYE_WAYPOINTS": str(
+        "SITE_HANDEYE_WAYPOINTS": he_modes_out["SITE_HANDEYE_WAYPOINTS_EYE_IN_HAND"],
            paths.get("handeye_waypoints")
            or str(_root() / "handeye" / "poses" / f"{he_stem}.yaml")
        ),
        "SITE_HANDEYE_CALIBRATE": str(
            paths.get("handeye_calibrate")
            or str(_root() / "handeye" / "calibrate.yaml")
@@ -355,18 +621,24 @@ def flatten(data: dict) -> dict:
            paths.get("handeye_workdir")
            or str(_root() / "handeye" / "tmp" / short)
        ),
-        "SITE_HANDEYE_FILE": str(
+        # 不带后缀的两个名字保持旧语义（= 腕部 eye-in-hand 的产物），因为
-            paths.get("handeye_file")
+        # launch / 其它脚本一直按这两个名字读；换成"当前模式"会让 site.yaml 里
-            or str(_root() / "handeye" / "results" / f"{he_stem}.yaml")
+        # 一改 handeye.mode 就把 S05 精拍定位的 TF 换掉，那是另一个功能的静默失效。
-        ),
+        # 两个模式各自的名字在 *_EYE_IN_HAND / *_EYE_TO_HAND 里。
        "SITE_HANDEYE_FILE": he_modes_out["SITE_HANDEYE_FILE_EYE_IN_HAND"],
        "SITE_HANDEYE_MODE": he_mode,
-        # 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。
+        **he_modes_out,
-        # 这里原先是 11/16/0.011（一张早期用过、现已不存在的板），与 site.yaml 的
+        # 不带后缀的板尺寸 = **当前模式**那一组，供 `show` 之类只看当前模式的调用方；
-        # 8/11/0.015 冲突：一旦 site.yaml 缺这几个键就会静默用错尺寸，
+        # 标定脚本用带后缀的那三组（按 --mode 选），不依赖这个。
-        # 表现为"标定能跑完但结果明显不对"。
+        "SITE_HANDEYE_BOARD_COLS": he_modes_out[
-        "SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 8)),
+            f"SITE_HANDEYE_BOARD_COLS_{he_mode.upper()}"
-        "SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 11)),
+        ],
-        "SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.015)),
+        "SITE_HANDEYE_BOARD_ROWS": he_modes_out[
            f"SITE_HANDEYE_BOARD_ROWS_{he_mode.upper()}"
        ],
        "SITE_HANDEYE_SQUARE_M": he_modes_out[
            f"SITE_HANDEYE_SQUARE_{he_mode.upper()}"
        ],
        "SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)),
        "SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)),
        "SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)),
@@ -396,6 +668,21 @@ def flatten(data: dict) -> dict:
        "SITE_CAM_LASER_POWER": str(float(cam.get("laser_power") if cam.get("laser_power") is not None else 0)),
        "SITE_CAM_EXPOSURE_US": str(float(cam.get("exposure_us") or 0)),
        "SITE_CAM_GAIN": str(float(cam.get("gain") or 0)),
        # 手外（固定）相机的**按角色**导出。eye-to-hand 标定要用它：标定时得订阅
        # 这台固定相机，而不是腕部相机（run_handeye.sh 原来恒用 SITE_CAM_NAME，
        # 于是 `--mode eye_to_hand` 会去开腕部相机 —— 标出一个"看着成功"的错结果）。
        "SITE_OVERHEAD_CAM_NAME": over_name,
        "SITE_OVERHEAD_CAM_SERIAL": str(over.get("serial") or ""),
        "SITE_OVERHEAD_CAM_WIDTH": str(int(over.get("width") or 1280)),
        "SITE_OVERHEAD_CAM_HEIGHT": str(int(over.get("height") or 720)),
        "SITE_OVERHEAD_CAM_FPS": str(int(over.get("fps") or 30)),
        "SITE_OVERHEAD_CAM_AUTO_EXPOSURE": "1" if over.get("auto_exposure", True) else "0",
        "SITE_OVERHEAD_CAM_AE_LIMIT_US": str(
            float(over.get("ae_limit_us") if over.get("ae_limit_us") is not None else 8000)
        ),
        "SITE_OVERHEAD_CAM_EMITTER": "1" if over.get("emitter_enabled", True) else "0",
        "SITE_OVERHEAD_CAM_EXPOSURE_US": str(float(over.get("exposure_us") or 0)),
        "SITE_OVERHEAD_CAM_GAIN": str(float(over.get("gain") or 0)),
        "SITE_D435_SERIAL": str(((data.get("cameras") or {}).get("d435") or {}).get("serial") or ""),
        "SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
        "SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
@@ -417,6 +704,14 @@ def flatten(data: dict) -> dict:
        "SITE_EE_Z_M": str(float(servo.get("ee_z_m") or 0.24)),
        "SITE_SERVO_WATCHDOG": str(servo.get("watchdog_timeout_sec") or 0.3),
        "SITE_CAMERA_FRAME": camera_frames(data)[1],
        # 一阶段跟踪用的相机/手眼（可与腕部精拍不同）。SITE_CAM_NAME 仍是腕部。
        "SITE_TRACK_CAMERA": _load_site_io().track_camera_role(data),
        "SITE_TRACK_CAM_NAME": track_camera_frames(data)[0],
        "SITE_TRACK_CAMERA_FRAME": track_camera_frames(data)[1],
        "SITE_TRACK_HANDEYE_MODE": track_camera_frames(data)[3],
        "SITE_TRACK_HANDEYE_FILE": _track_handeye_file(
            data, robot, track_camera_frames(data)[3]
        ),
        "SITE_GRIP_VENDOR": str(grip.get("vendor") or "dh"),
        "SITE_GRIP_MODEL": str(grip.get("model") or "PGEA-100-20-W"),
        "SITE_GRIP_SN": str(grip.get("sn") or ""),
@@ -490,14 +785,14 @@ def perception_overrides(data: dict) -> dict:
    data, _robot = apply_active_arm(data)
    paths = data.get("paths") or {}
    yolo = data.get("yolo") or {}
-    cam_ns, cam_frame, depth_topic = camera_frames(data)
+    cam_ns, cam_frame, depth_topic, _he_mode = track_camera_frames(data)
    obj = str(yolo.get("object_frame") or "").strip()
    if not obj or obj == "tracked_object":
        # 跟踪目标的坐标系按**臂号**派生（SN 末两位），两侧互不干扰。
        # 以前是从 SN 里找 "063989"/"063991" 子串，换个站点就失去意义了。
        short = arm_short_id(str((_robot or {}).get("sn") or ""))
        obj = f"tracked_object_{short}" if short else "tracked_object"
-    return {
+    out = {
        "weights": str(paths.get("yolo_weights") or ""),
        "conf": float(yolo.get("conf") or 0.10),
        "iou": float(yolo.get("iou") or 0.45),
@@ -528,10 +823,36 @@ def perception_overrides(data: dict) -> dict:
        "object_frame": obj,
        "depth_topic": depth_topic,
        "camera_source": cam_ns,
        "base_frame": str((_robot or {}).get("base_frame") or ""),
        "feature_mode": str(yolo.get("feature_mode") or "plane"),
        # feature_mode=keypoint 时用：哪个关键点当跟踪点（k0）、可信度门槛。
        "keypoint_index": int(yolo.get("keypoint_index") if yolo.get("keypoint_index") is not None else 0),
        "keypoint_min_conf": float(yolo.get("keypoint_min_conf") or 0.30),
        "keypoint_win_px": int(yolo.get("keypoint_win_px") if yolo.get("keypoint_win_px") is not None else 6),
        # 三条关键点量纲守卫（2026-10-08 真机一阶段被假 k0 带走，见
        # feature_guard.py 的注释）。**必须在这里透传**：节点里声明的默认值
        # 恰好等于 site.yaml 当前值，漏了不会报错，只会让以后改配置毫无反应
        # （R1.1 的"声明了却没接线"）。
        "keypoint_border_px": float(yolo.get("keypoint_border_px") if yolo.get("keypoint_border_px") is not None else 6.0),
        "track_max_step_m": float(yolo.get("track_max_step_m") if yolo.get("track_max_step_m") is not None else 0.04),
        "reacquire_far_px": float(yolo.get("reacquire_far_px") if yolo.get("reacquire_far_px") is not None else 120.0),
        "reacquire_confirm_frames": int(yolo.get("reacquire_confirm_frames") if yolo.get("reacquire_confirm_frames") is not None else 3),
        "plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008),
        "plane_band_m": float(yolo.get("plane_band_m") or 0.06),
    }
    sio = _load_site_io()
    role = sio.track_camera_role(data)
    perc = data.get("perception") or {}
    if role == "overhead":
        _apply_overhead_range(data, out)
        out["overhead_oneshot_enable"] = False
    elif perc.get("track_camera") or "wrist_handoff_conf" in perc:
        # 显式 wrist / 写了混合键 → 必须接全套。缺键就红，不许用代码默认。
        # 没写 perception 段的探针测试保持旧行为（不加一次性）。
        _apply_wrist_hybrid(data, _robot, out)
    else:
        out["overhead_oneshot_enable"] = False
    return out
 def servo_overrides(data: dict) -> dict:
@@ -540,12 +861,20 @@ def servo_overrides(data: dict) -> dict:
    paths = data.get("paths") or {}
    yolo = data.get("yolo") or {}
    servo = data.get("servo") or {}
-    _ns, cam_frame, _depth = camera_frames(data)
+    _ns, cam_frame, _depth, he_mode = track_camera_frames(data)
    he_file = _track_handeye_file(data, robot, he_mode)
    xy_track = str(servo.get("xy_track") or "hover").strip().lower()
    if he_mode == "eye_to_hand" and xy_track == "lookat":
        raise RuntimeError(
            "perception.track_camera=overhead 时相机固定，lookat（把目标对准画面中心）"
            "没有意义。site.yaml 的 servo.xy_track 必须是 hover。"
        )
    out = {
        "joint_prefix": str(robot.get("joint_prefix") or ""),
        "base_frame": str(robot.get("base_frame") or ""),
        "ee_frame": str(robot.get("ee_frame") or ""),
-        "handeye_file": str(paths.get("handeye_file") or ""),
+        "handeye_file": he_file,
        "handeye_mode": he_mode,
        "camera_frame": cam_frame,
        "handeye_child_frame": cam_frame,
        "arm_label": str(robot.get("label") or robot.get("arm") or ""),
@@ -555,14 +884,19 @@ def servo_overrides(data: dict) -> dict:
    keys = [
        "control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec",
        "max_extrapolate_sec", "predict_enable", "predict_sec",
-        "predict_v_max", "predict_jump_m",
+        "predict_v_max", "predict_jump_m", "jump_confirm_frames", "feat_ws_margin_m",
        "kp_xy", "ki_xy", "kd_xy", "kp_z", "ki_z", "kd_z", "i_sep_pos_m",
        "deadzone_m", "deadzone_z_m", "v_max_xy", "v_max_z", "v_approach_z",
        # 远处限速（见 _slow_xy_limit）：漏了这两个键，site.yaml 改了也不生效，
        # 节点会静默退回声明默认值（R1.1）。
        "slow_far_m", "v_slow_xy",
        "z_min_m", "approach_xy_gate_m", "min_ee_z_m", "e_xy_clip_m",
        "ee_x_m", "ee_y_m", "ee_z_m", "ee_hold_xy", "ee_vertical_down",
        "xy_track", "hover_offset_ee_xy_m", "arm_label",
        "ee_hold_kp", "ee_w_max",
        "lock_hold_sec",
        "lock_still_sec",
        "lock_still_move_m",
        "a_max", "max_qdot", "damping",
        "vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec",
        "min_cmd_delta_rad", "point_topic", "traj_topic",
@@ -572,6 +906,38 @@ def servo_overrides(data: dict) -> dict:
    for k in keys:
        if k in servo:
            out[k] = servo[k]
    sio = _load_site_io()
    perc = data.get("perception") or {}
    if sio.track_camera_role(data) == "wrist" and (
        perc.get("track_camera") or "wrist_handoff_conf" in perc
    ):
        if "wrist_handoff_conf" not in perc:
            raise RuntimeError(
                "servo 混合跟踪必须读 perception.wrist_handoff_conf "
                "（与感知同一门槛，不许两边各写一个数）。"
            )
        out["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"])
        oh = perc.get("overhead") if isinstance(perc.get("overhead"), dict) else {}
        if "oneshot_hold_s" not in oh:
            raise RuntimeError(
                "servo 混合跟踪必须写 perception.overhead.oneshot_hold_s "
                "（D435 一次性设定保持多久）。"
            )
        out["overhead_hold_s"] = float(oh["oneshot_hold_s"])
        if "lock_still_sec" not in servo:
            raise RuntimeError(
                "servo.lock_still_sec 必须显式写"
                "（D405 跟住后法兰静止多久才进二阶段）。不许用代码默认 3。"
            )
        if "lock_still_move_m" not in servo:
            raise RuntimeError(
                "servo.lock_still_move_m 必须显式写"
                "（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。"
            )
        out["lock_still_sec"] = float(servo["lock_still_sec"])
        out["lock_still_move_m"] = float(servo["lock_still_move_m"])
        out["overhead_k0_topic"] = "visual_servo/overhead_k0"
        out["wrist_conf_topic"] = "visual_servo/wrist_conf"
    return out
@@ -620,10 +986,21 @@ def main() -> int:
        print(f"  手眼路点     : {flat['SITE_HANDEYE_WAYPOINTS']}")
        print(f"  手眼目录     : {flat['SITE_HANDEYE_WORKDIR']}")
        print(f"  命名位姿     : {flat.get('SITE_NAMED_POSES_FILE')}")
        print(f"  眼在手外结果 : {flat['SITE_HANDEYE_FILE_EYE_TO_HAND']}")
        print(f"  眼在手外路点 : {flat['SITE_HANDEYE_WAYPOINTS_EYE_TO_HAND']}")
        for _m in ("eye_in_hand", "eye_to_hand"):
            _t = _m.upper()
            _src = flat.get(f"SITE_HANDEYE_BOARD_SRC_{_t}")
            print(
-            "  标定板       : "
+                f"  标定板[{_m:12s}]: "
-            f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
+                f"{flat.get(f'SITE_HANDEYE_BOARD_COLS_{_t}')}x"
-            f"内角点, square={flat['SITE_HANDEYE_SQUARE_M']} m"
+                f"{flat.get(f'SITE_HANDEYE_BOARD_ROWS_{_t}')} 内角点, "
                f"square={flat.get(f'SITE_HANDEYE_SQUARE_{_t}')} m"
                + ("  ← 没配过（标定会被拦下）" if _src == "default" else "")
            )
        print(
            f"  手外相机     : {flat.get('SITE_OVERHEAD_CAM_NAME') or '（未登记）'}  "
            f"serial={flat.get('SITE_OVERHEAD_CAM_SERIAL') or '（空）'}"
        )
        print(
            f"  相机         : serial={flat['SITE_CAM_SERIAL'] or '（空）'}  "
@@ -20,6 +20,7 @@
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview     # 相机带 Qt 预览窗
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera          # 相机已在别处拉起了
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile         # 不起触觉服务
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent         # 只对准不轻降（验证精对准）
 #
 # 前置（另开一个终端，先跑起来）：
 #   bash ~/ros2/scripts/run_real.sh --arm 89
@@ -41,6 +42,7 @@ RESNAP=false
 WITH_CAMERA=true
 WITH_TACTILE=true
 CAMERA_PREVIEW=false
 NO_DESCENT=false
 # --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
 # shellcheck disable=SC1091
@@ -57,6 +59,9 @@ while [[ $# -gt 0 ]]; do
    --depth)          DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
    --pitch)          PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
    --resnap)         RESNAP=true; shift ;;
    # 只验证**精对准**、不轻降。对准逻辑改完先这么跑，确认规真被对到孔心再放开下降
    # （对不准就下压会把规顶在孔沿上：2026-10-08 真机顶到 63N）。
    --no-descent)     NO_DESCENT=true; shift ;;
    --no-preview)     PIPELINE_PREVIEW=false; shift ;;
    --camera-preview) CAMERA_PREVIEW=true; shift ;;
    --no-camera)      WITH_CAMERA=false; shift ;;
@@ -138,6 +143,33 @@ if [[ "${WITH_CAMERA}" == true ]]; then
    fi
    echo "    相机 ${CAM_NS} 出图 OK"
  fi
  # 一阶段混合跟踪：D405 看不见时要马上拍 D435。顶视没起来就只能干等。
  D435_TOPIC="/d435/color/image_raw"
  _d435_ok() {
    timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
    timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
  }
  if _d435_ok; then
    echo "=== 顶视 ${D435_TOPIC} 已在发图，跳过拉起 ==="
  else
    echo "=== 拉起顶视 D435（一阶段一次性定位）==="
    D435_ARGS=()
    [[ "${CAMERA_PREVIEW}" == true ]] && D435_ARGS+=(--preview) || D435_ARGS+=(--no-preview)
    nohup bash "${ROOT}/scripts/run_camera_d435.sh" \
      "${D435_ARGS[@]}" > /tmp/run_all_d435.log 2>&1 &
    echo "    d435 pid=$! log=/tmp/run_all_d435.log"
    d435_ok=false
    for _i in $(seq 1 24); do
      if _d435_ok; then d435_ok=true; break; fi
      sleep 0.5
    done
    if [[ "${d435_ok}" != true ]]; then
      echo "ERROR: ${D435_TOPIC} 仍无图像。看日志：/tmp/run_all_d435.log" >&2
      echo "  混合跟踪没有 D435 就切不出去。USB 独占或没插好都会这样。" >&2
      exit 1
    fi
    echo "    顶视 D435 出图 OK"
  fi
 else
  echo "=== --no-camera：跳过相机检查 ==="
 fi
@@ -206,10 +238,11 @@ ARGS=(
  "skip_stage3:=${SKIP_STAGE3}"
  "preview:=${PIPELINE_PREVIEW}"
  "resnap:=${RESNAP}"
  "no_descent:=${NO_DESCENT}"
 )
 [[ -n "${HOLE}" ]]  && ARGS+=("target_hole:=${HOLE}")
 [[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
 [[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
-echo "=== launch pipeline  hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} ==="
+echo "=== launch pipeline  hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ==="
 exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
@@ -34,7 +34,11 @@ source "${ROOT}/scripts/env.sh"
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/lib/load_site_config.sh"
-pkill -f 'lib/arm_application/realsense_color_pub' 2>/dev/null || true
+CAM_NAME="${SITE_CAM_NAME:-d405}"
 # 只杀本臂这台腕部相机。以前 pkill 全部 realsense_color_pub / realsense_view，
 # 会把已经开着的顶视 D435 一起干掉（或反过来：后开 D435 把 D405 预览窗杀掉），
 # 现场看起来就像"只剩眼外画面、绿框也不见了"。
 pkill -f "camera_name:=${CAM_NAME}( |$)" 2>/dev/null || true
 sleep 2
 USB_INFO="$(python3 - <<PY 2>/dev/null || true
@@ -88,7 +92,6 @@ else
  echo "=== camera 1280x720@15 ==="
 fi
 CAM_NAME="${SITE_CAM_NAME:-d405}"
 CAM_ARGS=(
  --ros-args
  -p camera_name:="${CAM_NAME}"
@@ -140,12 +143,13 @@ if [[ "${DO_PREVIEW}" == "1" ]]; then
  echo "=== close window = hide preview only; camera keeps publishing ==="
 fi
-echo "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ==="
+echo "=== 本脚本只出图，不跑 YOLO。绿框来自 perception_node，请另开终端： ==="
 echo "===   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} ==="
 if [[ "${DO_PREVIEW}" == "1" ]]; then
  # 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制
  # 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。
-  pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
+  # 前面已经按 camera_name 杀过本臂进程，这里不要再杀全部 realsense_view。
  ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
  CAM_PID=$!
  trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
@@ -1,5 +1,6 @@
 #!/usr/bin/env bash
-# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。不会打开任何一个 D405。
+# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。
 # 可以和 run_camera_d405.sh 同时开：只杀 camera_name:=d435，不动腕部那台。
 #
 #   bash ~/ros2/scripts/run_camera_d435.sh --preview
 #
@@ -24,7 +25,9 @@ if [[ -z "${SN}" ]]; then
  exit 2
 fi
 echo "=== D435 overhead serial=${SN}  ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ==="
-pkill -f 'camera_name:=d435' 2>/dev/null || true
+# 只杀顶视这一台。不要 pkill 全部 realsense_view —— 那会把正在看的 D405 预览窗干掉，
 # 绿框叠在 D405 窗口上，窗没了就像"伺服没画面、也没工件框"。
 pkill -f "camera_name:=d435( |$)" 2>/dev/null || true
 CAM_ARGS=(
  --ros-args
  -p camera_name:=d435
@@ -40,11 +43,13 @@ CAM_ARGS=(
  -p emitter_enabled:=true
 )
 if [[ "${PREVIEW}" == "1" ]]; then
  pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
  ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
  CAM_PID=$!
  trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
-  ros2 run arm_application realsense_view --ros-args -p camera_name:=d435
+  # 工件框是 D405 像素，叠到 1280×720 的顶视图上位置全错，看起来像没检出。
  ros2 run arm_application realsense_view --ros-args \
    -p camera_name:=d435 \
    -p overlay_detection:=false
 else
  exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
 fi
@@ -68,25 +68,21 @@ if [[ ! -d "${APP_CFG}" ]]; then
 fi
 # 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。
-# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号**
+# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号与相机名**
-# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算）。
+# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算；相机名来自 cameras 注册表）。
 #
-# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id` 和 `HANDEYE_MODE`
+# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id`、`HANDEYE_MODE` 与相机名
 # 在下面几行之后才算得出来，在这里拼会拼出 `results/_.yaml` 这种空文件名，
 # 而症状是"标定写完了、用的时候读不到"。
 HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
 WP="${SITE_HANDEYE_WAYPOINTS:?site.yaml 没有给出该臂的手眼示教点文件}"
 CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
 TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
 RES="${SITE_HANDEYE_FILE:?site.yaml 没有给出该臂的手眼结果文件}"
 WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}"
 TAUGHT="${WORKDIR}/taught_poses.txt"
 PREFIX="${SITE_JOINT_PREFIX:?SITE_JOINT_PREFIX 为空 —— site.yaml 里需要该臂的 robot.sn}"
 CAM="${SITE_CAM_NAME:-d405}"
 # ---- --mode：切换 eye-in-hand / eye-to-hand ----
-# 必须在 WP/RES 定下来之前处理，因为它会改变读写的文件名。
+# 必须在 WP/RES/板/相机定下来之前处理：模式决定了读哪个结果文件、用哪块板、
-# 其余参数原样留给子命令（teach 自己有 --rdk/--ros 等）。
+# 开哪台相机、采点落到哪个文件。其余参数原样留给子命令（teach 自己有 --rdk 等）。
 HANDEYE_MODE="${SITE_HANDEYE_MODE:-eye_in_hand}"
 _rest=()
 while [[ $# -gt 0 ]]; do
@@ -109,15 +105,61 @@ set -- "${_rest[@]}"
 # 臂号取 Flexiv 序列号后两位：Rizon4-063989 → 89
 _arm_id="${SITE_ROBOT_SN: -2}"
-if [[ -n "${_arm_id}" && "${_arm_id}" != "${SITE_ROBOT_SN}" ]]; then
+
-  if [[ "${HANDEYE_MODE}" != "${SITE_HANDEYE_MODE:-eye_in_hand}" ]]; then
+# 取"当前模式"的那一份值。名字由 export_site_env 按模式导出成
-    # 显式指定了非默认模式 → 按 <臂号>_<模式>.yaml 重新定位
+#   SITE_HANDEYE_<字段>_EYE_IN_HAND / _EYE_TO_HAND
-    WP="${HE_DIR}/poses/${_arm_id}_${HANDEYE_MODE}.yaml"
+# 这样"模式 → 结果文件 / 示教点 / 标定板"的对应关系只有 site.yaml 一处定义，
-    RES="${HE_DIR}/results/${_arm_id}_${HANDEYE_MODE}.yaml"
+# 脚本不再自己拼文件名（拼错/漏拼都是**静默**用错文件，最难查）。
-    echo "[handeye] --mode ${HANDEYE_MODE}  →  poses: ${WP}"
+_HE_TAG="$(echo "${HANDEYE_MODE}" | tr '[:lower:]' '[:upper:]')"
-    echo "[handeye]                            result: ${RES}"
+_he() {
  local name="SITE_HANDEYE_$1_${_HE_TAG}"
  printf '%s' "${!name-}"
 }
 WP="$(_he WAYPOINTS)"
 WP="${WP:-${HE_DIR}/poses/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
 RES="$(_he FILE)"
 RES="${RES:-${HE_DIR}/results/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
 # 示教点临时文件也按模式分开：两个模式共用一份时，后一个 teach 会清掉前一个的采点，
 # 而症状只是"merge 出来的点位不对"，很难联想到是文件串了。
 TAUGHT="${WORKDIR}/taught_poses_${HANDEYE_MODE}.txt"
 # ---- 相机：两种模式用的是**两台不同的相机** ----
 # 眼在手上 = 腕部相机（跟着臂动）；眼在手外 = 固定在工位外的相机（不随臂动）。
 # 原来这里恒用 SITE_CAM_NAME（腕部），于是 `--mode eye_to_hand` 会去开腕部相机 ——
 # 板跟着臂动、相机也跟着臂动，"标定"能跑完，但解出的是个毫无意义的错外参。
 if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
  CAM="${SITE_OVERHEAD_CAM_NAME:-}"
  if [[ -z "${CAM}" ]]; then
    echo "ERROR: eye_to_hand 需要一台固定相机，但 site.yaml 的 cameras 段里没有登记的。" >&2
    echo "       加一条 role: overhead_eye_to_hand（眼在手外、不随臂动）的相机即可。" >&2
    exit 2
  fi
  CAM_SERIAL="${SITE_OVERHEAD_CAM_SERIAL:-}"
  CAM_WIDTH="${SITE_OVERHEAD_CAM_WIDTH:-}"
  CAM_HEIGHT="${SITE_OVERHEAD_CAM_HEIGHT:-}"
  CAM_FPS="${SITE_OVERHEAD_CAM_FPS:-30}"
  CAM_AE="${SITE_OVERHEAD_CAM_AUTO_EXPOSURE:-1}"
  CAM_AE_LIMIT="${SITE_OVERHEAD_CAM_AE_LIMIT_US:-8000}"
  CAM_EMITTER="${SITE_OVERHEAD_CAM_EMITTER:-1}"
  CAM_EXPOSURE="${SITE_OVERHEAD_CAM_EXPOSURE_US:-0}"
  CAM_GAIN="${SITE_OVERHEAD_CAM_GAIN:-0}"
 else
  CAM="${SITE_CAM_NAME:-d405}"
  CAM_SERIAL="${SITE_CAM_SERIAL:-}"
  CAM_WIDTH="${SITE_CAM_WIDTH:-}"
  CAM_HEIGHT="${SITE_CAM_HEIGHT:-}"
  CAM_FPS="${SITE_CAM_FPS:-30}"
  CAM_AE="${SITE_CAM_AUTO_EXPOSURE:-1}"
  CAM_AE_LIMIT="${SITE_CAM_AE_LIMIT_US:-1800}"
  CAM_EMITTER="${SITE_CAM_EMITTER:-0}"
  CAM_EXPOSURE="${SITE_CAM_EXPOSURE_US:-0}"
  CAM_GAIN="${SITE_CAM_GAIN:-0}"
 fi
 echo "[handeye] mode=${HANDEYE_MODE}  poses=${WP}"
 echo "[handeye]                  result=${RES}"
 echo "[handeye]                  camera=${CAM} serial=${CAM_SERIAL:-（空）}"
 _kill_servo() {
  pkill -f 'visual_servo.launch' 2>/dev/null || true
@@ -159,7 +201,9 @@ cmd_show() {
  echo "  tour        : ${TOUR}"
  echo "  result      : ${RES}"
  echo "  workdir     : ${WORKDIR}"
-  echo "  board       : ${SITE_HANDEYE_BOARD_COLS:-?}x${SITE_HANDEYE_BOARD_ROWS:-?} inner, square=${SITE_HANDEYE_SQUARE_M:-?} m"
+  echo "  camera      : ${CAM} serial=${CAM_SERIAL:-（空）}"
  echo "  board       : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m  (来源=$(_he BOARD_SRC))"
  echo "                （另一种模式的板见下面 export 的 标定板[...] 两行）"
  python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
  if [[ -f "${WP}" ]]; then
    python3 - <<PY
@@ -181,15 +225,26 @@ cmd_merge() {
 }
 cmd_calibrate() {
  # 先查配置（板尺寸）再看硬件：配置没配好的话，连硬件都不该去动。
  local cols="$(_he BOARD_COLS)"
  local rows="$(_he BOARD_ROWS)"
  local sq="$(_he SQUARE)"
  local src="$(_he BOARD_SRC)"
  # 板尺寸必须**真的配过**。没配就开标定 = 赌一个方格边长，而错了不会报错，
  # 只会让结果系统性偏（见 handeye/README.md 第 3 节）。所以这里直接拦下。
  if [[ -z "${src}" || "${src}" == "default" ]]; then
    echo "ERROR: 没有配置 [${HANDEYE_MODE}] 用的标定板尺寸。" >&2
    echo "       在 config/site.yaml 的 handeye.boards.${HANDEYE_MODE} 里写：" >&2
    echo "         cols / rows / square_size_m" >&2
    echo "       （内角点 = 方格数 − 1；测一下实际方格边长，别照抄另一块板。）" >&2
    exit 2
  fi
  _need_files "${CAL}" "${WP}"
  _need_robot
  _need_camera
  echo "=== kill visual_servo (avoid traj fight) ==="
  _kill_servo
  mkdir -p "${WORKDIR}"
  local cols="${SITE_HANDEYE_BOARD_COLS:-8}"
  local rows="${SITE_HANDEYE_BOARD_ROWS:-11}"
  local sq="${SITE_HANDEYE_SQUARE_M:-0.015}"
  local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}"
  local mt="${SITE_HANDEYE_MOVE_T:-8.0}"
  local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}"
@@ -200,7 +255,8 @@ cmd_calibrate() {
  local ps="${SITE_HANDEYE_PIN_SEC:-0.45}"
  local skip="${SITE_HANDEYE_SKIP_UNSTABLE:-true}"
  echo "=== calibrate mode=${HANDEYE_MODE} ==="
-  echo "    board ${cols}x${rows} inner, square=${sq} m  (from site.yaml)"
+  echo "    board ${cols}x${rows} inner, square=${sq} m  (site.yaml handeye.boards.${HANDEYE_MODE}，来源=${src})"
  echo "    camera ${CAM} serial=${CAM_SERIAL:-（空）}"
  echo "    motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
  echo "    poses=${WP}"
  echo "    result → ${RES}"
@@ -209,6 +265,7 @@ cmd_calibrate() {
    --params-file "${WP}" \
    -p joint_prefix:="${PREFIX}" \
    -p camera_source:="${CAM}" \
    ${CAM_SERIAL:+-p camera_serial:="${CAM_SERIAL}"} \
    -p output_dir:="${WORKDIR}" \
    -p result_file:="${RES}" \
    -p mode:="${HANDEYE_MODE}" \
@@ -304,18 +361,18 @@ cmd_teach() {
    -p output_dir:="${WORKDIR}"
    -p target_points:="${points}"
  )
-  if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
+  if [[ -n "${CAM_SERIAL}" ]]; then
-    TEACH_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
+    TEACH_ARGS+=(-p camera_serial:="${CAM_SERIAL}")
  fi
-  if [[ -n "${SITE_CAM_WIDTH:-}" ]]; then
+  if [[ -n "${CAM_WIDTH}" ]]; then
-    TEACH_ARGS+=(-p width:="${SITE_CAM_WIDTH}" -p height:="${SITE_CAM_HEIGHT:-480}" -p fps:="${SITE_CAM_FPS:-30}")
+    TEACH_ARGS+=(-p width:="${CAM_WIDTH}" -p height:="${CAM_HEIGHT:-480}" -p fps:="${CAM_FPS:-30}")
  fi
  TEACH_ARGS+=(
-    -p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
+    -p auto_exposure:="$([ "${CAM_AE:-1}" = "1" ] && echo true || echo false)"
-    -p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-1800}"
+    -p ae_limit_us:="${CAM_AE_LIMIT:-1800}"
-    -p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
+    -p emitter_enabled:="$([ "${CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
-    -p exposure_us:="${SITE_CAM_EXPOSURE_US:-0}"
+    -p exposure_us:="${CAM_EXPOSURE:-0}"
-    -p gain:="${SITE_CAM_GAIN:-0}"
+    -p gain:="${CAM_GAIN:-0}"
  )
  ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}" || true
@@ -362,8 +419,14 @@ cmd_teach() {
  # 但 teach 阶段把相机杀掉了（USB 独占），标定拍照要用，得先拉回来。
  echo
  echo "=== 重启相机（teach 阶段杀掉了它，标定拍照需要）==="
-  nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \
+  if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
-    --no-preview > /tmp/run_handeye_camera.log 2>&1 &
+    # 眼在手外用的是固定相机（D435），不是腕部 D405 —— 拉错相机的话，
    # 标定会去订一个没人发布的图，表现是"相机没数据"。
    _cam_cmd=(bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview)
  else
    _cam_cmd=(bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" --no-preview)
  fi
  nohup "${_cam_cmd[@]}" > /tmp/run_handeye_camera.log 2>&1 &
  echo "    pid=$!  log=/tmp/run_handeye_camera.log"
  # _need_camera 内部失败时会自己打印原因并 exit，不需要在这里再判一次。
  _need_camera
@@ -8,6 +8,11 @@
 # --control 从当前位姿直接开始跟踪（不抬升，也不回 home）。
 # 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
 #
 # 跟踪相机由 site.yaml 的 perception.track_camera 决定：
 #   wrist    腕部 D405 主跟踪；画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0
 #            —— 需要同时开 run_camera_d405.sh 与 run_camera_d435.sh
 #   overhead 整段只用顶置 D435（实验）—— 另开 run_camera_d435.sh
 #
 # 切换 YOLO 权重：改 site.yaml 里的 paths.yolo_weights
 set -eo pipefail
@@ -71,13 +76,37 @@ else
  echo "=== launch perception + servo (observe only, no arm motion) ==="
 fi
 EE="${SITE_EE_FRAME:?SITE_EE_FRAME 为空 —— site.yaml 的 robot.robots[] 里需要该臂的 SN}"
-HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}"
+TRACK="${SITE_TRACK_CAMERA:-wrist}"
-echo "    arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-}  handeye=${HANDEYE}"
+HANDEYE="${SITE_TRACK_HANDEYE_FILE:-${SITE_HANDEYE_FILE:-}}"
-echo "    weights=${SITE_YOLO_WEIGHTS}  camera=${SITE_CAM_NAME:-}  control=${CONTROL}"
+CAM_FRAME="${SITE_TRACK_CAMERA_FRAME:-${SITE_CAMERA_FRAME:-}}"
 CAM_NAME="${SITE_TRACK_CAM_NAME:-${SITE_CAM_NAME:-}}"
 if [[ -z "${HANDEYE}" ]]; then
  echo "ERROR: 跟踪用手眼文件为空（SITE_TRACK_HANDEYE_FILE / SITE_HANDEYE_FILE）" >&2
  exit 2
 fi
 if [[ ! -f "${HANDEYE}" ]]; then
  echo "ERROR: 跟踪用手眼文件不存在：${HANDEYE}" >&2
  if [[ "${TRACK}" == "overhead" ]]; then
    echo "  先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate --mode eye_to_hand" >&2
  else
    echo "  先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate" >&2
  fi
  exit 2
 fi
 echo "    arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-}  track=${TRACK}  camera=${CAM_NAME}"
 echo "    handeye=${HANDEYE}  frame=${CAM_FRAME}  control=${CONTROL}"
 if [[ "${TRACK}" == "overhead" ]]; then
  echo "=== 整段顶视：D435 追踪，D405 不参与一阶段 ==="
  echo "    相机请开：bash ~/ros2/scripts/run_camera_d435.sh --preview"
 else
  echo "=== 本脚本不弹预览窗。工件框画在 D405 那个查看器上 ==="
  echo "    bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview"
  echo "    bash ~/ros2/scripts/run_camera_d435.sh --preview   # 顶视可开，绿框仍在 D405 窗"
 fi
 LAUNCH_ARGS=(
  enable_control:="${CONTROL}"
  handeye_file:="${HANDEYE}"
  ee_frame:="${EE}"
-  camera_frame:="${SITE_CAMERA_FRAME:-d405_color_optical_frame}"
+  camera_frame:="${CAM_FRAME}"
 )
 exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}"
@@ -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())
@@ -28,6 +28,19 @@ if iface:
            break
        except TypeError:
            continue
        except RuntimeError as exc:
            # 这个 iface 用不了（实测：enp4s0 上有 5 个 IPv4 时，RDK 的
            # UDPv4Transport 会把整张白名单都过滤掉，报 "All whitelist
            # interfaces were filtered out"），别再拿它试了，交给下面的
            # 「不指定网卡」兜底。
            #
            # **这一条以前漏了**：老代码只捕获 TypeError，而真实失败是
            # RuntimeError —— 于是异常直接抛出去，下面那行兜底永远走不到。
            # 后果是预检报「机械臂 RDK 连不上」这个**假阻断**，
            # 而真机栈其实连得上（flexiv_hardware_interface.cpp 构造
            # flexiv::rdk::Robot 时只传 robot_sn、不传网卡）。
            print(f"  iface={iface} 不可用（{str(exc)[:70]}），回退到不指定网卡")
            break
 if robot is None:
    robot = flexivrdk.Robot(sn)
 print("Connected. Clearing fault if any...")
@@ -0,0 +1,200 @@
 #!/usr/bin/env python3
 """跟踪点的三道守卫：**位置合理性** + **每帧位移上限** + **远框重捕获确认**。
 为什么需要它们（2026-10-08 真机 arm 89，一阶段跑 150s 没锁上）
 ------------------------------------------------------------
 ``/tmp/pipeline_stage1.log`` 原话。真 k0 一开始是对的::
    764.79  跟踪 keypoint conf=0.87 uv=(410,246) z=0.299      ← 真 k0（画面中偏右）
 然后在 1 秒内被一串**假 k0** 顶掉（全都是高置信度）::
    766.96  重新捕获 conf=0.58 dpx=480
    766.96  跟踪 keypoint conf=0.58 uv=(76,75)   z=0.392      ← 贴左上角
    767.75  跟踪 keypoint conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near
    768.16  跟踪 keypoint conf=0.53 uv=(143,92)  z=0.376
    768.78  跟踪 keypoint conf=0.73 uv=(12,30)   k0(ok=1551 near=620 借深度) z=0.276
    769.66  重新捕获 conf=0.22 dpx=207
    770.07  跟踪 keypoint conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度) z=0.271
 伺服于是以 0.45m/s 追着它们跑：``ee_xy`` home ``(0.686,-0.113)`` → ``(0.354,-0.294)``
 → ``(0.083,-0.515)``（工作盒角落），1.2s 后看门狗急停；工件已在半米外，
 剩下 148s 全是"检测漏检"。三条结论：
  1. **置信度挡不住**：假 k0 的 conf 是 0.53~0.73，真 k0 是 0.87 —— 没有可分区间。
     但假 k0 有一个共同特征：**落在画面边上**（``uv=(12,30)`` / ``(76,75)`` /
     ``(143,92)``），而孔心长在工件上、不可能贴在画面边缘；
  2. **借深度把"没有深度"伪装成"有深度"**：``k0(… 借深度)`` 的 z 是从工件框里
     借的，看着很合理（0.27~0.39m），于是"物理量程"这道守卫也失效了。而
     **借来的深度意味着该像素处读不到自己的深度** ⇒ 那个点的绝对 XY 不可靠，
     不该用来**重定位**（换一个跟踪位置）；
  3. **位移不受限**：真 k0 到假 k0 一帧能差 480px / 400mm，伺服一次就全速扑过去。
 三道守卫各挡一层，全都是"能真的失败"的判据（见 test_feature_guard_*.py 的负向用例）：
 ``kpt_plausible``
    k0 必须落在自己的检测框内（含少量余量）、且不许贴画面边缘。违反 ⇒ 本帧的
    k0 **位置**不可信（不是"没有目标"），交回调用方走"按框上相对偏移重建"的
    连续性路径（``perception_node._keypoint_feature`` 本来就有这条路）。
 ``TrackSlewGuard``
    已发布跟踪点的**每帧位移上限**。工件不动、相机 30Hz，真实观测一帧不可能走
    4cm（``feature_jump_guard`` 实测真实运动 ≤15mm/帧）。超过上限：观测可信就
    **夹到上限**（限速跟随，不丢目标），观测来自借深度就**整帧不采信**。
 ``FarReacquireConfirm``
    "远框重新捕获"（``dpx`` 大于门限）必须连续 ``confirm_frames`` 帧指向同一处
    才采信 —— 一帧的远框更可能是模型在别的东西上误报，而不是工件真的被挪走了。
 """
 from __future__ import annotations
 from typing import Optional, Sequence, Tuple
 import numpy as np
 def _xy_dist(a: np.ndarray, b: np.ndarray) -> float:
    """两个 XY 点的距离（米/像素都行，单位跟随入参）。单一实现，别各写一份。"""
    return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
 def kpt_plausible(
    u: float,
    v: float,
    box: Optional[Sequence[float]],
    w: int,
    h: int,
    *,
    border_px: float = 6.0,
    box_margin_frac: float = 0.10,
 ) -> Optional[str]:
    """k0 的**像素位置**是否可信。返回 None = 可信；否则返回拒绝原因。
    ``box`` 是同一帧的工件检测框 ``(x0,y0,x1,y1)``；``w,h`` 是彩色图宽高。
    """
    if not (np.isfinite(u) and np.isfinite(v)):
        return "k0 像素非有限值"
    if box is None:
        # 没有框就没法判断 k0 是否落在工件上 —— 宁可当不可信（走连续性路径）。
        return "没有检测框，无法判断 k0 是否落在工件上"
    b = np.asarray(box, dtype=float).reshape(-1)
    if b.size < 4:
        return "检测框字段不完整"
    x0, y0, x1, y1 = (float(vv) for vv in b[:4])
    bw, bh = abs(x1 - x0), abs(y1 - y0)
    if not (np.isfinite(bw) and np.isfinite(bh)) or bw <= 1e-6 or bh <= 1e-6:
        return "检测框尺寸无效"
    mx, my = box_margin_frac * bw, box_margin_frac * bh
    if u < x0 - mx or u > x1 + mx or v < y0 - my or v > y1 + my:
        return (
            f"k0 ({u:.0f},{v:.0f}) 落在检测框 "
            f"[{x0:.0f},{y0:.0f},{x1:.0f},{y1:.0f}] 之外"
        )
    if w > 0 and h > 0:
        if u <= border_px or v <= border_px or u >= w - 1 - border_px or v >= h - 1 - border_px:
            return (
                f"k0 ({u:.0f},{v:.0f}) 贴画面边缘（{w}x{h}，余量 {border_px:.0f}px）"
                "—— 孔心长在工件上，不可能在画面边上"
            )
    return None
 class TrackSlewGuard:
    """已发布跟踪点的每帧位移上限。
    单实例服务**一个**跟踪点；调用方负责在"没有目标/重置"时 :meth:`reset`。
    """
    def __init__(self, max_step_m: float) -> None:
        # 只有**一个**门限，故意的：观测可信就限速跟随，借深度就整帧不采信。
        # 不做成两个门限是因为"借深度"只影响"能不能重定位"，不影响"小步跟不跟" ——
        # 两个门限会立刻变成"哪个该调、调到多少"的扯皮（且没有实测支撑）。
        self.max_step_m = float(max_step_m)
        self._last: Optional[np.ndarray] = None
        self.clamped_total = 0
        self.rejected_borrowed_total = 0
    def reset(self) -> None:
        self._last = None
    def update(
        self, p_xy: Optional[Sequence[float]], *, borrowed: bool = False
    ) -> Tuple[Optional[np.ndarray], Optional[str]]:
        """喂一次观测，返回 ``(采信的点, 拒绝原因)``。
        - 第一次观测（没有历史）：原样采信（冷启动没有更好的信息，见服务节点的
          照常起步）。
        - 位移 ≤ 上限：原样采信。
        - 位移 > 上限且 ``borrowed``：整帧不采信（返回 ``None`` + 原因）——
          借来的深度意味着这个像素读不到自己的深度，绝对位置不足以用来重定位。
        - 位移 > 上限且观测可信：**夹到上限**（限速跟随），不丢目标。
        """
        if p_xy is None:
            return None, "本帧没有观测"
        p = np.asarray(p_xy, dtype=float).reshape(2)
        if not np.all(np.isfinite(p)):
            return None, "跟踪点非有限值"
        if self._last is None:
            self._last = p.copy()
            return p, None
        d = _xy_dist(p, self._last)
        if d <= self.max_step_m:
            self._last = p.copy()
            return p, None
        if borrowed:
            self.rejected_borrowed_total += 1
            return None, (
                f"借深度的观测要求位移 {d*1000:.1f}mm > 上限 "
                f"{self.max_step_m*1000:.0f}mm —— 判误检，本帧不采信"
            )
        step = self._last + (p - self._last) * (self.max_step_m / d)
        self._last = step.copy()
        self.clamped_total += 1
        return step, (
            f"位移 {d*1000:.1f}mm > 上限 {self.max_step_m*1000:.0f}mm —— "
            "夹到上限限速跟随"
        )
 class FarReacquireConfirm:
    """远框"重新捕获"的多帧确认。
    ``dpx`` 是候选框中心与上一帧框中心的像素距离。超过 ``far_px`` 的候选必须
    连续 ``confirm_frames`` 帧落在同一处（相互间距 ≤ ``far_px``）才放行。
    """
    def __init__(self, far_px: float, confirm_frames: int = 3) -> None:
        self.far_px = float(far_px)
        self.confirm_frames = max(1, int(confirm_frames))
        self._cand: Optional[np.ndarray] = None
        self._count = 0
    def reset(self) -> None:
        self._cand = None
        self._count = 0
    def allow(
        self, center_xy: Optional[Sequence[float]], dpx: float, *, near_px: float
    ) -> Tuple[bool, Optional[str]]:
        """返回 ``(是否放行, 拒绝原因)``。``near_px`` 是"不算远框"的门限。"""
        if center_xy is None:
            self.reset()
            return True, None
        c = np.asarray(center_xy, dtype=float).reshape(2)
        if not np.all(np.isfinite(c)):
            return False, "候选框中心非有限值"
        if float(dpx) <= float(near_px):
            # 近框：正常跟踪，不需要确认
            self.reset()
            return True, None
        if self._cand is None or _xy_dist(c, self._cand) > self.far_px:
            self._cand = c.copy()
            self._count = 1
        else:
            self._count += 1
        if self._count >= self.confirm_frames:
            self.reset()
            return True, None
        return False, (
            f"远框重捕获 dpx={float(dpx):.0f}px 需连续 {self.confirm_frames} 帧一致"
            f"（当前 {self._count} 帧）—— 本帧不采信"
        )
@@ -0,0 +1,98 @@
 #!/usr/bin/env python3
 """跟踪点"假跳变"的确认门限：单帧大跳不采信，连续 N 帧一致才认。
 为什么需要它（2026-10-08 真机实测，一阶段跑 150s 没锁上）
 --------------------------------------------------------
 arm 89 的日志原话：
    0659.551  死区保持 |e_xy|=3.0mm              ← 其实已经锁定！误差只剩 3mm
    0659.598  k0 置信度 0.01 < 0.30，本帧不用它
    0659.604  HANDEYE snap feat_base=(0.675,-0.099)   ← 跟踪点瞬间横移 89mm
    0660.121  v=(-0.206,+0.400) |e_xy|=130mm     ← 伺服以 0.45m/s 去追这个假点
    0660.561  过期保持 —— 不跟踪冻结的检测框       ← 画面糊掉，目标彻底丢失
 原来的写法是"跳变超过 ``predict_jump_m`` 就 ``reset()``" —— 等于**单帧就采信**。
 可是相机 30Hz、工件不动，真实特征点一帧不可能跳 89mm（实测的真实运动
 在 15mm/帧 以内）。所以"大跳"几乎必然是检测/兜底产生的假点，不该一路追过去。
 判据
 ----
 - 位移 ≤ ``jump_m``：正常观测（``near``），照常滤波更新。
 - 位移 > ``jump_m``：进入"待确认"。此后**连续 ``confirm_frames`` 帧**都要落在
  同一个新位置附近（相互间距 ≤ ``jump_m``），才判为真实重定位（``jump``）。
 - 只要中间有一帧不对上（乱跳），计数清零、重新开始 —— 乱跳永远等不到确认。
 这也顺手挡住"感知层 k0 掉信后乱跳"那类现象：宁可原地不动，也不要追着假点跑。
 """
 from __future__ import annotations
 from typing import Optional
 import numpy as np
 # ``servo_node._update_feat_track`` 的三种判定结果。
 # 为什么要显式区分"没有目标"：perception 会把误检点也发过来（2026-10-08 真机，
 # 模型在运动模糊下把特征点丢到基座后方 -0.10,-0.65，工件其实在 0.5,-0.12），
 # 这种点在物理上不可能 —— 必须当"没看到目标"处理，而不是"钉在原地"。
 ACCEPT = "accept"  # 采信这次观测
 HOLD = "hold"  # 疑似假跳 —— 位置钉住不动
 REJECT = "reject"  # 物理上不合理（工作盒外）—— 当"没有目标"处理
 class FeatureJumpGate:
    """有状态的跳变确认门限。线程安全由调用方保证（servo 在锁内调用）。"""
    NEAR = "near"  # 正常观测：照常滤波更新
    HOLD = "hold"  # 疑似假跳：本次不采信，位置保持不动
    JUMP = "jump"  # 已确认的真实重定位：允许 reset 到新位置
    def __init__(self, jump_m: float, confirm_frames: int = 3) -> None:
        self.jump_m = float(jump_m)
        # 至少 1 帧才谈得上"确认"；<=1 就等于退回旧的"单帧采信"，但那是显式选择，
        # 不是静默降级 —— 这里仍然保留参数以方便现场调节。
        self.confirm_frames = max(1, int(confirm_frames))
        self._cand: Optional[np.ndarray] = None
        self._count = 0
        # 诊断计数：连续 hold 了多少帧（供日志/排查，见 servo_node 的告警）。
        self.hold_frames = 0
        self.hold_total = 0
    def reset(self) -> None:
        """清空待确认状态（正常观测、或已确认后都要清）。"""
        self._cand = None
        self._count = 0
        self.hold_frames = 0
    @staticmethod
    def _dist(a: np.ndarray, b: np.ndarray) -> float:
        return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
    def update(self, p_xy: np.ndarray, ref_xy: np.ndarray) -> str:
        """喂一次观测。
        参数
        ----
        p_xy   : 本帧测到的 XY（base 系，米）
        ref_xy : 当前滤波后的位置 XY（米）—— 判断"跳没跳"的基准
        返回 :data:`NEAR` / :data:`HOLD` / :data:`JUMP` 之一。
        """
        p = np.asarray(p_xy, dtype=float).reshape(2)
        ref = np.asarray(ref_xy, dtype=float).reshape(2)
        if self._dist(p, ref) <= self.jump_m:
            self.reset()
            return self.NEAR
        # 大跳：本次观测只算"候选"。候选之间互相对不上就从头再数。
        if self._cand is None or self._dist(p, self._cand) > self.jump_m:
            self._cand = p.copy()
            self._count = 1
        else:
            self._count += 1
        if self._count >= self.confirm_frames:
            self.reset()
            return self.JUMP
        self.hold_frames += 1
        self.hold_total += 1
        return self.HOLD
@@ -94,6 +94,7 @@ from arm_application.gauge_config import (
 )
 from arm_application.gauge_fsm import (
    DESCEND_STALL_STEPS,
    ENTRY_HIGH,
    ROT_AXIS,
    ROT_LIMIT,
    ROT_SHORT,
@@ -107,6 +108,7 @@ from arm_application.gauge_fsm import (
    descend_iter_budget,
    descend_needed_m,
    descend_progress,
    hole_entry_verdict,
    descend_tol_m,
    is_joint_limit_error,
    pixel_stable,
@@ -141,7 +143,13 @@ from arm_application.gauge_kin import (
 )
 from arm_application.pipeline_node import AssemblyPipeline
 from arm_application.handeye_realsense import resolve_realsense_topics
-from arm_application.site_io import deep_merge, load_site_dict, ros2_root
+from arm_application.site_io import (
    deep_merge,
    load_site_dict,
    overhead_camera,
    resolve_eye_to_hand_file,
    ros2_root,
 )
 # 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2）
 from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point
@@ -166,6 +174,8 @@ _LOCATE_CODE_HINT: Dict[int, str] = {
    5: "（对准迭代用满仍未收敛：检查手眼/相机安装，或放宽 align_tol_m）",
    6: "（工件被工具推走：不是手眼/检测的问题，是悬停时工具尖端已经进了工件"
       " —— 查 gauge_assembly.tool_tip_below_flange_m 与 servo.hole_hover_z_m）",
    8: "（规尖偏移一直没量到：残差看着达标、但用的是**法兰轴线**不是规尖 —— "
       "对的可能是法兰、不是规；摆到能看清规尖再跑。看日志『规尖视觉量取失败』）",
 }
@@ -445,6 +455,12 @@ class GaugeAssembly(AssemblyPipeline):
        # `$?` 判不出成败 —— 自动化里会把"流程失败"记成"成功"。结论文件是唯一
        # 能被脚本/CI 读到的东西，由 run_gauge_assembly.sh 传入并读回。
        self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json")
        # 眼在手外标定产物的**确切路径**，由 launch 按 site.yaml 派生后传入
        # （`<臂号>_<相机>_eye_to_hand.yaml`）。为什么不让节点自己拼：
        # "臂号 = SN 末两位"这条口径只有 export_site_env 一处实现，节点再写一遍
        # 必然漂移；而漂移的症状是"标定做完了、S03 却找不到文件"，回退到 S04 跟踪，
        # 看起来像跟踪的问题。留空则退回下面那串候选名（兼容旧命名/仿真）。
        self.declare_parameter("coarse_eye_to_hand_file", "")
        site = load_site_dict()
        # robots[].gauge_assembly 覆盖全局段 —— 与 servo / paths / perception 同一套规矩。
@@ -599,49 +615,30 @@ class GaugeAssembly(AssemblyPipeline):
    def _d435_ns(self, site: Dict[str, Any]) -> str:
        """手外相机的命名空间：``role: overhead*`` 且**属于当前这台臂**的那一个。
-        为什么必须按 ``arm`` 过滤：仿真那台也有手外相机（``sim_d435``），
+        规则（三级收敛）的**唯一实现**在 ``site_io.overhead_camera``——手眼标定
-        只按 role 扫会同时命中两台，而"命中哪一台"取决于字典顺序 ——
+        脚本也用它取相机名。这里只负责把"配置没区分开"这件事**出声**：静默取第一台
-        同一份配置在仿真/真机之间**错而不报**地互串。
+        会让"标定用的相机"和"推理用的相机"悄悄不是同一台。
        规则（确定性的三级）：
          1. ``arm`` 与当前臂相同的优先（仿真：sim_d435 / 真机若两台各有相机也适用）；
          2. 都没有 ``arm``（= 双臂共用的那台老相机）时回落到它；
          3. 还是多个 → 出声告警并取第一个，让人去配置里区分。
        """
-        cams = [
+        cfg, others = overhead_camera(site, self._robot_dict(site))
-            cfg
+        if others:
-            for cfg in (site.get("cameras") or {}).values()
+            names = ", ".join([str(cfg.get("name") or "?")] + others)
            if isinstance(cfg, dict)
            and str(cfg.get("role") or "").startswith("overhead")
        ]
        if not cams:
            return ""
        arm = str(self._robot_dict(site).get("arm") or "").strip()
        mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
        hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
        if len(hits) > 1:
            names = ", ".join(str(c.get("name") or "?") for c in hits)
            self.get_logger().warn(
-                f"有 {len(hits)} 台手外相机都符合当前臂（{names}）—— 取第一个，"
+                f"有 {len(others) + 1} 台手外相机都符合当前臂（{names}）—— 取第一个，"
                "但请给 site.yaml 的 cameras.* 补上 arm 字段区分，否则选哪台看运气"
            )
-        return str(hits[0].get("name") or "").strip("/")
+        return str(cfg.get("name") or "").strip("/")
    def _eye_to_hand_file(self) -> Optional[Path]:
        """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。
-        路径按**仓库根**解析而不是当前工作目录：相对 CWD 的路径会在"换个目录
+        规则（候选顺序 + 按仓库根解析）只有一份，在 ``site_io.resolve_eye_to_hand_file``。
-        启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
+        这里只负责把 launch 传入的确切路径和当前手外相机名交给它。
        """
-        root = ros2_root()
+        return resolve_eye_to_hand_file(
-        for rel in (
+            ros2_root(),
-            f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml",
+            explicit=str(self.get_parameter("coarse_eye_to_hand_file").value or ""),
-            "handeye/results/eye_to_hand.yaml",
+            camera_name=self.cfg.coarse_camera,
-        ):
+        )
            cand = root / rel
            if cand.is_file():
                return cand
        return None
    def _on_robot_description(self, msg: String) -> None:
        self._robot_description = str(msg.data)
@@ -1455,11 +1452,15 @@ class GaugeAssembly(AssemblyPipeline):
        ns = self.cfg.coarse_camera
        he = self._eye_to_hand_file()
        if he is None:
            expect = str(
                self.get_parameter("coarse_eye_to_hand_file").value or ""
            ).strip() or f"handeye/results/{ns}_eye_to_hand.yaml"
            self.get_logger().warn(
                "S03 需要手外相机的 eye-to-hand 标定文件，现在没有 —— "
                f"按兜底逻辑回 S04 跟踪。\n"
-                f"  相机 {ns} 的标定产物应放在 handeye/results/<名>_eye_to_hand.yaml；\n"
+                f"  应该生成：{expect}\n"
-                f"  做法：bash scripts/run_handeye.sh --mode eye_to_hand（标完再跑本流程）"
+                f"  做法：bash scripts/run_handeye.sh --mode eye_to_hand "
                f"teach/merge/calibrate（标完再跑本流程）"
            )
            return Event.COARSE_FAIL
        if not self._wait_coarse_frame(ns):
@@ -1792,6 +1793,9 @@ class GaugeAssembly(AssemblyPipeline):
        z0 = float(Tee[2, 3])
        step_m = self.cfg.contact_step_mm / 1000.0
        max_depth_m = self.cfg.contact_max_depth_mm / 1000.0
        # 台肩落面所需的下压量。在下面几何自检里算出来；**要留到接触判据那里用**
        # （看 hole_entry_verdict：下压深度远小于它 ⇒ 规没进孔）。
        needed_m: Optional[float] = None
        # ---- 工具几何自检：预算必须是**由几何推出来的**，不能拍 ------------------
        # 判"到底了"的那一下是**台肩落到工件上表面**（不是尖端碰面）。工件上表面
@@ -1920,6 +1924,11 @@ class GaugeAssembly(AssemblyPipeline):
                        f"{ft_ref:+.2f}N 超过 +{self.cfg.contact_axial_delta_n:.1f}N"
                        f"（下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
                    )
                    # 下压这么浅就"接触" ⇒ 尖端顶在工件上表面，没进孔。必须在这里
                    # 拦下：放它进 S08 只会干转几轮、憋住，最后报一句"转轴偏差"
                    # 把所有人引去查工具系。见 hole_entry_verdict 的说明。
                    if self._hole_lost_guard(z0, Tee, needed_m, q, "腕部 FT"):
                        return Event.HOLE_LOST
                    # 必须先卸载再交给 S08 —— 判到接触的这一刻臂上顶着的是**出力上限**
                    # 那个量级的力（实测 200N+，与阈值无关），而规的轴向传力上限
                    # 只有 µ·N≈73N。见 _relieve_after_contact 的完整说明。
@@ -1966,6 +1975,9 @@ class GaugeAssembly(AssemblyPipeline):
                    f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
                )
                # 与腕部 FT 那条同规矩：判到接触时臂上顶着的力远大于阈值，必须先卸载。
                # 同样先过"是不是根本没进孔"这道闸（见 hole_entry_verdict）。
                if self._hole_lost_guard(z0, Tee, needed_m, q, "触觉切向"):
                    return Event.HOLE_LOST
                self._relieve_after_contact(q, Tee, "触觉切向", ft_ref)
                return Event.CONTACT_OK
            if fn - self._fn_rest > self.cfg.contact_safe_delta:
@@ -2002,6 +2014,47 @@ class GaugeAssembly(AssemblyPipeline):
        self._retract(q, Tee, dz_m=0.01)
        return Event.TIMEOUT
    def _hole_lost_guard(
        self,
        z0: float,
        Tee: np.ndarray,
        needed_m: Optional[float],
        q: np.ndarray,
        why: str,
    ) -> bool:
        """S07 判到接触时：下压深度是不是**远小于**几何预期（= 规没进孔）。
        返回 True 表示"确实没进孔"，调用方要抬臂并返回 ``Event.HOLE_LOST``。
        完整来龙去脉在 ``gauge_fsm.hole_entry_verdict`` 的 docstring 里，这里只
        留现场要看的那几句：工件在 S08/S09/S10 会被工具推走，而 k2 的孔位是
        S05 一次性定的 ⇒ 工件一动孔位就失效 ⇒ 规落在孔旁边的平面上。
        不拦的话会干转 5 轮，最后报一句"转轴偏差…先确认工具系有没有装转接件"，
        把所有人引去查工具系（实测 2026-09-28）。
        """
        if needed_m is None:
            # 没算出几何预期（工件上表面 z 没标定且 S05 也没给）—— 没有判据就别猜。
            return False
        descended_m = abs(float(Tee[2, 3]) - z0)
        if hole_entry_verdict(descended_m, needed_m) != ENTRY_HIGH:
            return False
        self.get_logger().error(
            f"S07 接触（{why}）发生在下压 {descended_m * 1000:.2f}mm 处，"
            f"而按几何「台肩落面」需要 {needed_m * 1000:.1f}mm —— 只走了 "
            f"{descended_m / needed_m * 100.0:.0f}%。\n"
            "  **规没有进孔，尖端顶在工件上表面**（正常入孔要压到台肩落面那一下）。\n"
            "  最可能的原因：工件被工具推走了，而本孔的坐标是 **S05 一次性定下**的 ——\n"
            "  工件一动，那个坐标就失效（S06 的「实测残差」发现不了：它比的是\n"
            "  「命令位置 vs 实际位置」，不是「规尖 vs 真实孔位」）。\n"
            "  所以**不要**去查工具系/转接件/力阈值（那几个都是好的）。\n"
            "  正确处置：把臂退开、确认工件的实际位置；能复位就复位工件，然后\n"
            "  从 S05 重新拍照定位（重跑流程即会重新定位）。\n"
            "  若要治根：换孔前复核工件没动（当前只有 S05 阶段有 drag 看门狗）。\n"
            "  抬起并中止，不带着失效的孔位进 S08。"
        )
        self._retract(q, Tee, dz_m=0.01)
        return True
    def _relieve_after_contact(
        self,
        q: np.ndarray,
@@ -79,6 +79,10 @@ class Event(str, Enum):
    CONTACT_OK = "contact_ok"
    FORCE_OVER_LIMIT = "force_over_limit"  # 力超安全上限 → 抬起
    #: S07 判到接触、但**下压深度远小于几何预期** ⇒ 规没进孔，顶在工件上表面。
    #: 单列一个事件是为了让结论与日志都是实话：复用 FORCE_OVER_LIMIT 会把这种
    #: 情况报成"力超安全上限"，而它的力根本没超限 —— 又是一句误导。
    HOLE_LOST = "hole_lost"
    SCREW_DONE = "screw_done"
    SCREW_STOP = "screw_stop"            # 力判据满足：切向力>阈值 且 法向力突增
@@ -158,6 +162,7 @@ TRANSITIONS: Dict[Tuple[State, Event], State] = {
    # --- S07 柔顺接触 ---
    (State.CONTACT, Event.CONTACT_OK): State.SCREW_IN,
    (State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT,
    (State.CONTACT, Event.HOLE_LOST): State.ABORT,
    # --- S08 旋转装配循环 ---
    (State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW,
    (State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW,
@@ -406,6 +411,40 @@ def wrench_live(age_s: Optional[float]) -> bool:
    return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S
 #: 判定"六个分量全为 0"用的容差 [N]/[Nm]。真机 FT 的噪声量级是 1e-2~1e-1，
 #: 所以 1e-6 足够区分"真的是 0（数据没出来）"与"很小但活的读数"。
 WRENCH_ZERO_EPS: float = 1.0e-6
 def wrench_all_zero(
    fx: float,
    fy: float,
    fz: float,
    tx: float,
    ty: float,
    tz: float,
    eps: float = WRENCH_ZERO_EPS,
 ) -> bool:
    """六个分量**同时**为 0（容差 ``eps``）⇒ 这是"数据没出来"的指纹，不是"安静"。
    与 :func:`wrench_live` 的分工（两个都要有，缺一不可）：
    * ``wrench_live`` 只看"够不够新"，**故意不看数值** —— 静止时轴向力本来就接近 0，
      拿数值判活会把"安静的传感器"误判成掉线（2026-09-24 触觉上踩过）。
    * 但一个**带着负载**的腕部 FT 不可能六个分量**同时**恰好为 0：重力、装配预紧、
      传感器零漂至少让某个分量不为 0。所以"全零"是另一种失效 —— **数据源没出数**
      （话题名对、但内容是空的；或 RDK 侧没启用该传感）。
    2026-10-08 真机实测：应用在用的 ``/<sn>/ft_sensor_wrench`` 就是恒 (0,0,0)，
    于是"``abs(fz−基线)>2.5N`` 判接触"的兜底**永远触发不了** —— 而日志里一切正常
    （话题收得到、回调在跑、``wrench_live`` 报"活"），属于最危险的静默失效。
    改用 ``/<sn>/external_wrench_in_tcp`` 后有数（≈负载 54N）即可恢复兜底。
    所以：``wrench_live`` 通过 **且** ``wrench_all_zero`` 为真 ⇒ 判为"数据源是空的"。
    """
    return all(abs(float(v)) <= eps for v in (fx, fy, fz, tx, ty, tz))
 #: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。
 #: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、
 #: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内"
@@ -730,6 +769,58 @@ def descend_budget_m(needed_m: float) -> float:
    return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M)
 #: ``hole_entry_verdict`` 的结论。
 ENTRY_OK = "ok"        # 接触深度与几何预期相符 ⇒ 规是从孔里下去的、台肩落面
 ENTRY_HIGH = "high"    # 早早就接触 ⇒ 尖端顶在工件**上表面**上，根本没进孔
 #: 判"没进孔"的下压深度比例阈值。
 #: 实测两个锚点（2026-09-28，仿真）：
 #:   · 正常入孔：下压 28.6mm / 需要 31.9mm ⇒ 0.90
 #:   · 顶在平面上：下压 0.90mm / 需要 31.9mm ⇒ 0.03
 #: 两者差着一个台肩高度（30mm），取 0.5 时两侧余量都很大（正常那侧要掉到
 #: 一半才会误判）。**不要**为了"更灵敏"把它往 0.9 推 —— 台肩倒角没完全落面、
 #: 板厚公差都会让正常值在 0.85~0.95 之间晃。
 HOLE_ENTRY_MIN_RATIO: float = 0.5
 def hole_entry_verdict(
    descended_m: float,
    needed_m: float,
    *,
    min_ratio: float = HOLE_ENTRY_MIN_RATIO,
 ) -> str:
    """S07 判到接触的那一刻：规是**进了孔**、还是**顶在工件上表面**。
    为什么需要它（2026-09-28 实测，同一命令三跑里坏一跑：DONE / ABORT / DONE）：
      工件在 S08/S09/S10 阶段会被工具推走（仿真里实测被推 20~52mm）。而 k2 的
      孔位是 **S05 一次性定下**的（``_step_hover_k`` 取 ``_hole_xy``），工件一动
      它就失效。于是规落在孔**旁边的平面**上，只下压「悬停间隙」那么点就判到接触。
    不拦的代价，实测完整发生过一次：
      S08 干转 5 轮（切向增量 0.0、法向增量 ±0.0，**一点没进给**）→ 第 6 轮憋住 →
      报出来的是「转轴偏差 7.6° 超过阈值 —— 关节7 轴与工具 Z 不共线，
      先确认工具系是否垂直向下、有没有装转接件」。
      **那句话会把所有人引到错误方向**：真因既不是工具系也不是转接件，是孔位失效。
      而且为这句误导，白烧了 5 轮无效拧入（≈30 秒）和一次憋劲（有把规顶掉的风险）。
    为什么比 ``needed_m`` 而不是比"尖端离面高度"：正常入孔时接触发生在**台肩落面**
    那一刻 ≈ ``needed_m``；顶在平面上时接触发生在**尖端刚碰到面**那一刻，而台肩还在
    尖端上方 30mm。两者差着一个台肩高度，所以用比例判最稳（见上面两个实测锚点）。
    纯函数（R5.1）：判错两个方向都贵 —— 把正常的判成"没进孔"会白扔一整个孔；
    把"没进孔"判成正常，就是上面那 30 秒 + 一次误导。
    ``needed_m <= 0`` 时**不判**（几何本身已经错了，上面那条自检会报），
    返回 ``ENTRY_OK`` 不在这里抢着报第二个错。
    """
    if needed_m <= 0.0:
        return ENTRY_OK
    if float(descended_m) < float(needed_m) * float(min_ratio):
        return ENTRY_HIGH
    return ENTRY_OK
 def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
    """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
@@ -930,6 +1021,7 @@ EVENT_DOC: Dict[Event, str] = {
    Event.ALIGN_OUT: "对位超差",
    Event.CONTACT_OK: "已接触工件面",
    Event.FORCE_OVER_LIMIT: "力超安全上限",
    Event.HOLE_LOST: "孔位失效（规没进孔）",
    Event.SCREW_DONE: "达到目标圈数",
    Event.SCREW_STOP: "力判据满足（咬住螺纹）",
    Event.GAUGE_DROPPED: "通止规掉了",
@@ -126,6 +126,39 @@ def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
    return T
 def cam_to_base_T(
    T_handeye: np.ndarray, T_ee: np.ndarray, mode: str
 ) -> np.ndarray:
    """相机系 → 基座系的 4×4。
    * ``eye_in_hand``：``T_handeye`` 是 cam→法兰，再左乘 ``T_ee``（base→法兰）。
    * ``eye_to_hand``：``T_handeye`` **已经是** cam→基座。再乘 ``T_ee`` 会把
      固定相机错挂到法兰上 —— 定位总能到附近、永远差一截，看起来像标定不准。
    """
    T_he = np.asarray(T_handeye, dtype=float).reshape(4, 4)
    tag = str(mode or "").strip().lower()
    if tag == "eye_to_hand":
        return T_he
    if tag != "eye_in_hand":
        raise ValueError(
            f"手眼模式只能是 eye_in_hand 或 eye_to_hand，收到 {mode!r}"
        )
    return np.asarray(T_ee, dtype=float).reshape(4, 4) @ T_he
 def cam_point_to_base(
    p_cam: np.ndarray,
    T_handeye: np.ndarray,
    T_ee: np.ndarray,
    mode: str,
 ) -> np.ndarray:
    """工件三维点（相机系）→ 基座系 XYZ。"""
    T_bc = cam_to_base_T(T_handeye, T_ee, mode)
    xyz = np.asarray(p_cam, dtype=float).reshape(3)
    ph = np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
    return (T_bc @ ph)[:3]
 def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    Ri = R.T
    ti = -Ri @ t.reshape(3, 1)
@@ -134,6 +134,67 @@ def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None:
    return None
 def _keypoint_holes(
    dets,
    *,
    conf_thr: float,
    min_kconf: float,
    radius_px: float,
    min_dist_px: float,
 ):
    """pose 模型直接给出的 k1/k2 关键点当孔位。
    现场约定（见 ``pose_detector``）：k0 跟踪、k1/k2 对孔。
    返回 ``((k1x,k1y),(k2x,k2y),r,r)``，拿不到可信关键点时返回 None
    （不是 pose 模型 / 关键点置信度不够 / 两点几乎重合），调用方回落到找圆。
    为什么值得优先用关键点：Hough / 亮环那套是在**孔本身的图像特征**上找圆，
    光照、反光、通止规尖伸进视野都会把圆心拉偏；而关键点是模型在训练分布里
    直接回归出来的，对这几类干扰天然更稳。
    """
    if not dets:
        return None
    for det in sorted(dets, key=lambda d: d.conf, reverse=True):
        if float(det.conf) < conf_thr:
            continue
        kpts = getattr(det, "kpts", None)
        if kpts is None:
            continue
        kpts = np.asarray(kpts, dtype=float)
        if kpts.ndim != 2 or kpts.shape[0] < 3 or kpts.shape[1] < 3:
            continue
        k1, k2 = kpts[1].copy(), kpts[2].copy()
        if not (np.isfinite(k1[:2]).all() and np.isfinite(k2[:2]).all()):
            continue
        if float(k1[2]) < min_kconf or float(k2[2]) < min_kconf:
            logger.info(
                "k1/k2 关键点置信度不足（%.2f / %.2f < %.2f）—— 回落到找圆",
                float(k1[2]),
                float(k2[2]),
                min_kconf,
            )
            continue
        dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
        if dist < float(min_dist_px):
            logger.info(
                "k1/k2 两关键点只差 %.1fpx（< %.1fpx）—— 视为不可信，回落到找圆",
                dist,
                min_dist_px,
            )
            continue
        r = float(radius_px)
        # **刻意不按图像 x 交换左右**。
        # Hough 路径的 k1/k2 是"画面左孔 / 右孔"（位置定义），但 pose 模型学出来的
        # k1/k2 是**固定语义**（训练时标注的那个物理孔）。工件只要转个角度，
        # 位置就会翻面；按 x 强制交换会让 `target_hole=k1` 时左时右地指向两个不同
        # 的物理孔 —— 那是最难查的一类现场问题。
        # 下游怎么认这两个点由 `stage2.k1_label` 决定：
        #   hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2，
        #   所以 k1_label="left" ↔ k1 就是模型的 k1（见 stage2_locate_node._snap）。
        return (float(k1[0]), float(k1[1])), (float(k2[0]), float(k2[1])), r, r
    return None
 def _refine_circle_center(
    gray: np.ndarray,
    cx: float,
@@ -466,6 +527,35 @@ def detect_holes(
                gate_block=True,
            )
        # ---- 首选：pose 模型直接给出 k1/k2 关键点（不必再找圆） ----
        if bool(cfg.get("keypoint_enable", True)):
            kp = _keypoint_holes(
                dets,
                conf_thr=conf,
                min_kconf=float(cfg.get("keypoint_min_conf", 0.30)),
                radius_px=float(cfg.get("keypoint_hole_radius_px", 12.0)),
                min_dist_px=float(cfg.get("keypoint_min_dist_px", 15.0)),
            )
            if kp is not None:
                (k1, k2, r1, r2) = kp
                hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
                hole_radii = [float(r1), float(r2)]
                dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
                gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
                draw_img = _render_success(
                    image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
                )
                return {
                    "status": True,
                    "msg": f"pose 关键点 k1/k2，两孔距离 {dist:.1f}px",
                    "hole_points": hole_points,
                    "hole_radii": hole_radii,
                    "bbox": list(bbox),
                    "label": label,
                    "source": "keypoint",
                    "draw_img": draw_img,
                }
        # ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ----
        if bool(cfg.get("insert_pair_enable", True)):
            gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
@@ -21,7 +21,7 @@ DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
 # 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
 # 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
 # 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
-_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx"
+_CANONICAL_HOLE_MODEL = "models/yolo/gongjian0929.onnx"
@dataclass
@@ -143,17 +143,18 @@ class HoleDetector:
    def _load_yolo(self):
        if self._detector is not None:
            return self._detector
-        from arm_application.yolo_detector import YoloDetector, resolve_weights
+        from arm_application.yolo_detector import build_detector, resolve_weights
        weights = resolve_weights(
            self.model_path,
            backend=str(self.cfg.get("yolo_backend") or "auto"),
        )
-        self._detector = YoloDetector(
+        # build_detector 会自己识别 pose 模型（k0/k1/k2），不必在这里分支。
        self._detector = build_detector(
            weights,
            device=str(self.device),
            conf=float(self.cfg.get("conf", 0.25)),
            iou=float(self.cfg.get("iou", 0.7)),
            device=str(self.device),
            imgsz=int(self.cfg.get("imgsz", 640)),
        )
        return self._detector
@@ -62,6 +62,15 @@ class KalmanFilter3D:
        self._predict(dt)
        self._t = t
    def zero_velocity(self) -> None:
        """把速度状态清零、位置保持不动。
        用途：观测疑似假跳变（见 ``feature_jump_guard.FeatureJumpGate``）时本次不
        采信观测，但**不能**让上一次的速度继续把位置外推出去 —— 否则"不动"会变成
        "按旧速度匀速跑掉"，刚好是我们要避免的那件事。
        """
        self.x[3:] = 0.0
    def update(self, z: np.ndarray) -> None:
        """用一次三维位置观测做量测更新。"""
        z = np.asarray(z, dtype=float).reshape(3)
@@ -54,6 +54,21 @@ class OnnxWorkpieceDetector:
        self._iname = inp.name
        shape = [d if isinstance(d, int) and d > 0 else imgsz for d in inp.shape]
        self._in_h, self._in_w = int(shape[-2]), int(shape[-1])
        # 只认 ultralytics end2end 检测输出 ``[1, N, 6]``，否则**当场报错**。
        # 为什么必须在这里硬失败：2026-10-08 真机，把一个 ``task=pose`` 的模型
        # （输出 ``[1, 4+nc+3K, 8400]``）从这条分支解出来，得到的是 12 个垃圾框
        # ——conf 直接是坐标值（最高 70.87）、类名 ``class_73``、退化框 ``y1=y2=0``。
        # 而流水线照跑不误，只在二阶段"看不出工件"上体现出来，排查代价极大。
        # 宁可启动就失败（元规则：静默失效是头号敌人）。
        outs = self._sess.get_outputs()
        tail = outs[0].shape[-1] if (outs and outs[0].shape) else None
        if isinstance(tail, int) and tail != 6:
            raise RuntimeError(
                f"{path.name} 的检测输出最后一维是 {tail}，不是 6 —— 它不是"
                " ultralytics end2end 检测模型（[1,N,6]）。带 k0/k1/k2 关键点的"
                " pose 模型必须交给 pose_detector / build_detector，否则坐标会被"
                "当成置信度解出垃圾框，而且不会报错。"
            )
        self.conf = float(conf)
        self.imgsz = int(imgsz)
        # 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。
@@ -0,0 +1,245 @@
 """D405 主跟踪丢了之后，用顶置 D435 **拍一张** 把工件 / k0 定到基座系。
 为什么单独成模块
 ----------------
 一阶段平时走腕部 D405（眼在手上）。画面里没有 conf≥门槛的目标时，
 不能让伺服继续追 0.12 的假框，也不能把顶视外参再乘一遍法兰
 （那会让一次性定位差几十厘米还看起来合理）。
 判定、取 k0、反投影、臂是否静止 —— 全是纯函数，测试不启 ROS。
 """
 from __future__ import annotations
 from dataclasses import dataclass
 from typing import Iterable, Optional
 import numpy as np
 from arm_application.handeye_lib import cam_point_to_base
 def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
    """画面内最高检测置信度。空列表 / None → ``None``（不是 0：0 是"看到了但很差"）。"""
    if not dets:
        return None
    vals = [float(getattr(d, "conf", float("nan"))) for d in dets]
    vals = [c for c in vals if np.isfinite(c)]
    if not vals:
        return None
    return max(vals)
 def select_oneshot_det(dets: Optional[Iterable], min_conf: float):
    """顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。"""
    if not dets:
        return None
    ok = [
        d
        for d in dets
        if np.isfinite(float(getattr(d, "conf", float("nan"))))
        and float(d.conf) >= float(min_conf)
    ]
    if not ok:
        return None
    return max(ok, key=lambda d: float(d.conf))
 def depth_raw_to_m(raw: Optional[np.ndarray], encoding: str) -> Optional[np.ndarray]:
    """深度图 → 米。16UC1 常见是毫米；用中位数判量纲，不许默认当米。"""
    if raw is None:
        return None
    enc = (encoding or "").lower()
    arr = np.asarray(raw)
    if arr.size == 0:
        return None
    if enc in ("16uc1", "mono16"):
        pos = arr[arr > 0]
        if pos.size < 8:
            return None
        med = float(np.median(pos))
        scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20 else 0.0001)
        out = arr.astype(float) * scale
        out[arr <= 0] = np.nan
        return out
    if enc == "32fc1":
        return arr.astype(float)
    return None
 def map_uv_to_depth(
    u: float, v: float, color_wh: tuple[int, int], depth_wh: tuple[int, int]
 ) -> tuple[float, float]:
    """彩色图像素 → 对齐深度图像素。分辨率相同则原样返回。"""
    cw, ch = int(color_wh[0]), int(color_wh[1])
    dw, dh = int(depth_wh[0]), int(depth_wh[1])
    if cw <= 0 or ch <= 0 or (cw == dw and ch == dh):
        return float(u), float(v)
    return float(u) * (dw / float(cw)), float(v) * (dh / float(ch))
 def wrist_needs_handoff(best_conf: Optional[float], threshold: float) -> bool:
    """腕部这一帧有没有 conf≥门槛的目标。没有 → 该切 D435。
    ``None`` 当没有检测。门槛必须显式传入，不许在这里写死再抄一份到 site.yaml。
    """
    if best_conf is None:
        return True
    try:
        c = float(best_conf)
    except (TypeError, ValueError):
        return True
    if not np.isfinite(c):
        return True
    return c < float(threshold)
 def k0_uv_from_det(
    det,
    *,
    kpt_index: int = 0,
    kpt_min_conf: float = 0.30,
 ) -> Optional[tuple[float, float]]:
    """从 pose 检测取 k0 像素。k0 不可信就返回 None（不许拿框心冒充 k0）。"""
    kpts = getattr(det, "kpts", None)
    if kpts is None:
        return None
    arr = np.asarray(kpts, dtype=float)
    if arr.ndim != 2 or arr.shape[0] <= int(kpt_index) or arr.shape[1] < 3:
        return None
    u, v, kc = (float(x) for x in arr[int(kpt_index)][:3])
    if not (np.isfinite(u) and np.isfinite(v) and np.isfinite(kc)):
        return None
    if kc < float(kpt_min_conf):
        return None
    return u, v
 def workpiece_uv_from_det(
    det,
    *,
    kpt_index: int = 0,
    kpt_min_conf: float = 0.30,
 ) -> tuple[float, float, bool]:
    """优先 k0；没有可信 k0 时退回框心，并标明 ``is_k0=False``。
    顶视经常看不到腕部视角训出来的 k0。退回框心仍能把臂迁到工件上方，
    让 D405 重新看见 k0 —— 比假装"已经是 k0"好。调用方必须看第三个返回值。
    """
    uv = k0_uv_from_det(det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf)
    if uv is not None:
        return uv[0], uv[1], True
    return float(det.cx), float(det.cy), False
 def depth_at_m(
    depth_m: np.ndarray,
    u: float,
    v: float,
    *,
    win: int = 3,
 ) -> Optional[float]:
    """窗口中位数深度 [m]。无效则 None，不许拿 0 去反投影。"""
    if depth_m is None or depth_m.size == 0:
        return None
    h, w = int(depth_m.shape[0]), int(depth_m.shape[1])
    ui, vi = int(round(u)), int(round(v))
    r = max(1, int(win))
    x0, x1 = max(0, ui - r), min(w, ui + r + 1)
    y0, y1 = max(0, vi - r), min(h, vi + r + 1)
    if x1 <= x0 or y1 <= y0:
        return None
    patch = np.asarray(depth_m[y0:y1, x0:x1], dtype=float).reshape(-1)
    patch = patch[np.isfinite(patch) & (patch > 1e-4)]
    if patch.size == 0:
        return None
    return float(np.median(patch))
 def px_to_cam(
    u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float
 ) -> np.ndarray:
    return np.array(
        [(u - cx) * z / fx, (v - cy) * z / fy, z], dtype=float
    )
 def oneshot_k0_base(
    det,
    depth_m: np.ndarray,
    *,
    fx: float,
    fy: float,
    cx: float,
    cy: float,
    T_cam_to_base: np.ndarray,
    min_z_m: float,
    max_z_m: float,
    kpt_index: int = 0,
    kpt_min_conf: float = 0.30,
    min_det_conf: float = 0.5,
    color_wh: Optional[tuple[int, int]] = None,
    depth_wh: Optional[tuple[int, int]] = None,
 ) -> Optional[tuple[np.ndarray, bool]]:
    """D435 一张图 → 基座系 XYZ。失败返回 None。
    外参必须是 **cam→基座**（eye_to_hand）。传入腕部 T 会得到错位置且不报错，
    所以调用方必须自己保证文件对；本函数不再猜。
    反投影用**彩色图**像素 + 彩色图内参；深度只在对齐图上取样。
    """
    if det is None or float(getattr(det, "conf", 0.0)) < float(min_det_conf):
        return None
    u, v, is_k0 = workpiece_uv_from_det(
        det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf
    )
    u_d, v_d = u, v
    if color_wh is not None and depth_wh is not None:
        u_d, v_d = map_uv_to_depth(u, v, color_wh, depth_wh)
    z = depth_at_m(depth_m, u_d, v_d)
    if z is None or z < float(min_z_m) or z > float(max_z_m):
        return None
    p_cam = px_to_cam(u, v, z, fx, fy, cx, cy)
    T_ee = np.eye(4)  # eye_to_hand 不用法兰，占位
    p_base = cam_point_to_base(p_cam, T_cam_to_base, T_ee, "eye_to_hand")
    if not np.all(np.isfinite(p_base)):
        return None
    return np.asarray(p_base, dtype=float).reshape(3), bool(is_k0)
@dataclass
 class StillLock:
    """D405 跟住时，法兰 XY 连续 ``still_sec`` 秒位移不超过 ``still_move_m`` → 锁定。
    顶视一次性迁过去的那段路**不算**锁定：``tracking_ok`` 必须是腕部有效目标。
    大幅一动就重计时，避免"晃一下刚好满 3 秒"误进二阶段。
    """
    still_sec: float
    still_move_m: float
    _t0: Optional[float] = None
    _anchor_xy: Optional[np.ndarray] = None
    def reset(self) -> None:
        self._t0 = None
        self._anchor_xy = None
    def update(
        self,
        p_ee_xy: Optional[np.ndarray],
        now: float,
        *,
        tracking_ok: bool,
    ) -> bool:
        if (not tracking_ok) or p_ee_xy is None:
            self.reset()
            return False
        xy = np.asarray(p_ee_xy, dtype=float).reshape(2)
        if self._anchor_xy is None or self._t0 is None:
            self._anchor_xy = xy.copy()
            self._t0 = float(now)
            return False
        if float(np.linalg.norm(xy - self._anchor_xy)) > float(self.still_move_m):
            self._anchor_xy = xy.copy()
            self._t0 = float(now)
            return False
        return (float(now) - float(self._t0)) >= float(self.still_sec)
@@ -15,6 +15,7 @@ from __future__ import annotations
 import os
 import threading
 import time
 from pathlib import Path
 from typing import Optional
 import numpy as np
@@ -30,26 +31,48 @@ from rclpy.qos import (
    qos_profile_sensor_data,
 )
 from sensor_msgs.msg import CameraInfo, Image
-from std_msgs.msg import Float32MultiArray
+from std_msgs.msg import Float32, Float32MultiArray
 from tf2_ros import TransformBroadcaster
 from arm_application.box_tracker import BoxTracker
 from arm_application.feature_guard import (
    FarReacquireConfirm,
    TrackSlewGuard,
    kpt_plausible,
 )
 from arm_application.geom_ik import load_handeye_T
 from arm_application.handeye_realsense import (
    is_realsense_source,
    resolve_realsense_topics,
 )
 from arm_application.overhead_oneshot import (
    best_det_conf,
    depth_raw_to_m,
    oneshot_k0_base,
    select_oneshot_det,
    wrist_needs_handoff,
 )
 from arm_application.kalman_filter import KalmanFilter3D
-from arm_application.plane_feature import geometric_center_on_plane, workpiece_xyz_in_box
+from arm_application.plane_feature import (
    PlaneFeature,
    geometric_center_on_plane,
    workpiece_xyz_in_box,
 )
 from arm_application.pose_detector import TRACK_KPT_INDEX
 from arm_application.yolo_detector import (
    INFER_BUSY,
    YoloDetector,
    YoloInferProcess,
    build_detector,
    pick_best,
    resolve_weights,
 )
 from arm_application.ros_node import TolerantNode
 # D435 一次性推理用的 seq。必须远离腕部帧序号，结果绝不能当 D405 检测去立体匹配。
 _ONESHOT_SEQ = 2_000_000_000
 def _bgr_copy(msg: Image) -> np.ndarray:
    """返回调用方持有的 BGR uint8 数组（对异步 YOLO 安全；ROS 可能把原消息丢掉）。"""
@@ -76,17 +99,33 @@ class PerceptionNode(TolerantNode):
        self.declare_parameter("image_topic", "")
        self.declare_parameter("camera_info_topic", "")
        self.declare_parameter("depth_topic", "/d405/aligned_depth_to_color/image_raw")
-        self.declare_parameter("weights", "/home/cc/ros2/models/yolo/detect_best_0.onnx")
+        self.declare_parameter("weights", "/home/cc/ros2/models/yolo/gongjian0929.onnx")
        self.declare_parameter("conf", 0.10)
        self.declare_parameter("iou", 0.45)
        self.declare_parameter("imgsz", 640)
        self.declare_parameter("device", "0")
        self.declare_parameter("min_conf", 0.12)
        # 重新捕获门槛（跳到新位置必须达到的置信度）。为什么必须比 min_conf 高得多：
        # 真机实测假框 conf 0.13~0.47，而真 k0 是 0.87（推导见 site.yaml 同名字段）。
        # 旧写法把它接成 min_conf ⇒ 0.15 的远框被原样采信、kf.reset() 直接跳过去。
        self.declare_parameter("reacquire_conf", 0.55)
        # ---- 跟踪点可信性的三道守卫（实现与推导见 feature_guard.py）----
        # 2026-10-08 真机：真 k0（conf 0.87）被一串**高置信度假 k0**（conf 0.53~0.73）
        # 顶掉，1s 内把臂从 home 带到工作盒角落，之后 148s 全是"检测漏检"。
        # 假 k0 挡不住的地方：置信度没有可分区间；它们**贴在画面边上**、**借深度**。
        self.declare_parameter("keypoint_border_px", 6.0)
        self.declare_parameter("track_max_step_m", 0.04)
        self.declare_parameter("reacquire_far_px", 120.0)
        self.declare_parameter("reacquire_confirm_frames", 3)
        self.declare_parameter("vision_hz", 30.0)
        self.declare_parameter("z_ref_m", 0.40)
        self.declare_parameter("jump_px", 280.0)
        self.declare_parameter("jump_z_m", 0.20)
        self.declare_parameter("min_depth_m", 0.04)
        # 特征点的**相机量程下限**：比这更近的深度读数一定来自机械臂自身（爪上夹的
        # 通止规 / 相机座），不是工件。判据是实测两条带（见 site.yaml 同名字段的推导）：
        # 随臂一起动的近带 0.078~0.087m，工件面 0.19~0.29m ⇒ 取 0.14。
        self.declare_parameter("feature_min_range_m", 0.14)
        self.declare_parameter("max_depth_m", 0.55)
        self.declare_parameter("work_z_max_m", 0.50)  # 高于此值 = 孔 / 背景
        self.declare_parameter("object_frame", "tracked_object")
@@ -112,16 +151,41 @@ class PerceptionNode(TolerantNode):
        self.declare_parameter("hold_point_on_miss", True)
        self.declare_parameter("miss_hold_point_frames", 2)
        # plane = 立体表面质心（推荐）；box = YOLO 检测框中心
        # keypoint = pose 模型的 k0 关键点（跟踪孔）；见 pose_detector
        self.declare_parameter("feature_mode", "plane")
        self.declare_parameter("keypoint_index", TRACK_KPT_INDEX)
        self.declare_parameter("keypoint_min_conf", 0.30)
        self.declare_parameter("keypoint_win_px", 6)
        self.declare_parameter("plane_inlier_m", 0.008)
        self.declare_parameter("plane_band_m", 0.06)
        # ---- D405 主跟踪 + D435 一次性（键必须从 site 接入，缺了启动即红）----
        self.declare_parameter("wrist_handoff_conf", -1.0)
        self.declare_parameter("overhead_oneshot_enable", False)
        self.declare_parameter("overhead_min_depth_m", -1.0)
        self.declare_parameter("overhead_max_depth_m", -1.0)
        self.declare_parameter("overhead_oneshot_conf", -1.0)
        self.declare_parameter("overhead_cooldown_s", -1.0)
        self.declare_parameter("overhead_frame_max_age_s", -1.0)
        self.declare_parameter("overhead_handeye_file", "")
        self.declare_parameter("overhead_camera_source", "")
        self.declare_parameter("overhead_camera_frame", "")
        self.declare_parameter("overhead_image_topic", "")
        self.declare_parameter("overhead_info_topic", "")
        self.declare_parameter("overhead_depth_topic", "")
        self.declare_parameter("base_frame", "")
        self.z_ref = float(self.get_parameter("z_ref_m").value)
        self.min_conf = float(self.get_parameter("min_conf").value)
        self.reacquire_conf = float(self.get_parameter("reacquire_conf").value)
        self.jump_px = float(self.get_parameter("jump_px").value)
        self.jump_z_m = float(self.get_parameter("jump_z_m").value)
        self.min_depth = float(self.get_parameter("min_depth_m").value)
        self.max_depth = float(self.get_parameter("max_depth_m").value)
        # 特征点量程下限 = max(解码量程下限, 工具几何下限)：两者取严，不许被调松到
        # 比 min_depth 还小（那会让"工具自己"重新变成合法目标）。
        self.feature_min_range = max(
            self.min_depth, float(self.get_parameter("feature_min_range_m").value)
        )
        self.work_z_max = float(self.get_parameter("work_z_max_m").value)
        self.hold_point_on_miss = bool(self.get_parameter("hold_point_on_miss").value)
        self.object_frame = str(self.get_parameter("object_frame").value)
@@ -140,13 +204,87 @@ class PerceptionNode(TolerantNode):
            1, int(self.get_parameter("miss_hold_point_frames").value)
        )
        self.feature_mode = str(self.get_parameter("feature_mode").value or "plane").strip().lower()
        self.keypoint_index = max(0, int(self.get_parameter("keypoint_index").value))
        self.keypoint_min_conf = float(self.get_parameter("keypoint_min_conf").value)
        self.keypoint_win_px = max(1, int(self.get_parameter("keypoint_win_px").value))
        # 跟踪点三道守卫（单一实现见 feature_guard.py）
        self.keypoint_border_px = float(self.get_parameter("keypoint_border_px").value)
        self.track_max_step_m = float(self.get_parameter("track_max_step_m").value)
        self._slew = TrackSlewGuard(self.track_max_step_m)
        self._far = FarReacquireConfirm(
            float(self.get_parameter("reacquire_far_px").value),
            int(self.get_parameter("reacquire_confirm_frames").value),
        )
        self.plane_inlier_m = float(self.get_parameter("plane_inlier_m").value)
        self.plane_band_m = float(self.get_parameter("plane_band_m").value)
        self.overhead_oneshot_enable = bool(
            self.get_parameter("overhead_oneshot_enable").value
        )
        self._T_oh: Optional[np.ndarray] = None
        self._oh_image: Optional[Image] = None
        self._oh_depth: Optional[Image] = None
        self._oh_image_mono = -1.0
        self._oh_depth_mono = -1.0
        self._oh_fx = self._oh_fy = self._oh_cx = self._oh_cy = 0.0
        self._oh_frame = ""
        self._oneshot_t = -1.0e9
        self._need_oneshot = False
        self._oneshot_inflight = False
        self._oneshot_img: Optional[Image] = None
        self._oneshot_depth: Optional[Image] = None
        if self.overhead_oneshot_enable:
            self.wrist_handoff_conf = float(
                self.get_parameter("wrist_handoff_conf").value
            )
            if self.wrist_handoff_conf < 0.0:
                raise RuntimeError(
                    "overhead_oneshot_enable 时 wrist_handoff_conf 必须由 site.yaml 接入，"
                    "收到哨兵 -1 —— 配置没接线。"
                )
            self.overhead_min_depth = float(
                self.get_parameter("overhead_min_depth_m").value
            )
            self.overhead_max_depth = float(
                self.get_parameter("overhead_max_depth_m").value
            )
            self.overhead_oneshot_conf = float(
                self.get_parameter("overhead_oneshot_conf").value
            )
            self.overhead_cooldown_s = float(
                self.get_parameter("overhead_cooldown_s").value
            )
            self.overhead_frame_max_age_s = float(
                self.get_parameter("overhead_frame_max_age_s").value
            )
            if min(
                self.overhead_min_depth,
                self.overhead_max_depth,
                self.overhead_oneshot_conf,
                self.overhead_cooldown_s,
                self.overhead_frame_max_age_s,
            ) < 0.0:
                raise RuntimeError(
                    "D435 一次性参数仍是哨兵 -1 —— site.yaml 的 perception.overhead 没接到节点。"
                )
            oh_file = str(self.get_parameter("overhead_handeye_file").value or "")
            if not Path(oh_file).is_file():
                raise RuntimeError(
                    f"D435 一次性定位缺眼在手外结果：{oh_file}。"
                    "先跑 bash ~/ros2/scripts/run_handeye.sh --arm 89 --mode eye_to_hand calibrate"
                )
            self._T_oh = load_handeye_T(oh_file)
            self._oh_frame = str(
                self.get_parameter("overhead_camera_frame").value or ""
            )
        self._fx = self._fy = self._cx = self._cy = 0.0
        self._latest_image: Optional[Image] = None
        self._latest_depth: Optional[Image] = None
        self._last_uv: Optional[np.ndarray] = None
        # k0 相对工件检测框的**归一化偏移**（相对框中心、除以框宽高）。k0 掉信时
        # 用它在框上重建 k0 —— 只借"偏移"不借"绝对位置"，跟踪点就不会横移一大截
        #（见 `_keypoint_feature` 的真机反例）。
        self._kpt_box_frac: Optional[np.ndarray] = None
        self._last_good_z: Optional[float] = None
        self._last_plane_xyz: Optional[np.ndarray] = None
        self._last_filt: Optional[np.ndarray] = None
@@ -203,14 +341,16 @@ class PerceptionNode(TolerantNode):
                f"YOLO worker pid={self._yolo_proc._p.pid} weights={self._yolo_proc.weights_path}"
            )
        else:
-            self.detector = YoloDetector(
+            self.detector = build_detector(
                weights,
                backend=self.backend_name,
                conf=float(yolo_cfg["conf"]),
                iou=float(yolo_cfg["iou"]),
                device=str(yolo_cfg["device"]),
                imgsz=int(yolo_cfg["imgsz"]),
            )
-            if self._yolo_half:
+            # PoseDetector 自己管 GPU 张量，没有 ultralytics 的 _model 可挪。
            if self._yolo_half and hasattr(self.detector, "_model"):
                try:
                    import torch
@@ -220,6 +360,8 @@ class PerceptionNode(TolerantNode):
                        self._yolo_half = False
                except Exception:  # noqa: BLE001
                    self._yolo_half = False
            elif self._yolo_half:
                self._yolo_half = False
            try:
                self.detector.warmup(half=self._yolo_half)
            except Exception as exc:  # noqa: BLE001
@@ -276,6 +418,34 @@ class PerceptionNode(TolerantNode):
        self.create_subscription(
            Image, depth_topic, self._on_depth, image_qos, callback_group=self._io_cbg
        )
        self.base_frame = str(self.get_parameter("base_frame").value or "").strip()
        if self.overhead_oneshot_enable:
            oh_img = str(self.get_parameter("overhead_image_topic").value or "").strip()
            oh_info = str(self.get_parameter("overhead_info_topic").value or "").strip()
            oh_dep = str(self.get_parameter("overhead_depth_topic").value or "").strip()
            if not oh_img or not oh_dep:
                raise RuntimeError(
                    "overhead_oneshot_enable 但 overhead_image_topic / "
                    "overhead_depth_topic 为空 —— 配置没接线。"
                )
            self.create_subscription(
                Image, oh_img, self._on_oh_image, image_qos,
                callback_group=self._io_cbg,
            )
            self.create_subscription(
                Image, oh_dep, self._on_oh_depth, image_qos,
                callback_group=self._io_cbg,
            )
            if oh_info:
                self.create_subscription(
                    CameraInfo, oh_info, self._on_oh_info, image_qos,
                    callback_group=self._io_cbg,
                )
            self.get_logger().info(
                f"D435 一次性 image={oh_img} depth={oh_dep} "
                f"handoff_conf={self.wrist_handoff_conf:.2f} "
                f"T={self.get_parameter('overhead_handeye_file').value}"
            )
        point_qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
@@ -285,6 +455,12 @@ class PerceptionNode(TolerantNode):
        self._point_pub = self.create_publisher(
            PointStamped, "visual_servo/filtered_point", point_qos
        )
        self._wrist_conf_pub = self.create_publisher(
            Float32, "visual_servo/wrist_conf", 5
        )
        self._overhead_k0_pub = self.create_publisher(
            PointStamped, "visual_servo/overhead_k0", point_qos
        )
        # RELIABLE + keep_last(5)：相机预览能容忍短暂的 DDS 丢包间隙
        det_qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
@@ -345,6 +521,22 @@ class PerceptionNode(TolerantNode):
    def _on_depth(self, msg: Image) -> None:
        self._latest_depth = msg
    def _on_oh_image(self, msg: Image) -> None:
        self._oh_image = msg
        self._oh_image_mono = time.monotonic()
    def _on_oh_depth(self, msg: Image) -> None:
        self._oh_depth = msg
        self._oh_depth_mono = time.monotonic()
    def _on_oh_info(self, msg: CameraInfo) -> None:
        self._oh_fx = float(msg.k[0])
        self._oh_fy = float(msg.k[4])
        self._oh_cx = float(msg.k[2])
        self._oh_cy = float(msg.k[5])
        if msg.header.frame_id:
            self._oh_frame = str(msg.header.frame_id)
    def _wake_infer(self) -> None:
        if self._latest_image is None:
            return
@@ -377,6 +569,7 @@ class PerceptionNode(TolerantNode):
        try:
            self.detector.restart()
            self._submitted_seq = -1
            self._oneshot_inflight = False
            self._last_gpu_t = time.monotonic()
            self.get_logger().info("YOLO 工作进程已重启")
        except Exception as exc:  # noqa: BLE001
@@ -389,6 +582,8 @@ class PerceptionNode(TolerantNode):
        我们还会**先**提交再去等，这样下一次推理已经排进队列了。
        """
        while not self._stop.is_set():
            # 一次性占用 GPU 时不要再塞 D405，否则两帧挤一条队列，结果会对错图。
            if not self._oneshot_inflight:
                self._submit_latest()
            try:
                got = self.detector.wait_result(timeout=0.030)
@@ -407,8 +602,9 @@ class PerceptionNode(TolerantNode):
                        f"(worker={'存活' if getattr(self.detector, 'alive', True) else '已死'})",
                        throttle_duration_sec=1.0,
                    )
-                outstanding = self._submitted_seq > self._done_seq
+                outstanding = self._submitted_seq > self._done_seq or self._oneshot_inflight
                if outstanding and self._last_gpu_t is not None and silent > 2.0:
                    self._oneshot_inflight = False
                    self._restart_yolo(f"卡死 {silent:.1f}s")
                    continue
                if (
@@ -424,6 +620,16 @@ class PerceptionNode(TolerantNode):
            seq, dets, gpu_ms = got
            if dets is INFER_BUSY:
                continue
            if int(seq) >= _ONESHOT_SEQ:
                try:
                    self._finish_overhead_oneshot(dets, float(self._infer_meta.get(int(seq), 1.0)))
                except Exception as exc:  # noqa: BLE001
                    self.get_logger().error(
                        f"D435 一次性结果处理失败：{exc}", throttle_duration_sec=2.0
                    )
                    self._oneshot_inflight = False
                    self._oneshot_t = time.monotonic()
                continue
            scale = float(self._infer_meta.get(int(seq), 1.0))
            t_img = float(self._frame_t.get(int(seq), time.monotonic()))
            t0 = time.monotonic()
@@ -432,6 +638,9 @@ class PerceptionNode(TolerantNode):
            except Exception as exc:  # noqa: BLE001
                self.get_logger().error(f"应用检测结果出错：{exc}", throttle_duration_sec=2.0)
            self._done_seq = int(seq)
            if self._need_oneshot and not self._oneshot_inflight:
                self._submit_overhead_oneshot()
            else:
                self._submit_latest()
            dt_ms = (time.monotonic() - t0) * 1000.0
            lag_ms = (time.monotonic() - t_img) * 1000.0
@@ -490,7 +699,7 @@ class PerceptionNode(TolerantNode):
        else:
            meters = roi
            scale = 1.0
-        vals = meters[np.isfinite(meters) & (meters > self.min_depth) & (meters < self.max_depth)]
+        vals = meters[np.isfinite(meters) & (meters > self.feature_min_range) & (meters < self.max_depth)]
        if vals.size < 8:
            # 内部 ROI 经常落在孔上；改用整个检测框重试。
            y0 = int(np.clip(y1, 0, h - 1))
@@ -506,7 +715,7 @@ class PerceptionNode(TolerantNode):
                meters2 = pos2 * scale
            else:
                meters2 = roi2
-            vals = meters2[np.isfinite(meters2) & (meters2 > self.min_depth) & (meters2 < self.max_depth)]
+            vals = meters2[np.isfinite(meters2) & (meters2 > self.feature_min_range) & (meters2 < self.max_depth)]
            if vals.size < 8:
                med_m = float(np.median(meters)) if meters.size else 0.0
                return None, f"ok={vals.size} scale={scale} med={med_m:.3f}"
@@ -634,9 +843,16 @@ class PerceptionNode(TolerantNode):
        self._box_ema = None
        self._last_raw_xyxy = None
        self._last_uv = None
        self._kpt_box_frac = None
        self._last_filt = None
        self._last_plane_xyz = None
        self._box_trk.reset()
        # 两道有状态守卫跟着"真正丢目标"一起冷启动。**只在长丢时清**，不在单帧
        # 漏检时清：单帧漏检极常见（真机日志里几乎每隔一两帧就一次），每漏一帧
        # 就把锚点清空，会让下一帧的任何观测都变成"首次观测"而被 TrackSlewGuard
        # 无条件放行 —— 借深度的假点于是每次都能重新锚定，守卫形同虚设。
        self._slew.reset()
        self._far.reset()
        self._last_yolo_t = None
        self._publish_detection_clear()
@@ -645,6 +861,13 @@ class PerceptionNode(TolerantNode):
        if scale != 1.0:
            for d in dets:
                d.xyxy = d.xyxy * scale
        best_conf = best_det_conf(dets)
        self._publish_wrist_conf(best_conf)
        handoff = bool(
            self.overhead_oneshot_enable
            and wrist_needs_handoff(best_conf, self.wrist_handoff_conf)
        )
        self._need_oneshot = handoff
        now = time.monotonic()
        pred_at_img = self._box_trk.predict(t_img, self._img_wh())
        with self._lock:
@@ -656,12 +879,16 @@ class PerceptionNode(TolerantNode):
            prev_uv=prev_uv,
            prev_xyxy=prev_xyxy,
            max_track_px=max(400.0, self.jump_px),
-            reacquire_conf=self.min_conf,
+            reacquire_conf=self.reacquire_conf,
            min_conf=self.min_conf,
            lock_iou=0.05,
        )
        if best is None:
            self._miss_count += 1
            # 单帧漏检**不清**守卫锚点（见 _clear_track 的注释）：真机 2026-10-08
            # 漏检几乎每帧都有，一漏就 reset ⇒ 每帧都当"首次观测" ⇒ 借深度的假 k0
            # 次次都能重新锚定。真正丢目标由下面的 miss_clear_frames 长丢分支
            # 走 _clear_track（连丢 miss_clear_frames 帧才冷启动）。
            if self._miss_count >= self.miss_clear_frames:
                self._clear_track()
                self._last_yolo_t = None
@@ -676,6 +903,7 @@ class PerceptionNode(TolerantNode):
                if (
                    self.hold_point_on_miss
                    and self._miss_count <= self.miss_hold_point_frames
                    and not handoff
                ):
                    self._republish_held_point()
            return
@@ -708,12 +936,21 @@ class PerceptionNode(TolerantNode):
        # 悬停目标 = 检测框内工件表面的立体三维点（不是穿过孔 / 桌面的射线，
        # 也不是 z_ref）。红点 = 该三维点。
        # feature_mode=keypoint 时改用 pose 模型的 k0（跟踪孔）当跟踪点。
        feat = None
        p_cam: Optional[np.ndarray] = None
        note = "box"
        z_meas = None
        uv = box_uv
-        if self.feature_mode != "box":
+        # 本帧的深度是不是**借**来的（见 PlaneFeature.borrowed）—— 给"每帧位移上限"
        # 守卫用：借来的深度意味着该像素读不到自己的深度，绝对位置不足以重定位。
        borrowed = False
        if self.feature_mode == "keypoint":
            feat = self._keypoint_feature(best)
            # k0 掉信时 `_keypoint_feature` 会**按 k0 相对框的偏移**在框上重建跟踪点
            # （位置连续、不横移）。只有冷启动、连一次可信 k0 都没见过时才返回 None，
            # 此时才退到"框内深度 + 框中心"。
        elif self.feature_mode != "box":
            feat = self._workpiece_xyz(xyxy)
            if feat is None:
                feat = self._plane_center(xyxy)
@@ -722,6 +959,7 @@ class PerceptionNode(TolerantNode):
            uv = np.asarray(feat.uv, dtype=float).reshape(2)
            z_meas = float(p_cam[2])
            note = feat.note
            borrowed = bool(getattr(feat, "borrowed", False))
        else:
            z_meas, note = self._depth_in_box(xyxy)
            note = f"{note} box_z"
@@ -746,6 +984,24 @@ class PerceptionNode(TolerantNode):
        self._publish_detection_box(xyxy, float(best.conf), float(best.cls_id), uv, float(z))
        # ---- 守卫二：跟踪点的**每帧位移上限** ----
        # 工件不动、相机 30Hz，真实观测一帧走不了 4cm（实测真实运动 ≤15mm/帧）。
        # 观测可信 ⇒ 夹到上限限速跟随（不丢目标）；观测是**借深度**的 ⇒ 超上限整帧
        # 不采信（2026-10-08 真机，假 k0 带着借来的深度、z 看着很合理，1s 把臂带走
        # 400mm）。判不采信时**不发跟踪点**，让伺服按"目标丢失"处理（原地不动）。
        step, why_step = self._slew.update(
            np.asarray(p_cam, dtype=float).reshape(3)[:2], borrowed=borrowed
        )
        if step is None:
            self.get_logger().warn(
                f"跟踪点判误检，本帧不发点（{why_step}）"
                f" uv=({uv[0]:.0f},{uv[1]:.0f}) conf={best.conf:.2f}",
                throttle_duration_sec=0.5,
            )
            return
        p_cam = np.asarray(p_cam, dtype=float).reshape(3).copy()
        p_cam[0], p_cam[1] = float(step[0]), float(step[1])
        with self._lock:
            self.kf.advance(now)
            if self.kf.initialized:
@@ -753,6 +1009,21 @@ class PerceptionNode(TolerantNode):
                dpx = float(np.linalg.norm(box_uv - self._cam_to_pixel(pred)))
                dz = abs(float(p_cam[2] - pred[2])) if z_ok else 0.0
                if dpx > self.jump_px or (z_ok and dz > self.jump_z_m):
                    # ---- 守卫三：远框"重新捕获"要多帧确认 ----
                    # 一帧的远框更可能是模型在别的东西上误报，而不是工件真被挪走了
                    #（真机那串假框的 dpx 是 480/418/207，而且每帧换个地方）。
                    ok_far, why_far = self._far.allow(
                        None if self.feature_mode != "keypoint" else tuple(uv),
                        dpx,
                        near_px=self.reacquire_far_px,
                    )
                    if not ok_far:
                        self.get_logger().warn(
                            f"重新捕获未确认（{why_far}）conf={best.conf:.2f} —— "
                            "本帧不发点，不追这一帧的新位置",
                            throttle_duration_sec=0.5,
                        )
                        return
                    self.kf.reset(p_cam, now)
                    self.get_logger().info(
                        f"重新捕获 conf={best.conf:.2f} dpx={dpx:.0f}",
@@ -774,6 +1045,9 @@ class PerceptionNode(TolerantNode):
            filt[2] = z_use
            self._last_filt = filt.copy()
        if handoff:
            # 画面里没有 conf≥门槛的目标：绿框可以留，三维点不许发 —— 伺服会拿去追。
            return
        self._publish_point(filt, stamp)
        self.get_logger().info(
            f"跟踪 {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
@@ -791,6 +1065,191 @@ class PerceptionNode(TolerantNode):
            return True
        return age < 0.25
    def _keypoint_feature(self, det):
        """用 pose 模型的 ``keypoint_index``（默认 k0）当跟踪点。
        约定：k0 是跟踪孔，k1/k2 是对孔（见 ``pose_detector``）。
        返回 ``PlaneFeature``，坐标系 / 语义和 plane 模式完全一致 —— 下游
        （卡尔曼、visual_servo）不需要知道这是关键点。
        k0 掉信（置信度低 / 该处没深度）时**不许把跟踪点横移到框中心** ——
        那是另一个点，会瞬间平移一大截。正确做法是用 k0 相对工件框的**归一化偏移**
        在框上重建 k0：位置连续，工件动时还能跟着动。
        真机反例（2026-10-08，一阶段跑 150s 没锁上，arm 89）：
            0659.551  死区保持 |e_xy|=3.0mm      ← 其实已经锁定（误差 3mm）
            0659.598  k0 置信度 0.01 < 0.30，本帧不用它
            0659.604  feat_base=(0.675,-0.099)   ← 兜底切到框中心，跟踪点横移 89mm
            0660.121  v=(-0.206,+0.400)          ← 伺服以 0.45m/s 去追这个假点
            0660.561  过期保持 —— 不跟踪冻结的检测框  ← 画面糊掉，彻底丢失
        只有在"连一次可信的 k0 都没见过"（``_kpt_box_frac`` 为空）时才返回 None，
        交给调用方回落到框中心 —— 那是冷启动，没有更好的信息可用。
        """
        kpts = getattr(det, "kpts", None)
        if kpts is None:
            return None
        kpts = np.asarray(kpts, dtype=float)
        if kpts.ndim != 2 or kpts.shape[0] <= self.keypoint_index or kpts.shape[1] < 3:
            return None
        u, v, kc = (float(x) for x in kpts[self.keypoint_index][:3])
        # 工件框（用来把 k0 的偏移归一化，以及掉信时重建）
        box = getattr(det, "xyxy", None)
        box = None if box is None else np.asarray(box, dtype=float).reshape(4)
        cx = cy = bw = bh = None
        if box is not None:
            cx = 0.5 * (box[0] + box[2])
            cy = 0.5 * (box[1] + box[3])
            bw = abs(box[2] - box[0])
            bh = abs(box[3] - box[1])
        finite = bool(np.isfinite(u) and np.isfinite(v) and np.isfinite(kc))
        # k0 的**像素位置**是否可信：置信度够 ≠ 位置对。2026-10-08 真机，模型在
        # uv=(12,30)/(76,75)/(143,92) 这些**贴画面边**的地方以 conf 0.53~0.73 报 k0
        #（真 k0 是 0.87，没有可分区间）—— 孔心长在工件上，不可能在画面边上。
        # 位置不可信不是"没有目标"，而是"本帧 k0 位置不能用"，交回下面的连续性路径。
        why_pos = (
            kpt_plausible(
                u, v, box, *self._img_wh_or_zero(), border_px=self.keypoint_border_px
            )
            if finite
            else None
        )
        borrowed = False
        if finite and kc >= self.keypoint_min_conf and why_pos is None:
            # k0 可信：位置就用 k0 自己
            uv = np.array([u, v], dtype=float)
            z, znote = self._depth_at_uv(u, v)
            if z is None:
                # 位置可信、只是深度读不到：XY 仍然用 k0（不许横移），只借深度
                if box is None:
                    return None
                z, note2 = self._depth_in_box(box)
                if z is None:
                    return None
                znote = f"{note2} 借深度"
                borrowed = True
            if box is not None and bw is not None and bw > 1e-6 and bh > 1e-6:
                self._kpt_box_frac = np.array(
                    [(u - cx) / bw, (v - cy) / bh], dtype=float
                )
            reason = ""
        else:
            # k0 不可信：按"相对框的偏移"重建，位置连续、且跟着框走。
            if not finite:
                reason = "非有限值"
            elif why_pos is not None:
                # 置信度够但**位置**不合理（贴画面边 / 不在自己的框里）
                reason = why_pos
            else:
                reason = f"置信度 {kc:.2f} < {self.keypoint_min_conf:.2f}"
            if (
                self._kpt_box_frac is None
                or box is None
                or bw is None
                or bw <= 1e-6
                or bh is None
                or bh <= 1e-6
            ):
                self.get_logger().warn(
                    f"k{self.keypoint_index} {reason}，且还没有可信历史 —— 本帧不用它",
                    throttle_duration_sec=1.0,
                )
                return None
            uv = np.array(
                [cx + float(self._kpt_box_frac[0]) * bw,
                 cy + float(self._kpt_box_frac[1]) * bh],
                dtype=float,
            )
            z, znote = self._depth_at_uv(float(uv[0]), float(uv[1]))
            if z is None:
                z, note2 = self._depth_in_box(box)
                if z is None:
                    return None
                znote = f"{note2} 借深度"
                borrowed = True
            self.get_logger().warn(
                f"k{self.keypoint_index} {reason} —— 按框上相对偏移重建跟踪点"
                f"（uv=({uv[0]:.0f},{uv[1]:.0f})，框心=({cx:.0f},{cy:.0f})），不横移",
                throttle_duration_sec=1.0,
            )
            z = float(z)
            return PlaneFeature(
                uv=uv,
                z=z,
                xyz=self._pixel_to_cam(float(uv[0]), float(uv[1]), z),
                n_inliers=1,
                note=f"k{self.keypoint_index}保持({reason}) {znote} z={z:.3f}",
                borrowed=borrowed,
            )
        z = float(z)
        return PlaneFeature(
            uv=uv,
            z=z,
            xyz=self._pixel_to_cam(float(uv[0]), float(uv[1]), z),
            # 关键点特征没有"平面内点"的概念——支撑样本就它自己一个。
            # 这个字段目前没有下游消费者，给 1 是为了让日志里 n=1 说得通。
            n_inliers=1,
            note=f"k{self.keypoint_index}({znote}) z={z:.3f}",
            borrowed=borrowed,
        )
    def _img_wh_or_zero(self) -> tuple:
        """当前彩色图 ``(w,h)``；还没收到图时给 ``(0,0)``（守卫会跳过边缘判据）。"""
        wh = self._img_wh()
        return (0, 0) if wh is None else (int(wh[0]), int(wh[1]))
    def _depth_at_uv(self, u: float, v: float) -> tuple:
        """取 (u,v) 附近小窗口的中位深度（米）。``u,v`` 是**彩色图**像素。
        深度是 aligned-to-color，但分辨率不一定等于彩色图（D405 的 aligned
        深度会和彩色图不同尺寸），所以先按比例映射到深度图坐标 —— 这点和
        ``_depth_in_box`` 里的缩放保持一致，否则关键点深度会整体偏。
        """
        msg = self._latest_depth
        if msg is None:
            return None, "无深度"
        if not self._depth_fresh(msg):
            return None, "深度过期"
        buf = self._decode_depth_m(msg)
        if buf is None:
            return None, "深度无效"
        h, w = buf.shape
        img = self._latest_image
        if img is not None and (int(img.width) != w or int(img.height) != h):
            u = u * (w / float(img.width))
            v = v * (h / float(img.height))
        win = self.keypoint_win_px
        x0 = int(np.clip(round(u) - win, 0, w - 1))
        x1 = int(np.clip(round(u) + win, x0 + 1, w))
        y0 = int(np.clip(round(v) - win, 0, h - 1))
        y1 = int(np.clip(round(v) + win, y0 + 1, h))
        roi = buf[y0:y1, x0:x1].reshape(-1).astype(np.float32)
        if roi.size == 0:
            return None, "窗口越界"
        enc = (msg.encoding or "").lower()
        if enc in ("16uc1", "mono16"):
            pos = roi[roi > 0]
            if pos.size < 4:
                return None, "该处无有效深度"
            med = float(np.median(pos))
            # 和 _depth_in_box 同一套量纲判断（有的相机给 mm，有的给 m）
            scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20 else 0.0001)
            z = med * scale
        else:
            vals = roi[np.isfinite(roi) & (roi > 0)]
            if vals.size < 4:
                return None, "该处无有效深度"
            z = float(np.median(vals))
        if not (self.feature_min_range < z < self.max_depth):
            # 太近 ⇒ 十有八九是爪上夹的通止规（规尖在相机前 ~0.10m，工件面 0.17~0.22m）。
            # 当"这里没有目标"处理，绝不把它当工件去伺服 —— 否则臂会去追自己的工具。
            return None, f"深度 {z:.3f}m 命中工具量程(<{self.feature_min_range:.2f}m)"
        return z, f"({u:.0f},{v:.0f})"
    def _workpiece_xyz(self, xyxy):
        msg = self._latest_depth
        img = self._latest_image
@@ -811,7 +1270,7 @@ class PerceptionNode(TolerantNode):
                self._cx,
                self._cy,
                (int(img.width), int(img.height)),
-                min_depth=self.min_depth,
+                min_depth=self.feature_min_range,  # 太近=爪上工具，见 feature_min_range_m
                max_depth=self.max_depth,
                work_z_max=self.work_z_max,
                band_m=self.plane_band_m,
@@ -840,7 +1299,7 @@ class PerceptionNode(TolerantNode):
                self._cx,
                self._cy,
                (int(img.width), int(img.height)),
-                min_depth=self.min_depth,
+                min_depth=self.feature_min_range,  # 太近=爪上工具，见 feature_min_range_m
                max_depth=self.max_depth,
                work_z_max=self.work_z_max,
                inlier_m=self.plane_inlier_m,
@@ -862,6 +1321,139 @@ class PerceptionNode(TolerantNode):
            return float(self._last_good_z), f"{note} hold_near"
        return None, f"{note} no_depth"
    def _publish_wrist_conf(self, best_conf: Optional[float]) -> None:
        msg = Float32()
        msg.data = float("nan") if best_conf is None else float(best_conf)
        self._wrist_conf_pub.publish(msg)
    def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None:
        pt = PointStamped()
        pt.header.stamp = self.get_clock().now().to_msg()
        pt.header.frame_id = self.base_frame or "base"
        pt.point.x = float(p_base[0])
        pt.point.y = float(p_base[1])
        pt.point.z = float(p_base[2])
        self._overhead_k0_pub.publish(pt)
        del is_k0  # 语义写在日志里；话题保持 PointStamped，避免再抄一份类型
    def _submit_overhead_oneshot(self) -> None:
        """把最新 D435 帧排进同一条 YOLO 队列。不在这里等结果 —— 等会把 D405 卡死。"""
        if self._oneshot_inflight or not self.overhead_oneshot_enable:
            return
        now = time.monotonic()
        if now - self._oneshot_t < self.overhead_cooldown_s:
            return
        img = self._oh_image
        depth = self._oh_depth
        if img is None or depth is None:
            self.get_logger().warn(
                "D405 无合格目标，但 D435 还没出图 —— 不编造点",
                throttle_duration_sec=1.0,
            )
            return
        if (now - self._oh_image_mono) > self.overhead_frame_max_age_s:
            self.get_logger().warn(
                f"D435 彩图过期 {(now - self._oh_image_mono)*1000:.0f}ms —— 不反投影",
                throttle_duration_sec=1.0,
            )
            return
        if (now - self._oh_depth_mono) > self.overhead_frame_max_age_s:
            self.get_logger().warn(
                f"D435 深度过期 {(now - self._oh_depth_mono)*1000:.0f}ms —— 不反投影",
                throttle_duration_sec=1.0,
            )
            return
        if self._oh_fx <= 1.0:
            self.get_logger().warn(
                "D435 内参还没到（camera_info）—— 不反投影",
                throttle_duration_sec=1.0,
            )
            return
        if self._T_oh is None:
            self.get_logger().error("眼在手外 T 没加载 —— 一次性定位拒绝")
            return
        try:
            bgr = _bgr_copy(img)
            small, scale = self._prepare_infer(bgr)
            self._infer_meta[_ONESHOT_SEQ] = scale
            if not self.detector.submit(small, _ONESHOT_SEQ):
                return
        except Exception as exc:  # noqa: BLE001
            self.get_logger().warn(
                f"D435 一次性提交失败：{exc}", throttle_duration_sec=2.0
            )
            return
        self._oneshot_inflight = True
        self._oneshot_img = img
        self._oneshot_depth = depth
        self.get_logger().info("D435 一次性已提交（不打断 D405 队列）")
    def _finish_overhead_oneshot(self, dets, scale: float) -> None:
        """YOLO 返回的顶视结果 → 基座系 K0。失败不发假点。"""
        self._oneshot_inflight = False
        now = time.monotonic()
        self._oneshot_t = now
        img = self._oneshot_img
        depth = self._oneshot_depth
        self._oneshot_img = None
        self._oneshot_depth = None
        if dets is INFER_BUSY or dets is None or img is None or depth is None:
            return
        if scale != 1.0:
            for d in dets:
                d.xyxy = d.xyxy * scale
                if getattr(d, "kpts", None) is not None:
                    d.kpts = np.asarray(d.kpts, dtype=float).copy()
                    d.kpts[:, :2] *= scale
        det = select_oneshot_det(dets, self.overhead_oneshot_conf)
        if det is None:
            self._oneshot_t = now
            self.get_logger().warn(
                f"D435 一次性：画面里没有 conf≥{self.overhead_oneshot_conf:.2f} 的工件",
                throttle_duration_sec=1.0,
            )
            return
        raw = self._decode_depth_m(depth)
        depth_m = depth_raw_to_m(raw, depth.encoding or "")
        if depth_m is None:
            self._oneshot_t = now
            self.get_logger().warn(
                "D435 深度无法转成米 —— 不发点", throttle_duration_sec=1.0
            )
            return
        color_wh = (int(img.width), int(img.height))
        depth_wh = (int(depth_m.shape[1]), int(depth_m.shape[0]))
        got = oneshot_k0_base(
            det,
            depth_m,
            fx=self._oh_fx,
            fy=self._oh_fy,
            cx=self._oh_cx,
            cy=self._oh_cy,
            T_cam_to_base=self._T_oh,
            min_z_m=self.overhead_min_depth,
            max_z_m=self.overhead_max_depth,
            kpt_index=self.keypoint_index,
            kpt_min_conf=self.keypoint_min_conf,
            min_det_conf=self.overhead_oneshot_conf,
            color_wh=color_wh,
            depth_wh=depth_wh,
        )
        self._oneshot_t = now
        if got is None:
            self.get_logger().warn(
                "D435 一次性：深度或 k0 不可用，不发点",
                throttle_duration_sec=1.0,
            )
            return
        p_base, is_k0 = got
        self._publish_overhead_k0(p_base, is_k0)
        self.get_logger().info(
            f"D435 一次性定位 {'k0' if is_k0 else '框心（顶视看不见 k0）'} "
            f"base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
            f"conf={float(det.conf):.2f}"
        )
    def _publish_point(self, filt: np.ndarray, stamp) -> None:
        pt = PointStamped()
        pt.header.stamp = stamp
@@ -205,7 +205,6 @@ class AssemblyPipeline(Stage3Search):
            "enable_control:=true",
            "publish_handeye_tf:=false",
            f"ee_z_m:={lock_z:.3f}",
            "lock_hold_sec:=0.6",
            f"use_sim_time:={str(use_sim).lower()}",
        ]
        self.get_logger().info(
@@ -21,6 +21,11 @@ class PlaneFeature:
    xyz: np.ndarray  # (3,) 相机坐标系下的立体点云质心
    n_inliers: int
    note: str
    # 这个点的 Z 是不是**借**来的（该像素处读不到自己的深度，从检测框里借了一个）。
    # 为什么要显式带出来、而不是让下游去认 ``note`` 里的字：借深度意味着该像素的
    # 深度不可读 ⇒ 绝对位置不足以用来**重定位**（2026-10-08 真机，假 k0 带着借来的
    # 深度、z 看着很合理 0.27~0.39m，于是"物理量程"守卫也失效）。
    borrowed: bool = False
 def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]:
@@ -0,0 +1,479 @@
 #!/usr/bin/env python3
 """YOLO **pose** 模型的推理封装（自解码，不经过 ultralytics 的 pose 后处理）。
 背景：为什么会多出这一个模块
 --------------------------------
 `gongjian0929` 这类权重是 ``task=pose``：它输出**一个工件框 + K 个关键点**。
 本项目 K=3，就是现场说的 k0 / k1 / k2（k0 跟踪、k1/k2 对孔）。
 ultralytics 走不通，两条路都实测过（2026-09-29）：
 1. ``YOLO(engine, task="detect")`` —— 按检测去解 14 通道输出，结果吐出 300 个
   垃圾框，``conf`` 直接是坐标值（600+），全被算成 ``workpiece`` 类。
   跟踪会锁到空气上 —— 真机上这比不跑更危险。
 2. ``YOLO(engine, task="pose")`` —— 直接崩：``negative dimension -983``。
   根因是 ``scripts/weights.py`` 用**裸 TensorRT API** 构建 engine，engine 不带
   metadata；ultralytics 拿不到 ``kpt_shape``，后处理就错位了。
   （TensorRT 10.13 的 Python API 确实没有任何 metadata 接口 —— 见
   ``models/README.md`` 第 3 节。）
 所以这里自己解码。输出布局是 YOLO pose 的固定约定::
    output0 : (1, 4 + nc + 3*K, A)      # A = anchor 数（640 输入时是 8400）
      ch 0..3 : cx, cy, w, h            # 输入尺度下的像素
      ch 4..  : 每类置信度（nc 个）
      之后    : K 组 (x, y, conf)
 关键点是**同一个工件框的属性**，不是独立目标 —— 所以它们不参与 NMS 的竞争，
 只跟着自己那个框一起被选出/淘汰。这点和"每个孔一个类"的检测模型不一样。
 后端
 ----
 - ``.engine``：TensorRT 10 的 ``set_tensor_address`` + ``execute_async_v3``。
  用 torch 张量当设备缓冲、torch 的当前流当执行流 —— 不需要 pycuda。
  实测 ~1.5 ms/帧（640 输入，FP16 engine）。
 - ``.onnx``：onnxruntime。**本环境没有 CUDA provider**（只有 CPU），
  所以 ONNX 只是兜底，真机请用 engine。
 """
 from __future__ import annotations
 import ast
 import logging
 from pathlib import Path
 from typing import Any, List, Optional, Sequence, Tuple, Union
 import cv2
 import numpy as np
 from .yolo_detector import Detection, resolve_class_labels
 logger = logging.getLogger(__name__)
 # ultralytics 的默认填充值。改了它并不会错 —— 只是输入分布和训练时不一致，
 # 精度会掉一点，所以别改。
 LETTERBOX_FILL = 114
 # 关键点语义（现场约定）：k0 是跟踪孔，k1/k2 是对孔。
 # 默认值写在这里，site.yaml 可以覆盖（见 arm_application/pose_detector 的调用方）。
 TRACK_KPT_INDEX = 0
 HOLE_KPT_INDICES: Tuple[int, int] = (1, 2)
 # --------------------------------------------------------------------------- #
 # 模型信息：engine 没有 metadata，就从旁边同名的 .onnx 读
 # --------------------------------------------------------------------------- #
 def _parse_python_literal(raw: Any, default: Any = None) -> Any:
    """ONNX metadata 里存的是 python repr（单引号、嵌套 list/dict），要 literal_eval。"""
    if raw is None:
        return default
    if isinstance(raw, (dict, list, tuple, int)):
        return raw
    try:
        return ast.literal_eval(str(raw))
    except (ValueError, SyntaxError):
        return default
 def read_pose_info(weights: Union[str, Path]) -> Optional[dict]:
    """读同名 ``.onnx`` 的 metadata，判断这是不是一个 pose 模型。
    engine 本身读不到任何东西（TensorRT 10 的 Python API 没有 metadata 接口），
    但 ``.onnx`` 一定在它旁边（``resolve_weights`` 保证同目录同主干），
    所以统一以 ONNX 为准。
    返回 ``None`` = 不是 pose 模型（调用方应回落到普通检测器）。
    返回 dict 时至少含 ``kpt_count`` / ``nc`` / ``names``。
    """
    p = Path(weights).expanduser()
    onnx_path = p.with_suffix(".onnx")
    if not onnx_path.is_file():
        return None
    try:
        import onnx
    except Exception:  # noqa: BLE001
        logger.warning("没装 onnx，无法判断 %s 是不是 pose 模型", p.name)
        return None
    try:
        meta = {m.key: m.value for m in onnx.load(str(onnx_path)).metadata_props}
    except Exception as exc:  # noqa: BLE001
        logger.warning("读 %s 的 metadata 失败：%s", onnx_path.name, exc)
        return None
    task = str(meta.get("task", "")).strip().lower()
    kpt_shape = _parse_python_literal(meta.get("kpt_shape"))
    if task != "pose" and not kpt_shape:
        return None
    kpt_count = 0
    if isinstance(kpt_shape, (list, tuple)) and len(kpt_shape) >= 1:
        try:
            kpt_count = int(kpt_shape[0])
        except (TypeError, ValueError):
            kpt_count = 0
    if kpt_count <= 0:
        # metadata 缺 kpt_shape 也不能猜 —— 猜错就是"静默错位"，比报错难查得多。
        logger.warning("%s 标记为 pose 但读不到 kpt_shape，按非 pose 处理", onnx_path.name)
        return None
    names = _parse_python_literal(meta.get("names"), default={}) or {}
    kpt_names = _parse_python_literal(meta.get("kpt_names"), default={}) or {}
    nc = 0
    if isinstance(names, dict) and names:
        nc = max(int(k) for k in names) + 1 if all(
            str(k).lstrip("-").isdigit() for k in names
        ) else len(names)
    if nc <= 0 and isinstance(names, (list, tuple)):
        nc = len(names)
    return {
        "task": "pose",
        "kpt_count": kpt_count,
        "kpt_names": kpt_names,
        "nc": nc,
        "names": names,
        "model": str(meta.get("description") or p.name),
    }
 def is_pose_model(weights: Union[str, Path]) -> bool:
    return read_pose_info(weights) is not None
 # --------------------------------------------------------------------------- #
 # 预处理 / 后处理（纯 numpy，可离线单测）
 # --------------------------------------------------------------------------- #
 def letterbox(
    img: np.ndarray,
    new_shape: int = 640,
    color: Tuple[int, int, int] = (LETTERBOX_FILL,) * 3,
 ) -> Tuple[np.ndarray, float, Tuple[float, float]]:
    """等比缩放 + 居中填充到 ``new_shape``（和 ultralytics 推理时一致）。
    返回 ``(padded, gain, (dw, dh))``：``dw/dh`` 是**填充量**，还原坐标时减掉它。
    """
    h, w = img.shape[:2]
    gain = min(new_shape / h, new_shape / w)
    new_unpad = (max(1, int(round(w * gain))), max(1, int(round(h * gain))))
    if (w, h) != new_unpad:
        img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR)
    dw = (new_shape - new_unpad[0]) / 2.0
    dh = (new_shape - new_unpad[1]) / 2.0
    top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
    left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
    if top or bottom or left or right:
        img = cv2.copyMakeBorder(
            img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color
        )
    return img, float(gain), (float(dw), float(dh))
 def nms_xyxy(
    boxes: np.ndarray,
    scores: np.ndarray,
    iou_thr: float,
    max_det: int = 20,
 ) -> np.ndarray:
    """纯 numpy 的 NMS，返回保留下来的下标（按分数从高到低）。"""
    if boxes.size == 0:
        return np.zeros((0,), dtype=int)
    x1, y1, x2, y2 = boxes[:, 0], boxes[:, 1], boxes[:, 2], boxes[:, 3]
    areas = np.maximum(0.0, x2 - x1) * np.maximum(0.0, y2 - y1)
    order = scores.argsort()[::-1]
    keep: List[int] = []
    while order.size > 0 and len(keep) < int(max_det):
        i = int(order[0])
        keep.append(i)
        if order.size == 1:
            break
        rest = order[1:]
        ix1 = np.maximum(x1[i], x1[rest])
        iy1 = np.maximum(y1[i], y1[rest])
        ix2 = np.minimum(x2[i], x2[rest])
        iy2 = np.minimum(y2[i], y2[rest])
        inter = np.maximum(0.0, ix2 - ix1) * np.maximum(0.0, iy2 - iy1)
        union = areas[i] + areas[rest] - inter
        iou = np.where(union > 1e-9, inter / np.maximum(union, 1e-9), 0.0)
        order = rest[iou <= float(iou_thr)]
    return np.asarray(keep, dtype=int)
 # --------------------------------------------------------------------------- #
 # 后端：TensorRT engine / ONNX Runtime
 # --------------------------------------------------------------------------- #
 class _TrtRunner:
    """TensorRT 10 执行器：torch 张量当设备缓冲，torch 当前流当执行流。
    不用 pycuda —— 环境里只有 torch，而 ``set_tensor_address`` +
    ``execute_async_v3`` 正好够用。
    """
    def __init__(self, engine_path: Path, device: str = "0") -> None:
        import tensorrt as trt
        import torch
        self._torch = torch
        idx = 0
        dev = str(device or "").strip().lower()
        if dev and dev not in ("auto", "cuda", "gpu"):
            tail = dev.split(":")[-1]
            if tail.isdigit():
                idx = int(tail)
        if not torch.cuda.is_available():
            raise RuntimeError("backend=tensorrt 但 torch 看不到 CUDA")
        self._device = torch.device(f"cuda:{idx}")
        trt_logger = trt.Logger(trt.Logger.ERROR)
        with open(engine_path, "rb") as f:
            engine = trt.Runtime(trt_logger).deserialize_cuda_engine(f.read())
        if engine is None:
            raise RuntimeError(
                f"engine 反序列化失败：{engine_path}。"
                "engine 和 TensorRT 版本绑死，重装环境后要重新导出"
                "（bash scripts/weights.sh doctor --check-engines 可查）"
            )
        self._engine = engine
        self._ctx = engine.create_execution_context()
        names = [engine.get_tensor_name(i) for i in range(engine.num_io_tensors)]
        ins = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.INPUT]
        outs = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.OUTPUT]
        if len(ins) != 1 or len(outs) != 1:
            raise RuntimeError(f"pose engine 期望 1 进 1 出，实际 in={ins} out={outs}")
        self.in_name, self.out_name = ins[0], outs[0]
        with torch.cuda.device(self._device):
            self._x = torch.zeros(
                tuple(engine.get_tensor_shape(self.in_name)),
                dtype=torch.float32,
                device=self._device,
            )
            self._y = torch.zeros(
                tuple(engine.get_tensor_shape(self.out_name)),
                dtype=torch.float32,
                device=self._device,
            )
        self._ctx.set_tensor_address(self.in_name, self._x.data_ptr())
        self._ctx.set_tensor_address(self.out_name, self._y.data_ptr())
        self.out_shape = tuple(engine.get_tensor_shape(self.out_name))
    def run(self, chw: np.ndarray) -> np.ndarray:
        torch = self._torch
        with torch.cuda.device(self._device):
            self._x.copy_(torch.from_numpy(np.ascontiguousarray(chw)).to(self._device))
            self._ctx.execute_async_v3(torch.cuda.current_stream().cuda_stream)
            torch.cuda.current_stream().synchronize()
        return self._y.cpu().numpy()
 class _OnnxRunner:
    """onnxruntime 兜底。本环境只有 CPU provider —— 真机请用 .engine。"""
    def __init__(self, onnx_path: Path) -> None:
        import onnxruntime as ort
        providers = ort.get_available_providers()
        if "CUDAExecutionProvider" in providers:
            chosen = ["CUDAExecutionProvider", "CPUExecutionProvider"]
        else:
            logger.warning(
                "onnxruntime 没有 CUDA provider，%s 会跑在 CPU 上（慢）。"
                "真机请用同名的 .engine",
                onnx_path.name,
            )
            chosen = ["CPUExecutionProvider"]
        self._sess = ort.InferenceSession(str(onnx_path), providers=chosen)
        self.in_name = self._sess.get_inputs()[0].name
        self.out_shape = tuple(self._sess.get_outputs()[0].shape)
    def run(self, chw: np.ndarray) -> np.ndarray:
        return self._sess.run(None, {self.in_name: chw[None].astype(np.float32)})[0]
 # --------------------------------------------------------------------------- #
 # 检测器
 # --------------------------------------------------------------------------- #
 class PoseDetector:
    """把 pose 模型包装成和 ``YoloDetector`` 一样的 ``detect()/infer()`` 接口。
    多出来的一件事：每个 ``Detection`` 带上 ``kpts``（``(K, 3)`` 的
    ``x, y, conf``，原图像素坐标）。
    """
    def __init__(
        self,
        weights: Union[str, Path],
        *,
        conf: float = 0.25,
        iou: float = 0.45,
        imgsz: int = 640,
        device: str = "0",
        class_ids: Optional[Sequence[int]] = None,
        class_names: Optional[Sequence[str]] = None,
        max_det: int = 20,
    ) -> None:
        path = Path(weights).expanduser()
        if not path.is_file():
            raise FileNotFoundError(f"找不到 pose 权重：{path}")
        info = read_pose_info(path) or {}
        self.kpt_count = int(info.get("kpt_count") or 0)
        if self.kpt_count <= 0:
            raise RuntimeError(f"{path.name} 不是可识别的 pose 模型（缺 kpt_shape）")
        self.weights_path = str(path)
        self.conf = float(conf)
        self.iou = float(iou)
        self.imgsz = int(imgsz)
        self.max_det = int(max_det)
        if path.suffix.lower() in {".engine", ".trt"}:
            self._runner = _TrtRunner(path, device)
            self.backend = "tensorrt"
        else:
            self._runner = _OnnxRunner(path)
            self.backend = "onnx"
        # 类别标签仍然走统一入口（旁挂 .labels.yaml 优先）。
        # engine 没有 metadata，所以把 ONNX 里的 names 当"模型内嵌"传进去。
        names = info.get("names") or {}
        class _Meta:  # 只为 resolve_class_labels 能读到 ONNX 内嵌标签
            pass
        meta = _Meta()
        meta.names = names
        meta.nc = int(info.get("nc") or 0)
        self._labels = resolve_class_labels(path, meta)
        self.class_ids = set(int(v) for v in class_ids) if class_ids else None
        self.class_names = (
            {str(n).strip().lower() for n in class_names if str(n).strip()}
            if class_names
            else None
        )
        self._warmed = False
    # -- 对外 ---------------------------------------------------------------
    def warmup(self, *, half: bool = False) -> None:
        del half  # engine 已是 FP16；ONNX 这边没有 half 的开关
        if self._warmed:
            return
        # 先置位：infer() 开头会检查 _warmed，不置位就会反过来再调 warmup。
        self._warmed = True
        try:
            self.infer(np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8))
        except Exception:
            self._warmed = False
            raise
    def detect(
        self,
        image_bgr: np.ndarray,
        conf: float | None = None,
        iou: float | None = None,
        imgsz: int | None = None,
        **_kwargs,
    ) -> List[Detection]:
        """``detect_holes`` 用的别名接口（xyxy + conf + kpts）。"""
        if conf is not None:
            self.conf = float(conf)
        if iou is not None:
            self.iou = float(iou)
        # imgsz 在这里**不能**改：engine 的输入尺寸是编译期定死的。
        if imgsz is not None and int(imgsz) != self.imgsz:
            logger.warning(
                "pose engine 输入固定 %d，忽略调用方的 imgsz=%s", self.imgsz, imgsz
            )
        return self.infer(image_bgr)
    def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]:
        del half, seq
        if image_bgr is None or getattr(image_bgr, "size", 0) == 0:
            return []
        if not self._warmed:
            self.warmup()
        padded, gain, (dw, dh) = letterbox(image_bgr, self.imgsz)
        chw = np.ascontiguousarray(
            padded[:, :, ::-1].transpose(2, 0, 1), dtype=np.float32
        ) / 255.0
        out = self._runner.run(chw)
        return self._decode(out, gain, dw, dh)
    # -- 解码 ---------------------------------------------------------------
    def _decode(
        self, raw: np.ndarray, gain: float, dw: float, dh: float
    ) -> List[Detection]:
        o = np.asarray(raw, dtype=np.float32)[0]  # (C, A)
        ch, n_anchor = o.shape
        k = self.kpt_count
        nc = int(ch - 4 - 3 * k)
        if nc < 1:
            raise RuntimeError(
                f"pose 输出通道数不对：C={ch}，按 kpt={k} 反推 nc={nc}。"
                "引擎/onnx 与 kpt_shape 对不上"
            )
        scores = o[4 : 4 + nc]                      # (nc, A)
        cls_ids = scores.argmax(axis=0)             # (A,)
        confs = scores[cls_ids, np.arange(n_anchor)]
        keep = confs >= self.conf
        if not np.any(keep):
            return []
        xywh = o[0:4, keep].T                       # (N, 4)
        confs = confs[keep]
        cls_ids = cls_ids[keep]
        kpts = o[4 + nc :, keep].reshape(k, 3, -1).transpose(2, 0, 1)   # (N, k, 3)
        xyxy = np.empty_like(xywh)
        xyxy[:, 0] = xywh[:, 0] - 0.5 * xywh[:, 2]
        xyxy[:, 1] = xywh[:, 1] - 0.5 * xywh[:, 3]
        xyxy[:, 2] = xywh[:, 0] + 0.5 * xywh[:, 2]
        xyxy[:, 3] = xywh[:, 1] + 0.5 * xywh[:, 3]
        keep_i = nms_xyxy(xyxy, confs, self.iou, self.max_det)
        if keep_i.size == 0:
            return []
        xyxy, confs, cls_ids, kpts = xyxy[keep_i], confs[keep_i], cls_ids[keep_i], kpts[keep_i]
        # 去 letterbox 的填充，再还原到原图尺度
        xyxy = xyxy.astype(float)
        xyxy[:, [0, 2]] = (xyxy[:, [0, 2]] - dw) / gain
        xyxy[:, [1, 3]] = (xyxy[:, [1, 3]] - dh) / gain
        kpts = kpts.astype(float)
        kpts[:, :, 0] = (kpts[:, :, 0] - dw) / gain
        kpts[:, :, 1] = (kpts[:, :, 1] - dh) / gain
        out: List[Detection] = []
        for i in range(len(xyxy)):
            cid = int(cls_ids[i])
            name = self._labels.get(cid)
            if self.class_ids is not None and cid not in self.class_ids:
                continue
            if self.class_names is not None and name.lower() not in self.class_names:
                continue
            out.append(
                Detection(
                    xyxy=xyxy[i],
                    conf=float(confs[i]),
                    cls_id=cid,
                    cls_name=name,
                    kpts=kpts[i].copy(),
                )
            )
        out.sort(key=lambda d: d.conf, reverse=True)
        return out
    def close(self) -> None:
        return None
 def keypoint_roles(kpt_count: int) -> Tuple[int, Tuple[int, ...]]:
    """返回 ``(跟踪点下标, 孔下标元组)``，供调用方统一口径。
    现场约定：k0 跟踪，k1/k2 对孔。模型关键点少于 3 个时只返回存在的部分。
    """
    track = TRACK_KPT_INDEX if kpt_count > TRACK_KPT_INDEX else -1
    holes = tuple(i for i in HOLE_KPT_INDICES if i < kpt_count)
    return track, holes
@@ -62,10 +62,12 @@ class RealsenseView(TolerantNode):
        super().__init__('realsense_view')
        self.declare_parameter('camera_name', 'd405')
        self.declare_parameter('overlay_hold_sec', 0.10)
        self.declare_parameter('overlay_detection', True)
        self.declare_parameter('view_hz', 20.0)
        self.declare_parameter('hud_label', '')
        self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
        self._hold = max(0.0, float(self.get_parameter('overlay_hold_sec').value))
        self._overlay_detection = bool(self.get_parameter('overlay_detection').value)
        self._hud = str(self.get_parameter('hud_label').value or '').strip()
        self._color: Optional[Image] = None
@@ -102,6 +104,7 @@ class RealsenseView(TolerantNode):
            depth=5,
            reliability=ReliabilityPolicy.RELIABLE,
        )
        if self._overlay_detection:
            self.create_subscription(
                Float32MultiArray, 'workpiece/detection', self._on_det, det_qos
            )
@@ -22,7 +22,7 @@ from typing import Deque, List, Optional
 import numpy as np
 import rclpy
 from geometry_msgs.msg import PointStamped, Twist
-from std_msgs.msg import Bool
+from std_msgs.msg import Bool, Float32
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
@@ -38,8 +38,16 @@ from arm_application.geom_ik import (
    load_handeye_T,
    quat_xyzw_to_R,
 )
 from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
 from arm_application.overhead_oneshot import StillLock, wrist_needs_handoff
 from arm_application.pid_controller import PidController
 from arm_application.kalman_filter import KalmanFilter3D
 from arm_application.feature_jump_guard import (
    ACCEPT,
    HOLD,
    REJECT,
    FeatureJumpGate,
 )
 from arm_application.site_io import (
    base_frame_default,
    ee_frame_default,
@@ -79,6 +87,8 @@ class ServoNode(TolerantNode):
        # 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
        # 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
        self.declare_parameter("handeye_file", "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml")
        # eye_in_hand = cam→法兰；eye_to_hand = cam→基座。写错会把固定相机当腕部用。
        self.declare_parameter("handeye_mode", "eye_in_hand")
        self.declare_parameter("use_static_handeye_tf", False)
        self.declare_parameter("handeye_child_frame", "d405_color_optical_frame")
@@ -93,6 +103,12 @@ class ServoNode(TolerantNode):
        self.declare_parameter("predict_sec", 0.10)
        self.declare_parameter("predict_v_max", 0.35)
        self.declare_parameter("predict_jump_m", 0.08)
        # 大跳变要连续多少帧指向同一新位置才采信（见 feature_jump_guard 的注释）。
        # 30Hz 下 3 帧 ≈ 0.1s：真的重定位 0.1s 就被认，假点（乱跳）永远认不了。
        self.declare_parameter("jump_confirm_frames", 3)
        # 特征点位置合理性：必须落在工作盒 XY 内（含此余量 [m]）。跑出去的点几乎必然
        # 是误检（见 feature_jump_guard 的注释），当"没有目标"处理。
        self.declare_parameter("feat_ws_margin_m", 0.05)
        # --- 看门狗（丢失目标即硬停） ---
        self.declare_parameter("watchdog_timeout_sec", 1.2)
@@ -115,6 +131,10 @@ class ServoNode(TolerantNode):
        self.declare_parameter("deadzone_z_m", 0.012)
        self.declare_parameter("v_max_xy", 0.06)
        self.declare_parameter("v_max_z", 0.04)
        # 远处限速（防"追着假目标把臂带走"）：目标离得超过 slow_far_m 时 XY 限速到
        # v_slow_xy。真机 2026-10-08：0.7s 内被假 k0 从 home 带走 400mm 到工作盒角落。
        self.declare_parameter("slow_far_m", 0.05)
        self.declare_parameter("v_slow_xy", 0.08)
        self.declare_parameter("a_max", 0.45)
        self.declare_parameter("lost_timeout_sec", 1.2)  # 别名 / 用时间戳年龄兜底
        self.declare_parameter("e_xy_clip_m", 0.080)  # 误差绝不按大于 8 cm 处理
@@ -162,6 +182,13 @@ class ServoNode(TolerantNode):
        self.declare_parameter("ki_hover", 0.8)
        # 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True
        self.declare_parameter("lock_hold_sec", 2.0)
        # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
        self.declare_parameter("lock_still_sec", -1.0)
        self.declare_parameter("lock_still_move_m", -1.0)
        self.declare_parameter("wrist_handoff_conf", -1.0)
        self.declare_parameter("overhead_hold_s", -1.0)
        self.declare_parameter("overhead_k0_topic", "visual_servo/overhead_k0")
        self.declare_parameter("wrist_conf_topic", "visual_servo/wrist_conf")
        prefix = str(self.get_parameter("joint_prefix").value or "")
        base = list(
@@ -186,12 +213,17 @@ class ServoNode(TolerantNode):
        self.predict_sec = float(self.get_parameter("predict_sec").value)
        self.predict_v_max = float(self.get_parameter("predict_v_max").value)
        self.predict_jump_m = float(self.get_parameter("predict_jump_m").value)
        self.jump_confirm_frames = max(1, int(self.get_parameter("jump_confirm_frames").value))
        self.feat_ws_margin_m = float(self.get_parameter("feat_ws_margin_m").value)
        self.watchdog_timeout = float(self.get_parameter("watchdog_timeout_sec").value)
        self.stale_hold_sec = float(self.get_parameter("stale_hold_sec").value)
        self.deadzone_m = float(self.get_parameter("deadzone_m").value)
        self.deadzone_z_m = float(self.get_parameter("deadzone_z_m").value)
        self.v_max_xy = float(self.get_parameter("v_max_xy").value)
        self.v_max_z = float(self.get_parameter("v_max_z").value)
        # 远处限速：目标离得比 slow_far_m 远时，XY 速度降到 v_slow_xy（见 hover 段注释）
        self.slow_far_m = float(self.get_parameter("slow_far_m").value)
        self.v_slow_xy = float(self.get_parameter("v_slow_xy").value)
        self.a_max = float(self.get_parameter("a_max").value)
        self.lost_timeout = float(self.get_parameter("lost_timeout_sec").value)
        self.e_xy_clip = float(self.get_parameter("e_xy_clip_m").value)
@@ -235,6 +267,24 @@ class ServoNode(TolerantNode):
        self.seek_timeout = float(self.get_parameter("seek_timeout_sec").value)
        self.ki_hover = float(self.get_parameter("ki_hover").value)
        self.lock_hold_sec = float(self.get_parameter("lock_hold_sec").value)
        self.lock_still_sec = float(self.get_parameter("lock_still_sec").value)
        self.lock_still_move_m = float(self.get_parameter("lock_still_move_m").value)
        self.wrist_handoff_conf = float(self.get_parameter("wrist_handoff_conf").value)
        self.overhead_hold_s = float(self.get_parameter("overhead_hold_s").value)
        self._hybrid = (
            self.lock_still_sec >= 0.0
            and self.lock_still_move_m >= 0.0
            and self.wrist_handoff_conf >= 0.0
            and self.overhead_hold_s >= 0.0
        )
        self._still = StillLock(
            still_sec=max(0.0, self.lock_still_sec),
            still_move_m=max(0.0, self.lock_still_move_m),
        )
        self._track_source = "wrist"  # wrist | overhead
        self._wrist_conf: Optional[float] = None
        self._wrist_conf_t = -1.0
        self._overhead_t = -1.0e9
        i_sep_pos = float(self.get_parameter("i_sep_pos_m").value)
        self.i_sep_ang = float(self.get_parameter("i_sep_ang_rad").value)
        ws_min = list(
@@ -251,11 +301,36 @@ class ServoNode(TolerantNode):
        self.T_cam_to_gripper = load_handeye_T(
            str(self.get_parameter("handeye_file").value)
        )
        self.handeye_mode = str(
            self.get_parameter("handeye_mode").value or "eye_in_hand"
        ).strip().lower()
        if self.handeye_mode not in ("eye_in_hand", "eye_to_hand"):
            raise RuntimeError(
                f"handeye_mode 只能是 eye_in_hand 或 eye_to_hand，"
                f"收到 {self.handeye_mode!r}"
            )
        if self.handeye_mode == "eye_to_hand" and self.xy_track == "lookat":
            raise RuntimeError(
                "眼在手外相机固定，lookat 没有意义。xy_track 必须是 hover"
            )
        if self.handeye_mode == "eye_in_hand" and not self._hybrid:
            raise RuntimeError(
                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / lock_still_move_m / "
                "wrist_handoff_conf / overhead_hold_s。收到哨兵 -1 = 配置没接线，"
                "不许用代码默认 3 秒假装已经静止判据。"
            )
        R_he = np.asarray(self.T_cam_to_gripper[:3, :3], dtype=float)
        t_he = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float)
        axis_z = float(np.clip(R_he[2, 2], -1.0, 1.0))
        self._cam_tilt_deg = float(np.degrees(np.arccos(axis_z)))
        he_path = str(self.get_parameter("handeye_file").value)
        if self.handeye_mode == "eye_to_hand":
            self.get_logger().warn(
                f"HANDEYE yaml={he_path} mode=eye_to_hand "
                f"cam_in_base=({t_he[0]:.3f},{t_he[1]:.3f},{t_he[2]:.3f})m "
                f"— 悬停用 cam→基座，不乘法兰"
            )
        else:
            self.get_logger().warn(
                f"HANDEYE yaml={he_path} "
                f"cam_in_flange=({t_he[0]*1000:.1f},{t_he[1]*1000:.1f},{t_he[2]*1000:.1f})mm "
@@ -312,6 +387,8 @@ class ServoNode(TolerantNode):
        self._p_feat_base: Optional[np.ndarray] = None
        self._he_cmp_done = False
        self._feat_kf = KalmanFilter3D(q_pos=2.0e-3, q_vel=1.5e-1, r_pos=4.0e-4)
        # 大跳变确认门限：单帧不采信（见 feature_jump_guard 的注释与真机反例）。
        self._jump_gate = FeatureJumpGate(self.predict_jump_m, self.jump_confirm_frames)
        # 只有当法兰 Z 已经落到锁定平面上（深度新鲜）才开启 XY 悬停。
        self._xy_armed = bool(self.ee_z_m <= 0.0)
        # Seek（搜索）状态：在锁定高度且没有目标时扫描工件。
@@ -334,6 +411,23 @@ class ServoNode(TolerantNode):
            point_qos,
            callback_group=self._state_cbg,
        )
        if self._hybrid:
            oh_topic = str(self.get_parameter("overhead_k0_topic").value)
            conf_topic = str(self.get_parameter("wrist_conf_topic").value)
            self.create_subscription(
                PointStamped,
                oh_topic,
                self._on_overhead_k0,
                point_qos,
                callback_group=self._state_cbg,
            )
            self.create_subscription(
                Float32,
                conf_topic,
                self._on_wrist_conf,
                5,
                callback_group=self._state_cbg,
            )
        self.create_subscription(
            JointState,
            "joint_states",
@@ -387,8 +481,66 @@ class ServoNode(TolerantNode):
    def _on_js(self, msg: JointState) -> None:
        self._last_js = msg
    def _on_wrist_conf(self, msg: Float32) -> None:
        c = float(msg.data)
        with self._lock:
            self._wrist_conf = None if not np.isfinite(c) else c
            self._wrist_conf_t = time.monotonic()
    def _on_overhead_k0(self, msg: PointStamped) -> None:
        """D435 一次性：点已经在基座系。再乘腕部 T 会差几十厘米还看起来合理。"""
        if not self._hybrid:
            return
        fid = str(msg.header.frame_id or "").strip()
        if fid and self.base_frame and fid != self.base_frame:
            self.get_logger().error(
                f"overhead_k0 的 frame_id={fid!r} 不是基座 {self.base_frame!r} —— 丢弃，"
                "不许当相机系点去乘手眼 T"
            )
            return
        p_base = np.array(
            [float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float
        )
        if not np.all(np.isfinite(p_base)):
            return
        if not self._feat_workspace_ok(p_base):
            self.get_logger().warn(
                f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 —— 丢弃",
                throttle_duration_sec=1.0,
            )
            return
        now_m = time.monotonic()
        now_sec = self.get_clock().now().nanoseconds * 1e-9
        with self._lock:
            self._p_feat_base = p_base.copy()
            self._p_meas[:] = 0.0
            self._p_meas[2] = self.z_ref
            self._feat_kf.reset(p_base, now_m)
            self._jump_gate.reset()
            self._have_target = True
            self._target_lost_flag = False
            self._track_source = "overhead"
            self._overhead_t = now_m
            self._last_seen_time = now_m
            self._stamp_sec = now_sec
            self._seek_t0 = 0.0
            self._seek_warned = False
        self._still.reset()
        self.get_logger().info(
            f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
            f"—— 迁过去等 D405 重新看见"
        )
    def _on_point(self, msg: PointStamped) -> None:
        """新的视觉测量 —— 给看门狗“上弦”并清除丢失标志。"""
        if (
            self._hybrid
            and self._wrist_conf is not None
            and wrist_needs_handoff(self._wrist_conf, self.wrist_handoff_conf)
        ):
            # 已经收到腕部 conf 且低于门槛：这条 D405 点不能当跟踪源。
            # conf 还是 None = 感知还没报到，不要把开场几帧也丢掉。
            return
        p_cam = np.array(
            [float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float
        )
@@ -403,25 +555,34 @@ class ServoNode(TolerantNode):
            self._he_cmp_done = True
            self._log_handeye_tf_vs_yaml()
        now_m = time.monotonic()
        rejected = False
        with self._lock:
            was_lost = self._target_lost_flag
            if self._track_source == "overhead":
                # 从顶视切回腕部：允许一次重定位，跳变门不拿顶视点比。
                was_lost = True
            self._track_source = "wrist"
            self._p_meas[:] = p_cam
            if p_base is not None and at_lock and self._xy_armed:
-                if was_lost or (not self._feat_kf.initialized):
+                verdict = self._update_feat_track(p_base, was_lost=was_lost, now_m=now_m)
-                    self._feat_kf.reset(p_base, now_m)
+                if verdict == ACCEPT:
-                else:
+                    self._p_feat_base = np.asarray(p_base, dtype=float).copy()
-                    dxy = float(np.hypot(p_base[0] - self._feat_kf.position[0],
+                elif verdict == HOLD:
-                                         p_base[1] - self._feat_kf.position[1]))
+                    # hold 时下游必须拿到"钉住的旧位置"：`_hover_vs_feature` 用的是
-                    if dxy > self.predict_jump_m:
+                    # `_p_feat_base`，这里若照抄 p_base，就等于还是去追那个假点。
-                        self._feat_kf.reset(p_base, now_m)
+                    self._p_feat_base = self._feat_kf.position.copy()
-                    else:
+                else:  # REJECT：物理上不合理的点，当"没有目标"处理
-                        self._feat_kf.advance(now_m)
+                    # 关键：必须同时清掉 `_have_target`。只清 `_p_feat_base` 不够 ——
-                        self._feat_kf.update(p_base)
+                    # `_hover_vs_feature` 在 frozen=None 时会退回原始 `p_cam`，
-                self._p_feat_base = p_base.copy()
+                    # 那还是去追那个假点。
                    self._p_feat_base = None
                    self._have_target = False
                    rejected = True
            self._stamp_sec = float(msg.header.stamp.sec) + 1e-9 * float(
                msg.header.stamp.nanosec
            )
            self._last_seen_time = now_m
            if not rejected:
                self._have_target = True
                self._target_lost_flag = False
                self._seek_t0 = 0.0  # 已捕获目标 —— 重置 seek，供下次使用
@@ -443,6 +604,97 @@ class ServoNode(TolerantNode):
                throttle_duration_sec=0.5,
            )
    def _feat_workspace_ok(self, p: np.ndarray) -> bool:
        """特征点 XY 是否落在工作盒内（含余量）。
        只查 XY、**不查 Z**：特征点是**工件表面点**（z≈0.022，贴台面），而工作盒
        的 z 区间（0.18~0.65）是给**法兰**用的，两者不是一回事。
        """
        m = float(self.feat_ws_margin_m)
        return bool(
            self.ws_min[0] - m <= float(p[0]) <= self.ws_max[0] + m
            and self.ws_min[1] - m <= float(p[1]) <= self.ws_max[1] + m
        )
    def _slow_xy_limit(self, nerr: float, v_xy: float) -> tuple:
        """远处限速：目标离得超过 ``slow_far_m`` 就把 XY 速度上限降到 ``v_slow_xy``。
        返回 ``(上限, 原因或 None)``。单拎出来是为了能**离线测**（不需要 ROS 实体）。
        为什么需要（2026-10-08 真机，一阶段）：被一串高置信度假 k0（conf 0.53~0.73）
        顶掉后，伺服在 0.7s 内以 ``v_max_xy``=0.45m/s 把臂从 home ``(0.686,-0.113)``
        带到工作盒角落 ``(0.083,-0.515)``，之后 148s 全是"检测漏检"、150s 超时。
        这道限速**与"目标真不真"无关** —— 它不试图判断真假，只保证"追错时走得少"：
        真目标（实测真 k0 离 145mm）只是晚 ~1.8s 到位；假目标最多把臂带走几厘米。
        """
        if nerr > self.slow_far_m:
            return min(float(v_xy), self.v_slow_xy), (
                f"目标离 {nerr*1000:.0f}mm > {self.slow_far_m*1000:.0f}mm"
            )
        return float(v_xy), None
    def _update_feat_track(
        self, p_base: np.ndarray, *, was_lost: bool, now_m: float
    ) -> str:
        """把一次特征观测并入 ``_feat_kf``；返回 :data:`ACCEPT` / ``HOLD`` / ``REJECT``。
        为什么要单拎出来（2026-10-08 真机实测）：一阶段已经收敛到 9mm 之后，模型在
        运动模糊下误检出一连串"假工件"（框置信度 0.13~0.47，位置各不相同），伺服每
        个都追 —— 机械臂被带到工作盒边缘卡住（`ee_xy` 从 0.68,-0.12 漂到 0.07,-0.50，
        横移 0.62m）。两道独立防线：
        1. **跳变确认**（:class:`FeatureJumpGate`）：单帧大跳只 hold、不动；乱跳
           （每帧不同位置）永远确认不了 ⇒ 这一串假点全被挡掉。
        2. **物理合理性**：跑出工作盒 XY 的点直接 reject —— 实测假点落在基座后方
           ``(-0.10,-0.65)``，而工件在 ``(0.5,-0.12)``。
        调用方必须在 ``self._lock`` 内调用（会改 ``_feat_kf`` 与 ``_jump_gate``）。
        """
        p = np.asarray(p_base, dtype=float).reshape(3)
        # --- 防线 2：物理合理性（先判，因为它在"单帧 vs 多帧"之外） ---
        if not self._feat_workspace_ok(p):
            self._feat_kf.zero_velocity()
            self._jump_gate.reset()
            self.get_logger().warn(
                f"特征点 ({p[0]:.3f},{p[1]:.3f}) 在工作盒外"
                f"（x[{self.ws_min[0]:.2f},{self.ws_max[0]:.2f}] "
                f"y[{self.ws_min[1]:.2f},{self.ws_max[1]:.2f}] ±{self.feat_ws_margin_m:.2f}）"
                "—— 判为误检，当「没有目标」处理，不去追",
                throttle_duration_sec=1.0,
            )
            return REJECT
        # --- 防线 1：跳变确认 ---
        if was_lost or (not self._feat_kf.initialized):
            # 刚重新捕获：没有可信的历史位置可比，直接以新观测重定位。
            self._feat_kf.reset(p, now_m)
            self._jump_gate.reset()
            return ACCEPT
        verdict = self._jump_gate.update(p[:2], self._feat_kf.position[:2])
        if verdict == FeatureJumpGate.JUMP:
            self._feat_kf.reset(p, now_m)
            return ACCEPT
        if verdict == FeatureJumpGate.NEAR:
            self._feat_kf.advance(now_m)
            self._feat_kf.update(p)
            return ACCEPT
        # HOLD：本次观测不采信。位置钉住不动，并把速度清零 —— 否则旧速度会把
        # "不动"变成"按旧速度匀速跑掉"，正是要避免的那件事。
        self._feat_kf.zero_velocity()
        dxy = float(
            np.hypot(p[0] - self._feat_kf.position[0], p[1] - self._feat_kf.position[1])
        )
        self.get_logger().warn(
            f"特征点疑似假跳 {dxy * 1000:.0f}mm > {self.predict_jump_m * 1000:.0f}mm"
            f"（连续 {self._jump_gate.hold_frames} 帧）—— 本次不采信，位置保持不动；"
            f"连续 {self._jump_gate.confirm_frames} 帧一致才认",
            throttle_duration_sec=1.0,
        )
        return HOLD
    def _reset_motion_state(self) -> None:
        """硬停内部状态：I=0、滤波速度=0、dq 窗口清空。"""
        self.pid_x.reset()
@@ -455,6 +707,7 @@ class ServoNode(TolerantNode):
        self._v_xy_cmd[:] = 0.0
        self._p_feat_base = None
        self._feat_kf.initialized = False
        self._jump_gate.reset()
        self._dq_hist.clear()
    def _pub_zero_twist(self) -> None:
@@ -492,6 +745,9 @@ class ServoNode(TolerantNode):
            last_seen = self._last_seen_time
            p_meas = self._p_meas.copy()
            lost = self._target_lost_flag
            source = self._track_source
        if self._hybrid:
            self._tick_still_lock()
        # ----- 启动阶段：**先**下降到锁定高度，甚至在任何检测到来之前。
        # 在第一个 PointStamped 到达之前 have 一直是 False，所以这是一个纯启动
@@ -522,6 +778,7 @@ class ServoNode(TolerantNode):
        if (
            self.enable_control
            and self.seek_enabled
            and (not self._hybrid)  # 混合模式用 D435 一次性，不螺旋搜
            and self.ee_z_m > 0.0
            and not have
            and self._xy_armed
@@ -563,7 +820,15 @@ class ServoNode(TolerantNode):
        seen_age = now_mono - last_seen
        stamp_age = now_sec - stamp if stamp >= 0.0 else 1e9
-        if seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout:
+        if self._hybrid and source == "overhead":
            if seen_age > self.overhead_hold_s:
                self._lock_feedback(False)
                self._watchdog_trip(
                    f"D435 一次性设定已过期 {seen_age:.1f}s > {self.overhead_hold_s:.1f}s"
                )
                return
            stamp_age = 0.0  # 一次性不是流：别按图像戳判过期
        elif seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout:
            self._watchdog_trip(
                f"目标丢失 seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s"
            )
@@ -571,8 +836,11 @@ class ServoNode(TolerantNode):
            return
        # 检测框冻结 / USB 卡住：锁住关节，不要继续按上一个误差驱动。
        # D435 一次性不是流，保持设定直到 hold 超时或 D405 接回。
        stale = max(0.15, float(self.stale_hold_sec))
-        if seen_age > stale or stamp_age > stale:
+        if (not (self._hybrid and source == "overhead")) and (
            seen_age > stale or stamp_age > stale
        ):
            self.get_logger().info(
                f"过期保持 seen_age={seen_age:.2f}s —— 不跟踪冻结的检测框",
                throttle_duration_sec=1.0,
@@ -749,15 +1017,21 @@ class ServoNode(TolerantNode):
        self._watchdog_trip(reason)
    def _R_cam_to_base(self, T_ee: np.ndarray) -> np.ndarray:
-        return (T_ee @ self._T_ee_cam())[:3, :3]
+        return self._T_cam_in_base(T_ee)[:3, :3]
    def _T_ee_cam(self) -> np.ndarray:
        # 始终使用标定出的 yaml。TF 只用于 RViz / 打印做核对。
        return np.array(self.T_cam_to_gripper, dtype=float)
    def _T_cam_in_base(self, T_ee: np.ndarray) -> np.ndarray:
        return cam_to_base_T(self.T_cam_to_gripper, T_ee, self.handeye_mode)
    def _log_handeye_tf_vs_yaml(self) -> None:
        t_yaml = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float)
-        T_tf = self._lookup_T(self.ee_frame, self.handeye_child)
+        parent = (
            self.base_frame if self.handeye_mode == "eye_to_hand" else self.ee_frame
        )
        T_tf = self._lookup_T(parent, self.handeye_child)
        if T_tf is None:
            self.get_logger().warn(
                "HANDEYE TF 查询失败（无妨）—— 伺服使用 yaml "
@@ -801,7 +1075,7 @@ class ServoNode(TolerantNode):
        而不是法兰的 XY。在固定高度 / 姿态下平移法兰 XY，会把该 look-at 点
        按 1:1 一起带走。
        """
-        T_bc = T_ee @ self._T_ee_cam()
+        T_bc = self._T_cam_in_base(T_ee)
        R = T_bc[:3, :3]
        o = T_bc[:3, 3]
        zc = float(np.clip(float(p_cam[2]), 0.04, 0.60))
@@ -819,11 +1093,9 @@ class ServoNode(TolerantNode):
    def _feature_in_base(self, T_ee: np.ndarray, p_cam: np.ndarray) -> np.ndarray:
        """工件三维点（相机系）→ base 系 XYZ。保持 XYZ 一致。"""
-        T_bc = T_ee @ self._T_ee_cam()
+        return cam_point_to_base(
-        ph = np.array(
+            p_cam, self.T_cam_to_gripper, T_ee, self.handeye_mode
            [float(p_cam[0]), float(p_cam[1]), float(p_cam[2]), 1.0], dtype=float
        )
        return (T_bc @ ph)[:3]
    def _hover_vs_feature(
        self, T_ee: np.ndarray, p_cam: np.ndarray
@@ -904,12 +1176,57 @@ class ServoNode(TolerantNode):
            self._xy_in_dead = dead
        return dead
    def _tick_still_lock(self) -> None:
        """D405 跟住且法兰 XY 连续 still_sec 秒没大幅动 → 锁定进二阶段。
        顶视一次性迁臂不算。门槛与位移上限只从 site 来，这里不再写死 3 / 15mm。
        """
        if not self._hybrid:
            return
        T = self._lookup_T(self.base_frame, self.ee_frame)
        xy = None if T is None else np.asarray(T[:2, 3], dtype=float)
        with self._lock:
            have = self._have_target
            lost = self._target_lost_flag
            source = self._track_source
            conf = self._wrist_conf
        tracking_ok = (
            source == "wrist"
            and have
            and not lost
            and (not wrist_needs_handoff(conf, self.wrist_handoff_conf))
            and self._xy_armed
        )
        locked_now = self._still.update(
            xy, time.monotonic(), tracking_ok=tracking_ok
        )
        now_m = time.monotonic()
        if locked_now and self.enable_control:
            if not self._locked:
                self._locked = True
                self.get_logger().info(
                    f"=== 一阶段已锁定（D405 跟踪且臂静止 {self.lock_still_sec:.1f}s，"
                    f"位移≤{self.lock_still_move_m*1000:.0f}mm）"
                    " → /visual_servo/locked ==="
                )
                self._lock_pub.publish(Bool(data=True))
                self._lock_pub_t = now_m
            elif now_m - self._lock_pub_t > 1.0:
                self._lock_pub_t = now_m
                self._lock_pub.publish(Bool(data=True))
        else:
            if self._locked:
                self._lock_pub.publish(Bool(data=False))
            self._locked = False
            self._lock_since = None
    def _lock_feedback(self, converged: bool) -> None:
        """串联锁定信号：持续收敛 lock_hold_sec → /visual_servo/locked=True。
-        只有「有活目标 + XY 入死区 + Z 死区」的驻留分支算收敛；跟踪中、
+        混合模式由 ``_tick_still_lock`` 独占，这里直接返回，避免死区 0.6s 抢先锁定。
        启动下高、寻孔、看门狗冻结等路径都要显式喂 False 打断持续计时。
        """
        if self._hybrid:
            return
        now_m = time.monotonic()
        if converged and self.enable_control:
            if self._lock_since is None:
@@ -1120,6 +1437,19 @@ class ServoNode(TolerantNode):
                if nerr > clip_xy:
                    err = err * (clip_xy / nerr)
                    nerr = clip_xy
                # ---- 远处限速：抓错目标时只走一小段 ----
                # 2026-10-08 真机：一阶段被一串高置信度假 k0 顶掉，伺服 0.7s 内以
                # v_max_xy(0.45m/s) 把臂从 home (0.686,-0.113) 带到工作盒角落
                # (0.083,-0.515)，之后 148s 全是"检测漏检"。留一道**与"目标真不真"
                # 无关**的防线：目标离得远就慢走 —— 真目标（真 k0 离 145mm）只是晚
                # ~1.8s 到位，假目标则最多把臂带走几厘米。
                v_xy, why_slow = self._slow_xy_limit(nerr, v_xy)
                if why_slow:
                    self.get_logger().info(
                        f"{why_slow} —— 限速 {self.v_slow_xy*1000:.0f}mm/s 逼近"
                        "（防追错目标大范围移动）",
                        throttle_duration_sec=1.0,
                    )
                if self._xy_deadzone(nerr, update=False):
                    v_base[0] = 0.0
                    v_base[1] = 0.0
@@ -80,3 +80,137 @@ def ee_frame_default() -> str:
    sn = os.environ.get("SITE_ROBOT_SN", "").strip()
    return f"{sn}_flange" if sn else ""
 def overhead_camera(
    site: dict[str, Any], robot: dict[str, Any] | None = None
 ) -> tuple[dict[str, Any], list[str]]:
    """手外（固定）相机 → ``(相机配置字典, 同样合格但未被选中的相机名)``。
    手外相机 = ``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台（眼在手外时
    相机固定在工位外，不随臂动）。
    **为什么必须有唯一实现**：这份"按 role 找，再按 arm 收敛"的规则有两个调用方——
    S03 粗定位取命名空间（本模块给节点用）、手眼标定脚本取相机名。两边各写一份的
    后果不是报错，而是"标定用的相机"和"推理用的相机"悄悄不是同一台，且没人发现。
    所以规则只写在这里，两边都调它（`export_site_env.overhead_camera` 也走这里）。
    收敛规则（确定性的三级）：
      1. ``arm`` 与当前臂相同的优先（仿真 ``sim_d435`` 靠这级选中）；
      2. 都没有 ``arm``（= 双臂共用的那台）时回落到它；
      3. 还是多个 → 取第一个，并把其余名字放进第二个返回值，让调用方**出声告警**。
    """
    cams = [
        cfg
        for cfg in (site.get("cameras") or {}).values()
        if isinstance(cfg, dict)
        and str(cfg.get("role") or "").strip().lower().startswith("overhead")
    ]
    if not cams:
        return {}, []
    arm = str((robot or {}).get("arm") or "").strip()
    mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
    hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
    chosen = dict(hits[0])
    others = [str(c.get("name") or "?") for c in hits[1:]]
    return chosen, others
 def eye_to_hand_result_name(short: str, camera_name: str) -> str:
    """眼在手外结果文件的**文件名**（不含目录）。
    外参绑定「这条臂的基座 + 这台相机」：换条臂或换台相机都不通用，
    所以名字必须同时带上臂号与相机。调用方只许调这里，不许自己拼
    （launch / export_site_env / S03 找文件都走这一处）。
    """
    short = str(short or "").strip()
    cam = str(camera_name or "").strip().strip("/")
    if short and cam:
        return f"{short}_{cam}_eye_to_hand.yaml"
    if cam:
        return f"{cam}_eye_to_hand.yaml"
    if short:
        return f"{short}_eye_to_hand.yaml"
    return "eye_to_hand.yaml"
 def resolve_eye_to_hand_file(
    root: Path,
    *,
    explicit: str = "",
    camera_name: str = "",
 ) -> Path | None:
    """找手外相机的手眼文件。没有就返回 None。
    路径一律按 ``root`` 解析，不跟当前工作目录走——相对 CWD 的路径会在
    "换个目录启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
    候选顺序：
      1. ``explicit`` —— launch 按 site.yaml 派生的确切路径
         （``<臂号>_<相机>_eye_to_hand.yaml``）；
      2. ``<相机名>_eye_to_hand.yaml`` —— 旧命名 / 仿真（``sim_d435_...``）；
      3. ``eye_to_hand.yaml`` —— 再兜底。
    """
    candidates: list[Path] = []
    if explicit:
        p = Path(explicit).expanduser()
        candidates.append(p if p.is_absolute() else root / p)
    cam = str(camera_name or "").strip().strip("/")
    if cam:
        candidates.append(root / "handeye" / "results" / f"{cam}_eye_to_hand.yaml")
    candidates.append(root / "handeye" / "results" / "eye_to_hand.yaml")
    for cand in candidates:
        if cand.is_file():
            return cand
    return None
 TRACK_CAMERA_ROLES = ("wrist", "overhead")
 def track_camera_role(site: dict[str, Any] | None) -> str:
    """一阶段跟踪用哪台相机：``wrist``（腕部 D405）或 ``overhead``（顶置 D435）。
    写错**当场报错**，不会退回腕部 —— 退回会让人以为在验 D435，其实还在用 D405。
    缺省 ``wrist``：没写这个键时行为与改之前一致（腕部追踪）。
    """
    perc = (site or {}).get("perception")
    perc = perc if isinstance(perc, dict) else {}
    raw = perc.get("track_camera")
    if raw is None or str(raw).strip() == "":
        return "wrist"
    role = str(raw).strip().lower()
    if role not in TRACK_CAMERA_ROLES:
        raise ValueError(
            f"perception.track_camera 只能是 wrist 或 overhead，收到 {raw!r}。"
            "写错不会退回腕部 —— 退回会让人以为在用 D435 其实还在用 D405。"
        )
    return role
 def track_handeye_mode(role: str) -> str:
    """跟踪相机角色 → 手眼装法。overhead 必须用 cam→基座，不能用 cam→法兰。"""
    if str(role or "").strip().lower() == "overhead":
        return "eye_to_hand"
    return "eye_in_hand"
 def handeye_static_tf_frames(
    *,
    mode: str,
    ee_frame: str,
    base_frame: str,
    cam_frame: str,
 ) -> tuple[str, str]:
    """手眼静态 TF 的 ``(父系, 子系)``。
    两种装法的父子关系**不同**，不能混：
      * ``eye_in_hand``：法兰 ← 相机（相机跟着臂动）
      * ``eye_to_hand``：基座 ← 相机（相机固定，不随臂动）
    把眼在手外的文件按"法兰 ← 相机"发出去，会得到一条**看着合理**的错 TF ——
    定位总能到附近、永远差一截，而人只会去怀疑标定精度。
    """
    if str(mode or "").strip().lower() == "eye_to_hand":
        return str(base_frame), str(cam_frame)
    return str(ee_frame), str(cam_frame)
@@ -6,7 +6,7 @@
  1. verticalize — 法兰 +Z 严格朝世界 -Z（垂直朝下），XY/Z 不动
  2. hover       — 闭环升到/降到 robots[].servo.hole_hover_z_m（悬停锁高）
-  3. coarse      — 拍照，detect_best_1 YOLO 框出工件，粗移到 bbox 中心上方
+  3. coarse      — 拍照，工件（pose 模型给框；k1/k2 用关键点）粗移到 bbox 中心上方
  4. fine        — 拍照，two_stage（YOLO+HoughCircles）解 k1/k2 孔心像素
  5. align       — 深度（孔外工作面中值，防孔底拉偏）+ 内参 + 手眼 + TF
                   算出孔在基座系 XY，把夹爪/通止规轴线移到目标孔正上方，
@@ -47,7 +47,7 @@ from arm_application.geom_ik import (
 )
 from arm_application.handeye_realsense import resolve_realsense_topics
 from arm_application.gauge_boot import perception_source_of
-from arm_application.gauge_fsm import drag_verdict, hover_z_guard
+from arm_application.gauge_fsm import drag_verdict, hover_z_guard, wrench_all_zero
 from arm_application.hole_detect_hough import detect_holes
 from arm_application.hole_detector import config_from_site, weights_from_site
 from arm_application.site_io import deep_merge, load_site_dict
@@ -167,6 +167,10 @@ class Stage2Locate(TolerantNode):
        # 仿真里由 run_gauge_assembly.sh --sim 传**实测值**（每次上料都可能不同），
        # 真机不传（走 site.yaml）。悬停高度与 S07 下探预算都按它算。
        self.declare_parameter("tool_tip_below_flange_m", -1.0)
        # 调试开关：只验证**精对准**、不轻降（避免"对准还没confirmed就把规往孔里带"）。
        # 用 int 不用 bool —— `ros2 launch` 传 bool 参数容易踩到 CLI 解析的坑。
        # -1=按 site 的 stage2.final_descent_enable；0=强制不降；1=强制降。
        self.declare_parameter("final_descent_enable", -1)
        self.declare_parameter("preview", True)
        self.declare_parameter("action_name", "rizon_arm_controller/follow_joint_trajectory")
        # 真值孔位（perception.source=truth）要把 scene.yaml 的物体位姿投影成像，
@@ -234,6 +238,12 @@ class Stage2Locate(TolerantNode):
        self.coarse_max_moves = int(stage2.get("coarse_max_moves") or 2)
        self.align_speed = float(stage2.get("align_speed") or 0.03)
        self.move_speed = float(stage2.get("move_speed") or 0.05)
        # 粗居中的两条**误检守卫**（2026-10-08 真机 arm 89 的 exit=4，见
        # _coarse_target_reject / _bbox_on_border 的注释）：粗居中的目的是把臂
        # 挪到工件正上方，**挪错比不挪危险得多** —— 一挪错精定位就再也看不到
        # k1/k2，而流程会把它当"孔检测失败"，现场根本想不到是粗居中把臂带走的。
        self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.12)
        self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
        # 最后轻降到孔面上方 final_gap_mm（不接触）
        self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
        # 悬停高度**必须**让工具尖端留在工件面上方（2026-09-24 的教训，完整说明见
@@ -250,6 +260,10 @@ class Stage2Locate(TolerantNode):
        if _tool_len <= 0.0:
            _tool_len = float(_ga.get("tool_tip_below_flange_m") or 0.0)
        _surface_z = float(_ga.get("workpiece_surface_z_m") or 0.0)
        # 记成成员：轻降的几何夹逼（_descend_above_hole）要用同一套数字，
        # 不能各推一遍 —— 否则改了一处另一处就悄悄失配。
        self.tool_tip_below_flange_m = _tool_len
        self.surface_z_m = _surface_z
        if _tool_len > 0.0:
            _fixed, _why = hover_z_guard(self.hover_z, _surface_z, _tool_len, self.final_gap_m)
            if _why:
@@ -257,8 +271,28 @@ class Stage2Locate(TolerantNode):
                    f"二阶段悬停高度自检不通过 —— 用几何修正值 {_fixed:.4f}m：\n        {_why}"
                )
                self.hover_z = _fixed
        # 轻降**最多能降多少**：从悬停高度降到"规尖停在工件面上方 final_gap"为止。
        # 这是纯几何量，与深度测量无关 —— 测量的唯一职责是让下降**提前**收工，
        # 绝不允许它把下降量**放大**（2026-09-29 实测就是这么撞的：测量出的
        # gap0=141mm 比几何真值大 ~86mm，于是多降 86mm 把规撞进孔）。
        # 工具长/工件面未知（<=0）时给 None = 不启用夹逼，行为与改动前一致。
        self.descent_budget_m: Optional[float] = None
        if _tool_len > 0.0 and _surface_z > 0.0:
            self.descent_budget_m = max(
                0.0, self.hover_z - _tool_len - _surface_z - self.final_gap_m
            )
        # 轻降结束后**按几何**算出的真实间隙 [m]，供收尾日志如实汇报（不是把目标值
        # 当结果念一遍 —— 那会让"停在离面 50mm"也显示成"~2.0mm"，R1.1 的静默失效）。
        self.descent_final_gap_m: Optional[float] = None
        self.final_descent_speed = float(stage2.get("final_descent_speed") or 0.006)
        self.final_descent_enable = _as_bool(stage2.get("final_descent_enable"), True)
        _fd = int(self.get_parameter("final_descent_enable").value)
        if _fd in (0, 1):  # -1 = 按 site；0/1 = 启动参数强制（调试用）
            self.final_descent_enable = bool(_fd)
            self.get_logger().warn(
                f"final_descent_enable 被启动参数覆盖为 {self.final_descent_enable} "
                f"（site 里是 {stage2.get('final_descent_enable')}）—— 只在调试时这么用"
            )
        self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8))
        self.settle_sec = float(stage2.get("settle_sec") or 0.45)
        self.snap_frames = int(stage2.get("snap_frames") or 5)
@@ -266,6 +300,37 @@ class Stage2Locate(TolerantNode):
        self.max_depth = float(stage2.get("max_depth_m") or 0.60)
        self.linger_sec = float(stage2.get("linger_sec") or 8.0)
        # ---- 精对准的「工具位置」：视觉量规尖（默认）还是法兰+固定偏置 ----
        # 为什么必须能视觉量（2026-10-08 真机 arm 89）：
        #   规是**夹在爪里**的零件，它相对法兰轴线可以整体偏出约 10mm（实测规杆
        #   轴线相对法兰 = (+10.7, −0.1)mm）。二阶段原来把**法兰轴线**对到孔上
        #   （hover_offset_ee_xy_m 写死 [0,0]），于是规尖实际离孔心 ~10mm，
        #   轻降一压，φ6 规尖就骑在 φ7 孔的**孔沿**上、力从 −1N 蹦到 63N 卡死。
        #   现场看到的是"轻降在离面 4mm 停住"，离真正原因（规偏了）很远。
        #   改成在同一帧里同时解「规尖基座 XY」和「孔基座 XY」，两者走同一条手眼链，
        #   链路误差/装夹变动一起抵消 —— 这才是"用视觉把孔对正"。
        # 规尖偏移**逐帧实测+一致性校验**（见 _tip_offset_from_depth）：偏移是刚体量，
        # 竖直朝下姿态下在基座 XY 里恒定，所以可以用"上一拍的偏移"剔掉偶发误检。
        # 规尖偏移**逐帧实测 + 一致性校验后冻结**（见 _refresh_tip_offset）：偏移是刚体量，
        # 竖直朝下姿态下在基座 XY 里恒定。为什么要冻结而不是逐拍重算：
        # 2026-10-08 真机实测，逐拍重算时残差在 0.44~0.85mm 之间来回荡、收不进 0.25mm ——
        # 元凶是**规杆略微倾斜**（<1°）：每拍取到的"最深像素"高度不同（工具长在
        # 192~223mm 之间跳），同一根杆在不同高度上的基座 XY 差 ~0.4mm。
        # 偏移既然是刚体量，量准一份冻结住，那 0.4mm 的抖动就没了。
        self.tip_vision = _as_bool(stage2.get("align_tip_vision"), True)
        self.tip_near_m = float(stage2.get("tip_near_m") or 0.15)
        self.tip_min_m = float(stage2.get("tip_min_m") or 0.04)
        self.tip_max_m = float(stage2.get("tip_max_m") or 0.60)
        self.tip_band_frac = float(stage2.get("tip_search_band_frac") or 0.25)
        self.tip_min_px = int(stage2.get("tip_min_px") or 150)
        self.tip_off_tol = float(stage2.get("tip_offset_tol_m") or 0.003)
        self.tip_len_min = float(stage2.get("tip_len_min_m") or 0.15)
        self.tip_len_max = float(stage2.get("tip_len_max_m") or 0.32)
        self.tip_calib_n = int(stage2.get("tip_calib_n") or 4)
        self._tip_off: Optional[np.ndarray] = None  # 规尖相对法兰的基座 XY 偏移（冻结后不变）
        self._tip_samp: list = []  # 校准期攒下的 (偏移, 工具长) 样本
        self._tip_len_m: Optional[float] = None  # 定偏移时并记下的工具长（用于交叉核对配置）
        self.hover_only = _as_bool(self.get_parameter("hover_only").value, False)
        self.preview = _as_bool(self.get_parameter("preview").value, True)
@@ -300,6 +365,9 @@ class Stage2Locate(TolerantNode):
        #: 最近一帧的收到时刻（单调钟）。判"有没有活的数据"用它，**不用数值大小** ——
        #: 静止时轴向力本来就接近 0，拿数值判活会把"安静的传感器"误判成掉线。
        self._t_ft: Optional[float] = None
        #: "FT 全零"守卫的状态（见 _on_wrench）：零值起始时刻 + 是否已喊过一次。
        self._ft_zero_since: Optional[float] = None
        self._ft_zero_warned: bool = False
        robot_ns = str(self.get_parameter("robot_sn").value).replace("-", "_")
        # 话题可配：默认按 sn 推导，与真机 flexiv_robot_states_broadcaster 一致。
        # 换 FT 来源（换个厂家、或走 external_wrench_in_tcp）时不用改代码。
@@ -354,6 +422,28 @@ class Stage2Locate(TolerantNode):
        # 记收到时刻：下游（尤其 S07）要区分"收到过但读数为 0"（活）与
        # "一帧都没收到"（话题名/网络/传感器的问题）。
        self._t_ft = time.monotonic()
        # —— "全零"守卫（2026-10-08 加）：上面那个"收到过就算活"的口径有个盲区 ——
        # 数据源没出数时，话题照样在发、回调照样在跑、读数恒为 (0,0,0)，于是
        # `wrench_live` 报"活"，而"abs(fz−基线)>2.5N 判接触"的兜底**永远不触发**。
        # 真机就踩到了（ft_sensor_wrench 恒零，external_wrench_in_tcp 才有数）。
        # 判据见 wrench_all_zero：带负载的 FT 不可能六个分量同时恰好为 0。
        w = msg.wrench
        if wrench_all_zero(
            w.force.x, w.force.y, w.force.z, w.torque.x, w.torque.y, w.torque.z
        ):
            if self._ft_zero_since is None:
                self._ft_zero_since = self._t_ft
            elif not self._ft_zero_warned and self._t_ft - self._ft_zero_since > 5.0:
                self._ft_zero_warned = True
                self.get_logger().error(
                    f"腕部 FT 话题 {self.ft_wrench_topic} 连续 >5s 六个分量**全为 0** —— "
                    "这不是'安静'（带负载的 FT 不可能全零），是**数据源没有出数**。"
                    "后果：轻降/下压的接触兜底 abs(fz−基线)>2.5N **永远判不到接触**"
                    "（静默失效）。真机请指向有数的话题，例如 "
                    "/<sn>/external_wrench_in_tcp（site.yaml 的 servo.ft_wrench_topic）。"
                )
        else:
            self._ft_zero_since = None
    def _contact_detected(self, baseline_fz: float, thresh_n: float = 2.5) -> bool:
        """腕部力 Z 偏离基线 thresh_n 牛以上 → 判为规尖碰工件（接触）。"""
@@ -913,23 +1003,37 @@ class Stage2Locate(TolerantNode):
        w = str(weights)
        if not Path(w).is_file():
            raise RuntimeError(f"工件权重不存在: {w}")
-        if w.lower().endswith(".onnx"):
+        from arm_application.pose_detector import is_pose_model
        from arm_application.yolo_detector import YoloDetector, build_detector, resolve_weights
        conf = float(self._hole_cfg.get("conf", 0.25))
        iou = float(self._hole_cfg.get("iou", 0.7))
        imgsz = int(self._hole_cfg.get("imgsz", 640))
        if is_pose_model(w):
            # pose 模型同时给工件框 + k1/k2 关键点。走自解码实现 ——
            # ultralytics 解不了 pose 的 TensorRT engine（见 pose_detector 模块头），
            # OnnxWorkpieceDetector 同样只认检测输出。
            det = build_detector(
                resolve_weights(w, "auto"),
                conf=conf,
                iou=iou,
                imgsz=imgsz,
            )
        elif w.lower().endswith(".onnx"):
            from arm_application.onnx_workpiece import OnnxWorkpieceDetector
            det = OnnxWorkpieceDetector(
                w,
-                conf=float(self._hole_cfg.get("conf", 0.25)),
+                conf=conf,
-                imgsz=int(self._hole_cfg.get("imgsz", 640)),
+                imgsz=imgsz,
            )
        else:
            from arm_application.yolo_detector import YoloDetector, resolve_weights
            det = YoloDetector(
                resolve_weights(w, "auto"),
                device="cpu",
-                conf=float(self._hole_cfg.get("conf", 0.25)),
+                conf=conf,
-                iou=float(self._hole_cfg.get("iou", 0.7)),
+                iou=iou,
-                imgsz=int(self._hole_cfg.get("imgsz", 640)),
+                imgsz=imgsz,
            )
        self.get_logger().info(f"工件检测器已加载: {w}")
        return det
@@ -1153,15 +1257,210 @@ class Stage2Locate(TolerantNode):
                    best, best_area = d, area
        if best is None:
            return None
-        return tuple(int(v) for v in best.xyxy)  # type: ignore[return-value]
+        bbox = tuple(int(v) for v in best.xyxy)  # type: ignore[assignment]
        h, w = bgr.shape[:2]
        if self._bbox_on_border(bbox, w, h):
            return None
        return bbox  # type: ignore[return-value]
    def _bbox_on_border(
        self, bbox: Any, w: int, h: int, margin_px: Optional[int] = None
    ) -> bool:
        """框是否被画面边缘切到（= 只看到工件的一部分，不是整个工件）。
        为什么这种框一个都不能用（2026-10-08 真机 arm 89 实测）
        --------------------------------------------------------
        粗居中要的是**框中心落在工件的哪个位置**，再对那个点做深度反投影。
        框被边缘切掉时，框中心根本不在工件上 —— 真机那一帧::
            snap_coarse_0: 工件 bbox=(0, 142, 97, 222) 中心=(48,182)
                           base=(0.280,-0.747)[深度] 偏差=211.5mm
        ``x0=0`` 就是左边缘切的。反投影出来的 (0.280,-0.747) 落在工作盒外
        197mm，而下一步**没有判误检，而是"截断到盒边"照样横移** 56mm，
        把臂带离工件 ⇒ 精定位再也看不到 k1/k2 ⇒ ``exit=4``。
        同一个判据顺手挡掉另一种退化输入：YOLO 空框时兜底用的"孔检测器 bbox"
        在真机那帧是 ``[0, 0, 847, 479]``（整幅画面，四边全贴边）。
        """
        if margin_px is None:
            margin_px = int(getattr(self, "coarse_border_px", 2))
        if bbox is None or len(bbox) < 4 or w <= 0 or h <= 0:
            return True
        x0, y0, x1, y1 = (float(v) for v in bbox[:4])
        m = float(margin_px)
        return x0 <= m or y0 <= m or x1 >= (w - 1) - m or y1 >= (h - 1) - m
    def _coarse_target_reject(
        self,
        p_base: np.ndarray,
        gxy: np.ndarray,
        max_offset_m: Optional[float] = None,
    ) -> Optional[str]:
        """粗居中的目标点是不是**误检**。返回 None = 可信；否则返回拒绝原因。
        为什么必须能在**移动前**否掉（2026-10-08 真机 arm 89，exit=4）
        --------------------------------------------------------------
        真机那一帧::
            snap_coarse_0: base=(0.280,-0.747)[深度] 偏差=211.5mm
              目标超出工作盒，已截断: (0.280,-0.747,0.320) → (0.280,-0.545,0.320)
              coarse Δ=(-56,-2,-0)mm          ← 臂被带离工件 56mm
            snap_fine_*: 两孔基座间距 210.0/213.2/212.2mm 不像 k1/k2，丢弃本拍
            二阶段失败 exit=4
        工件其实在 (0.5,-0.12) 一带 —— 也就是说这个"截断"不是安全兜底，
        而是**把一次误检变成了对机械臂的一条真实横移指令**：越界只是被夹到盒边，
        臂照样走出去。这就是规矩里禁的"宽容降级"：把"没检查"伪装成"检查通过"。
        两条判据（都是"盒外/离谱 ⇒ 判误检"，不是"夹到盒边继续"）：
        1. 目标点落在 ``workspace_xyz_*`` 的 XY 之外（含 5mm 余量）—— 一阶段刚锁
           定时臂就在工件上方，工件**不该**在盒外；
        2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.12m）—— 粗居中
           是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数。
        注意：判误检后**原地不动**，由精定位（直接量 k1/k2）继续。不动的代价只是
        "少挪一次"，而挪错的代价是精定位彻底看不到孔 —— 所以这里必须选不动。
        """
        if max_offset_m is None:
            max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.12))
        p = np.asarray(p_base, dtype=float).reshape(3)
        if not np.all(np.isfinite(p[:2])):
            return "目标点非有限值（深度/三角化失败）"
        lo = np.asarray(getattr(self, "ws_min", None), dtype=float).reshape(-1)
        hi = np.asarray(getattr(self, "ws_max", None), dtype=float).reshape(-1)
        if lo.size >= 2 and hi.size >= 2:
            margin = 0.005
            for i, name in ((0, "x"), (1, "y")):
                if p[i] < lo[i] - margin or p[i] > hi[i] + margin:
                    return (
                        f"目标 {name}={p[i]:.3f} 在工作盒 [{lo[i]:.3f},{hi[i]:.3f}] 外 "
                        f"⇒ 反投影/检测坏了"
                    )
        g = np.asarray(gxy, dtype=float).reshape(-1)
        if g.size >= 2:
            d = float(np.hypot(p[0] - g[0], p[1] - g[1]))
            if d > max_offset_m:
                return (
                    f"目标离夹爪轴线 {d*1000:.1f}mm > 上限 {max_offset_m*1000:.0f}mm "
                    f"⇒ 不像'臂已在工件上方'的粗居中量"
                )
        return None
    def _gauge_xy(self, Tee: np.ndarray) -> np.ndarray:
-        """夹爪/通止规轴线（法兰原点 + hover_offset）在基座系 XY。"""
+        """夹爪/通止规轴线（法兰原点 + hover_offset）在基座系 XY。
        退路用（视觉规尖不可用时）。注意它假设"规和法兰同轴"——2026-10-08 真机实测
        该假设不成立（规偏出 ~10mm），所以**不是**首选，见 _tip_offset_from_depth。
        """
        p = Tee[:3, 3]
        R = Tee[:3, :3]
        off = R @ np.array([self.hover_offset[0], self.hover_offset[1], 0.0])
        return np.array([p[0] + off[0], p[1] + off[1]])
    def _tip_offset_from_depth(
        self, depth_m: np.ndarray, intr: Dict[str, float], Tee: np.ndarray
    ) -> Tuple[Optional[np.ndarray], Optional[float], str]:
        """从一帧深度里找**规尖**，返回 (规尖相对法兰的基座 XY 偏移, 工具长, 说明)。
        找不到/不可信时返回 (None, None, 原因)。判据（每条都能真的失败）：
          1. 只看法兰轴线附近的**中央竖带**——规就夹在爪里，不会跑到画面边上；
          2. 近端连通域要够大（规杆），且连通域内最深 3% 就是**规尖**；
          3. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max] ——
             这一条同时挡住"深度读到别的东西"（如台面/工件），是主干守卫。
        ⚠️ 工具长这一项**只在单帧内**可信；实测它会跳（192~223mm）——规尖细、反光，
        掠射角下深度容易丢，丢了以后"最深有效像素"就往上跑、工具长只会**变短**。
        所以：
          · **XY 偏移**对这件事不敏感（规杆细且接近竖直，杆上任一像素反投回基座 XY
            都在杆轴 ~3mm 内）⇒ 逐帧拿来用是稳的；
          · **工具长**必须挑"端头看得全"的拍（最长那一簇），见 _refresh_tip_offset。
        """
        import cv2
        h, w = depth_m.shape
        valid = (depth_m > self.tip_min_m) & (depth_m < self.tip_max_m)
        near = (valid & (depth_m < self.tip_near_m)).astype(np.uint8)
        band = np.zeros_like(near)
        half = max(1, int(self.tip_band_frac * w))
        c0, c1 = int(intr["cx"]) - half, int(intr["cx"]) + half
        band[:, max(0, c0):min(w, c1)] = 1
        near = near & band
        if int(near.sum()) < self.tip_min_px:
            return None, None, f"近端(<{self.tip_near_m*1000:.0f}mm)中央竖带内像素 {int(near.sum())} < {self.tip_min_px}"
        n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8)
        if n <= 1:
            return None, None, "近端无连通域（规不见了吗）"
        big = max(range(1, n), key=lambda i: stats[i, 4])
        ys, xs = np.where(lab == big)
        dv = depth_m[ys, xs]
        k = np.percentile(dv, 97)  # 连通域内最深 3% ≈ 规尖
        sel = dv >= k - 1e-6
        u = float(np.median(xs[sel]))
        v = float(np.median(ys[sel]))
        y0, y1 = max(0, int(v) - 2), min(h, int(v) + 3)
        x0, x1 = max(0, int(u) - 2), min(w, int(u) + 3)
        patch = depth_m[y0:y1, x0:x1]
        pv = patch[(patch > self.tip_min_m) & (patch < self.tip_max_m)]
        if pv.size == 0:
            return None, None, "规尖邻域深度无效"
        z_tip = float(np.median(pv))
        try:
            p_tip = self._cam_to_base(self._px_to_cam(u, v, z_tip, intr))
        except Exception as exc:  # noqa: BLE001
            return None, None, f"规尖反投影失败（手眼 TF 缺？）: {exc}"
        tool_len = float(Tee[2, 3]) - float(p_tip[2])
        if not (self.tip_len_min <= tool_len <= self.tip_len_max):
            return None, None, (
                f"反投影出的工具长 {tool_len*1000:.0f}mm 不在 "
                f"[{self.tip_len_min*1000:.0f},{self.tip_len_max*1000:.0f}]mm —— "
                f"近端那个物体不是规"
            )
        off = np.array([float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])])
        return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm"
    def _refresh_tip_offset(self) -> None:
        """攒够**一致**样本后，定下并**冻结**「规尖相对法兰」的偏移。
        判据只有一条：取中位数剔掉外点后，还要剩 ``tip_calib_n`` 个样本落在
        ``tip_offset_tol_m`` 内。**不再要求"工具长分簇"** —— 实测那样太严：
        规尖像素时有时无，工具长在 203/222mm 之间跳，"工具长最长那一簇"往往只有
        1~2 拍，永远凑不满 4 拍 ⇒ 永远冻不住 ⇒ 退回"法兰+偏置"（那就白改了）。
        而"规杆细且接近竖直"⇒ 杆上任一像素反投回基座 XY 都在杆轴 ~3mm 内，
        所以**工具长差不影响偏移**，一致性校验就够。
        工具长只用来跟 site.yaml 交叉核对（见下面的告警），不参与对准。
        为什么必须冻结：偏移是刚体量；逐拍重算会把"规杆略有倾斜"引入的 ±0.2~0.4mm
        抖动带进残差（2026-10-08 真机残差 0.44~0.85mm 来回荡、收不进 0.25mm）。
        """
        if self._tip_off is not None or len(self._tip_samp) < self.tip_calib_n:
            return
        arr = np.stack([o for o, _t in self._tip_samp])
        med = np.median(arr, axis=0)
        inl = np.linalg.norm(arr - med, axis=1) <= self.tip_off_tol
        if int(inl.sum()) < self.tip_calib_n:
            return
        idx = np.where(inl)[0]
        self._tip_off = arr[idx].mean(0)
        tl = np.array([self._tip_samp[i][1] for i in idx], dtype=float)
        self._tip_len_m = float(np.median(tl))
        cfg = float(self.tool_tip_below_flange_m or 0.0)
        agree = "" if cfg <= 0 else (
            "，与 site.yaml 的工具长一致"
            if abs(self._tip_len_m - cfg) <= 0.003
            else f" ⚠️ 与 site.yaml 的工具长 {cfg*1000:.0f}mm 差 "
                 f"{(self._tip_len_m-cfg)*1000:+.0f}mm —— 夹持变动？"
                 "下降量按 site 的几何走，这时要人工核对（别让规撞进孔）"
        )
        self.get_logger().info(
            f"  规尖偏移已冻结 = ({self._tip_off[0]*1000:+.1f},{self._tip_off[1]*1000:+.1f})mm"
            f"（{int(inl.sum())} 拍一致，最大散布 "
            f"{float(np.max(np.linalg.norm(arr[idx]-self._tip_off, axis=1)))*1000:.2f}mm，"
            f"工具长 {self._tip_len_m*1000:.0f}mm{agree}）"
            " —— 刚体量，对准期间只量这一份"
        )
    def _grab_median_depth(self, n: int) -> Optional[np.ndarray]:
        """拍 n 帧深度取时间中值（稳）。"""
        deps = []
@@ -1214,46 +1513,112 @@ class Stage2Locate(TolerantNode):
            return None
        return float(np.median(depth[ring]))
    def _measure_z_wp(self, rod_mask: Optional[np.ndarray]) -> Optional[float]:
        """下降后重测局部工件面深度（与悬停时 _local_wp_depth 同一方法，规杆掩码恒定直接复用）。"""
        if rod_mask is None:
            return None
        depth = self._grab_median_depth(6)
        if depth is None:
            return None
        return self._local_wp_depth(depth, rod_mask)
    def _descend_above_hole(self, q, Tee):
        """对准后柔顺轻降，把规尖悬在孔面上方 final_gap_m（不接触）。
-        原理：相机和规都固定在臂上 → 相机到规尖的距离 tip_d 恒定；纯 Z 下降使
+        **下降量只由几何决定**（这条是 2026-10-08 真机修出来的）::
-        「相机到工件面深度 z_wp」1:1 减小。下降到 z_wp = tip_d + final_gap 即规尖
+
-        悬在孔面上方 final_gap。按悬停时测得的间隙一次算下降量，分小步很慢下降，
+            规尖到工件面的间隙 = 法兰 Z(正运动学) − 工具长 − 工件面 Z
-        最后重测 z_wp 核验。不依赖规长/基座绝对 z，稳健；深度反馈保证不接触。
+            需下降            = 该间隙 − final_gap
        三个量都是确定的：法兰 Z 来自 FK（0.1mm 量级），工具长是标定值，工件面 Z
        是现场实测值（``gauge_assembly.workpiece_surface_z_m``）。
        ⚠️ 为什么**不再**让深度测量决定下降量（或叫停）——
        2026-09-29：``z_wp``（"规杆外环形"深度中值）报 141mm，几何真值 53mm，
        于是多降 86mm 把规撞进工件（工件被推走 5mm）。
        2026-10-08：同一个读数又反着错 —— 悬停时报 144mm（几何真值 75mm），
        只降 24mm 后又翻成 **−7mm**，触发 ``SAFE_FLOOR`` 把轻降停在离面 ~50mm 处，
        永远到不了 2mm。同一套读数在两个方向上各错一次、幅度 150mm 量级、还随位姿
        乱跳 —— 它对"能降多少"和"该不该停"都没有发言权。
        所以现在深度读数**只做交叉核对**：与几何对不上就喊出来（把"测距方法待修"
        摆在日志里），但不再参与任何决策。真正的安全由三条**物理/几何**机制保证：
          1. 几何预算上限 ``descent_budget_m``（与 ``gauge_fsm.hover_z_guard`` 同源）——
             FK/配置异常时也压不下去；
          2. 腕部 FT 接触检测：``force.z`` 相对悬空基线突变 ⇒ 立即停 + 回退 1mm；
          3. 任一步 ``_goto`` 没走到位（多半是碰到东西/被挡）⇒ 停止，保持当前高度。
        """
        if not self.final_descent_enable:
            return True, q, Tee
        self._state("descend_above_hole")
-        tip_d, rod_mask, z_wp = self._measure_gauge_state()
+        tip_len = float(self.tool_tip_below_flange_m)
-        if tip_d is None or z_wp is None:
+        surf = float(self.surface_z_m)
-            self.get_logger().warn("未测到通止规/工件间隙（规可能不在视野），跳过轻降，停在悬停高")
+        if tip_len <= 0.0 or surf <= 0.0 or self.descent_budget_m is None:
-            return True, q, Tee
+            # 缺几何就没法算安全下降量 —— 停在悬停高，**绝不**拿测距去赌
-        gap0 = z_wp - tip_d
+            # （2026-09-29 撞工件就是"没有几何、只有测距"时发生的）。配置缺键
            # 必须当失败处理（R1.1），不能静默降级成"按测量降"。
            self.get_logger().error(
                "轻降缺少几何：tool_tip_below_flange_m / workpiece_surface_z_m / "
                "descent_budget 至少一个没配 —— 停在悬停高，不下降。"
                "请补 site.yaml 的 robots[].gauge_assembly 两把尺子。"
            )
            return False, q, Tee
        gap0 = float(Tee[2, 3]) - tip_len - surf  # 几何间隙（唯一决定下降量的量）
        descent = gap0 - self.final_gap_m
        if descent > self.descent_budget_m + 5e-4:
            self.get_logger().warn(
                f"轻降超过配置几何预算：按 FK 算需降 {descent * 1000:.1f}mm > "
                f"预算 {self.descent_budget_m * 1000:.1f}mm"
                f"（当前法兰 {Tee[2, 3]:.4f} 与配置悬停 {self.hover_z:.4f} 不符）"
                "—— 收到预算（宁高勿撞）"
            )
            descent = self.descent_budget_m
        # ---- 深度读数只做交叉核对（无权叫停、无权加码）----
        tip_d, _rod_mask, z_wp = self._measure_gauge_state()
        if tip_d is not None and z_wp is not None:
            gap_meas = float(z_wp) - float(tip_d)
            if abs(gap_meas - gap0) > 0.010:
                self.get_logger().warn(
                    f"深度测距与几何不符：测量间隙 {gap_meas * 1000:.1f}mm vs "
                    f"几何 {gap0 * 1000:.1f}mm（差 {(gap_meas - gap0) * 1000:+.1f}mm）"
                    "—— 本次**按几何下降**，深度读数不参与决策"
                    "（测距方法待修，见 site.yaml 的 final_descent_enable 注释）"
                )
        else:
            self.get_logger().warn(
                "未测到通止规/工件（规可能不在视野）—— 深度核对跳过，仍按几何下降"
                "（下降量不依赖规是否可见）"
            )
        self.get_logger().info(
-            f"轻降准备: 规尖-工件间隙 {gap0 * 1000:.1f}mm → 目标 {self.final_gap_m * 1000:.1f}mm，"
+            f"轻降准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {self.final_gap_m * 1000:.1f}mm，"
-            f"需下降 {descent * 1000:.1f}mm（规尖深 {tip_d * 1000:.0f}mm 恒定，很慢很柔）"
+            f"需下降 {descent * 1000:.1f}mm（法兰 {Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm"
            f" − 工件面 {surf:.4f}；很慢很柔）"
        )
        if descent <= 0.001:
            self.get_logger().info("规尖已在目标间隙附近，无需下降")
            self.descent_final_gap_m = gap0
            return True, q, Tee
        # 腕部力基线（规尖悬空时的 force.z），用于接触检测兜底
        base_fz = self._ft_fz
-        # 闭环轻降：每降一小步就重测局部工件面深度，间隙到 final_gap 就停——绝不一次冲到位，防过冲接触。
+        if base_fz is None:
-        SAFE_FLOOR = 0.0012  # 安全下限 1.2mm：实测间隙≤它立即停（防深度噪声/过冲导致接触）
+            self.get_logger().warn(
-        for _ in range(12):
+                "轻降开始前还没收到腕部 FT —— 接触兜底不生效（只剩几何预算与 _goto 两道）"
            )
        # 步数上限：预算 ≤~80mm、单步 ≤4mm ⇒ 20 步足够；给 60 步余量，真正收工靠
        # 几何预算（下面的 remaining），不靠步数。
        for step_i in range(60):
            if not rclpy.ok():
                return False, q, Tee
            # 每步把 FT 轴向力打进日志：这是"停住到底是碰到了还是没跟上"的**唯一**
            # 直接证据（2026-10-08 排查轻降在离面 ~4mm 处停住时，就靠这一行区分
            # "接触"与"驱动没跟上"——没有它只能靠猜）。
            if self._ft_fz is None:
                ft_txt = "FT_fz=None"
            elif base_fz is None:
                ft_txt = f"FT_fz={self._ft_fz:+.2f}N（无基线）"
            else:
                ft_txt = (
                    f"FT_fz={self._ft_fz:+.2f}N（基线 {base_fz:+.2f}，"
                    f"Δ{self._ft_fz - base_fz:+.2f}）"
                )
            self.get_logger().info(
                f"  轻降第 {step_i + 1} 步: 法兰 Z={Tee[2, 3]:.4f} {ft_txt}"
            )
            # 力觉兜底：规尖一碰工件 force.z 突变，立即停并回退 1mm
            if base_fz is not None and self._contact_detected(base_fz):
                self.get_logger().warn("腕部力突变（规尖触到工件），立即停止并回退 1mm（安全优先）")
@@ -1264,20 +1629,13 @@ class Stage2Locate(TolerantNode):
                    v=self.final_descent_speed, max_hops=3, hold_xy=True, settle_s=0.25,
                )
                self._pin(q)
                self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
                return True, q, Tee
-            z_now = self._measure_z_wp(rod_mask)
+            # 用 FK 现算剩余下降量（不是累加指令）—— 单步没走到也不会积累误差。
-            if z_now is not None:
+            remaining = (float(Tee[2, 3]) - tip_len - surf) - self.final_gap_m
-                gap_now = z_now - tip_d
+            if remaining <= 1e-4:
                excess = gap_now - self.final_gap_m
                # 安全下限优先：宁可停在孔上方多一点，也绝不压到工件
                if gap_now <= SAFE_FLOOR:
                    self.get_logger().info(f"已达安全下限间隙 {gap_now * 1000:.1f}mm，停止下降（不接触）")
                break
-            else:
+            step = min(remaining, 0.004)  # 每步 ≤4mm，很慢很柔
                excess = descent  # 测不到就按预算量保守走
            if excess <= 0.0004:  # 已到 final_gap+0.4mm 容差内
                break
            step = min(excess, 0.004)  # 每步 ≤4mm，很慢很柔
            p = Tee[:3, 3].copy()
            target = np.array([p[0], p[1], p[2] - step])
            ok, q, Tee = self._goto(
@@ -1288,15 +1646,16 @@ class Stage2Locate(TolerantNode):
            self._pin(q)
            if not ok:
                self.get_logger().warn("轻降中途未到位，停止（保持当前高度，安全优先）")
                self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
                return False, q, Tee
-        # 核验：重测工件面深度，算最终间隙（深度反馈，确保不接触）
+        # 收尾：按几何如实汇报最终间隙（不再用那个错 150mm 的深度读数当结论）
-        z2 = self._measure_z_wp(rod_mask)
+        gap_f = float(Tee[2, 3]) - tip_len - surf
-        if z2 is not None:
+        self.descent_final_gap_m = gap_f
-            gap_f = z2 - tip_d
+        lvl = self.get_logger().info if gap_f >= 0.0 else self.get_logger().warn
            lvl = self.get_logger().info if 0.0003 < gap_f else self.get_logger().warn
        lvl(
-                f"=== 轻降完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm "
+            f"=== 轻降完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 "
-                f"（目标 {self.final_gap_m * 1000:.1f}mm，不接触）==="
+            f"{Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm − 工件面 {surf:.4f}；"
            f"目标 {self.final_gap_m * 1000:.1f}mm，不接触）==="
        )
        return True, q, Tee
@@ -1525,6 +1884,8 @@ class Stage2Locate(TolerantNode):
            bbox = None
            truth: Optional[dict] = None
            bgr = depth_m = intr = None
            # 本拍是否"跳过粗居中"（精定位已看到 k1/k2）—— 见下面 holes 分支
            skip_coarse = False
            time.sleep(self.settle_sec if move_i == 0 else min(0.20, self.settle_sec))
            for attempt in range(8):
                if attempt:
@@ -1537,13 +1898,21 @@ class Stage2Locate(TolerantNode):
                    break
                holes = self._detect_holes_img(bgr, skip_center_gate=True)
                if holes.get("status"):
-                    self.get_logger().info("粗定位 YOLO 空框，但已看到 k1/k2，跳过居中直进精对准")
+                    # 精定位已经能看到 k1/k2（它是**直接量孔**，比粗居中可靠），
-                    bbox = holes.get("bbox")
+                    # 所以不居中、不移动，直接交给精定位。
-                    truth = holes
+                    # 旧写法在这里把孔检测器的整幅 bbox 拿来当工件目标，然后**照样算
                    # 偏差横移**：真机那帧 bbox=[0,0,847,479]（整幅画面）⇒ 偏差
                    # 55.9mm ⇒ 又挪一次。一句话："跳过居中"就必须真的不动。
                    self.get_logger().info(
                        "粗定位 YOLO 无可信框（空框/被边缘切），但已看到 k1/k2 "
                        "—— 跳过粗居中、原地直进精对准"
                    )
                    self._publish_overlay(holes.get("draw_img", bgr))
-                    move_i = self.coarse_max_moves
+                    skip_coarse = True
                    break
                self.get_logger().warn(f"粗定位第 {attempt + 1}/8 帧未见工件，复拍")
            if skip_coarse:
                break
            if bbox is None:
                if bgr is not None:
                    self._publish_overlay(bgr)
@@ -1572,6 +1941,17 @@ class Stage2Locate(TolerantNode):
                Tee = T_now
            gxy = self._gauge_xy(Tee)
            err_xy = float(np.hypot(p_base[0] - gxy[0], p_base[1] - gxy[1]))
            # 目标点先过**误检守卫**：宁可原地不动交给精定位，也不许按一个坏点横移。
            # 一阶段刚锁定时臂已经在工件上方，粗居中的合理位移是厘米级；出现
            # 十几厘米以上的"偏差"或盒外目标，只可能是反投影/检测坏了。
            why = self._coarse_target_reject(p_base, gxy)
            if why:
                self.get_logger().warn(
                    f"粗定位目标判误检（{why}）—— **不移动**，原地直进精定位："
                    f"base=({p_base[0]:.3f},{p_base[1]:.3f})[{src}] "
                    f"夹爪=({gxy[0]:.3f},{gxy[1]:.3f}) 偏差={err_xy*1000:.1f}mm"
                )
                break
            coarse_tol = float(self._hole_cfg.get("coarse_tol_m", 0.006))
            self.get_logger().info(
                f"工件 bbox={bbox} 中心=({bx:.0f},{by:.0f}) "
@@ -1674,7 +2054,34 @@ class Stage2Locate(TolerantNode):
            Tee_now = self._lookup(self.base_frame, self.ee_frame)
            if Tee_now is not None:
                Tee = Tee_now
            # ---- 工具位置取"视觉规尖"，不是"法兰轴线"--------------------
            # 见 _tip_offset_from_depth / _refresh_tip_offset：规可偏出法兰轴线 ~10mm，
            # 把法兰对到孔上等于把规偏着对过去。偏移量准一次后冻结（刚体量）。
            tip_src = "法兰+偏置"
            if self.tip_vision and self._tip_off is None:
                off, tlen, why = self._tip_offset_from_depth(depth_m, intr, Tee)
                if off is not None and tlen is not None:
                    self._tip_samp.append((off, tlen))
                    self._refresh_tip_offset()
                    tip_src = (
                        f"视觉规尖(校准中 n={len(self._tip_samp)} {why})"
                        if self._tip_off is None
                        else "视觉规尖(已冻结)"
                    )
                else:
                    self.get_logger().warn(f"  规尖视觉量取失败：{why}")
            elif self._tip_off is not None:
                tip_src = "视觉规尖(已冻结)"
            if self._tip_off is not None:
                gxy = np.array(
                    [Tee[0, 3] + self._tip_off[0], Tee[1, 3] + self._tip_off[1]]
                )
            else:
                gxy = self._gauge_xy(Tee)
                if self.tip_vision and self._tip_off is None and len(self._tip_samp) == 0:
                    self.get_logger().warn(
                        "  规尖还没量到，本拍先用法兰+偏置兜着 —— 此时可能对的是法兰不是规"
                    )
            err = float(np.hypot(tgt_b[0] - gxy[0], tgt_b[1] - gxy[1]))
            final = {
@@ -1688,6 +2095,13 @@ class Stage2Locate(TolerantNode):
                "workpiece_surface_z_m": round(z_wp, 4),
                "hole_z_m": [round(z_left, 4), round(z_right, 4)],
                "gauge_xy_m": [round(float(gxy[0]), 5), round(float(gxy[1]), 5)],
                "gauge_source": tip_src,
                "tip_offset_m": (
                    [round(float(self._tip_off[0]), 5), round(float(self._tip_off[1]), 5)]
                    if self._tip_off is not None
                    else None
                ),
                "tip_len_m": (round(self._tip_len_m, 5) if self._tip_len_m else None),
                "residual_mm": round(err * 1000.0, 2),
                "hover_z_m": round(float(Tee[2, 3]), 4),
                "iter": it,
@@ -1696,7 +2110,8 @@ class Stage2Locate(TolerantNode):
            self.get_logger().info(
                f"[fine {it}] k1=({k1_b[0]:.4f},{k1_b[1]:.4f}) k2=({k2_b[0]:.4f},{k2_b[1]:.4f}) "
                f"zL={z_left*1000:.0f} zR={z_right*1000:.0f}mm "
-                f"gauge=({gxy[0]:.4f},{gxy[1]:.4f}) 残差={err*1000:.2f}mm (tol {self.align_tol*1000:.1f})"
+                f"工具=({gxy[0]:.4f},{gxy[1]:.4f})[{tip_src}] "
                f"残差={err*1000:.2f}mm (tol {self.align_tol*1000:.1f})"
            )
            # ---- 工件被推走的看门狗 -----------------------------------------
@@ -1758,6 +2173,21 @@ class Stage2Locate(TolerantNode):
            self._publish_result(final)
            if err <= self.align_tol:
                if self.tip_vision and self._tip_off is None:
                    # 容差达标了，但这一拍的"工具位置"其实是**法兰**（退路了）——
                    # 对的是法兰不是规，δ 就是那个 ~10mm 的装夹偏置。2026-10-08 真机
                    # 就出现过：tip_offset_m=None、残差 0.15mm 看着漂亮，其实法兰
                    # 压在 k1 上、规尖偏出 ~11mm。**不许把没做到说成做到**。
                    self.get_logger().error(
                        f"残差 {err*1000:.2f}mm 达标，但规尖偏移一直没量到（用的是法兰轴线）"
                        " —— 这个结果不可信：对的可能是法兰、不是规。"
                        "判失败，摆到能看清规尖再跑（别按这个结果下降）"
                    )
                    final["ok"] = False
                    final["align_trusted"] = False
                    final["fail_reason"] = "未量到规尖偏移，工具位置退化为法兰轴线"
                    self._publish_result(final)
                    return 8, q, Tee, final
                self._state("aligned")
                break
            if last_good is not None:
@@ -1828,10 +2258,18 @@ class Stage2Locate(TolerantNode):
                self.get_logger().warn("对准移动未完全到位，复拍核验")
        # ---- 4.5) 柔顺轻降到孔面上方 1-2mm（不接触）----
-        # 用户要求：对准后轻轻悬在孔上方 1-2mm，不下臂接触。深度反馈闭环，很慢很柔。
+        # 下降量由**几何**决定（法兰 FK − 工具长 − 工件面 − final_gap），深度读数只
        # 做交叉核对，见 _descend_above_hole 的说明。
        okd, q, Tee = self._descend_above_hole(q, Tee)
        if not okd:
            self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）")
        # 如实汇报**实际**间隙（几何量）——不是把目标值念一遍。停在离面 50mm 时
        # 也显示 ~50mm，现场才看得出"轻降没做到"（R1.1：不许把没做到说成做到）。
        gap_txt = (
            f"{self.descent_final_gap_m * 1000:.1f}"
            if self.descent_final_gap_m is not None
            else f">{self.final_gap_m * 1000:.1f}(未能测得/未下降)"
        )
        # ---- 5) 锁定保持 ----
        self._state("hold")
@@ -1839,7 +2277,7 @@ class Stage2Locate(TolerantNode):
        self._pin(q_fin if q_fin is not None else q)
        self.get_logger().info(
            f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
-            f"悬在孔面上方 ~{self.final_gap_m * 1000:.1f}mm（不接触）。"
+            f"规尖悬在孔面上方 ≈{gap_txt}mm（不接触）。"
            f"结果: /stage2/result, /tmp/stage2_result.json ==="
        )
        return 0, q_fin if q_fin is not None else q, Tee, final
@@ -43,6 +43,9 @@ class Detection:
    conf: float
    cls_id: int
    cls_name: str
    # pose 模型才有：(K, 3) = 每个关键点的 x, y, conf（**原图像素坐标**）。
    # 检测模型恒为 None —— 下游必须先判 None，别假设它有。
    kpts: Optional[np.ndarray] = None
    @property
    def cx(self) -> float:
@@ -301,12 +304,51 @@ class YoloDetector:
        return seq, dets, (time.monotonic() - t0) * 1000.0
 def build_detector(
    weights: Union[str, Path],
    *,
    backend: str = "auto",
    conf: float = 0.25,
    iou: float = 0.45,
    device: str = "",
    imgsz: int = 640,
 ):
    """按权重类型挑检测器：pose 模型走 ``PoseDetector``，否则走 ``YoloDetector``。
    为什么要这个工厂：ultralytics 解不了 pose 的 TensorRT engine（见
    ``pose_detector`` 模块头），所以 pose 必须换成自解码实现。判断依据是模型
    是不是 ``task=pose`` —— 直接读旁边 ``.onnx`` 的 metadata，不靠调用方声明。
    返回对象统一有 ``detect()/infer()/warmup()/imgsz/class_names/class_ids``。
    """
    path = Path(weights).expanduser()
    from .pose_detector import PoseDetector, is_pose_model
    if is_pose_model(path):
        logger.info("检测到 pose 模型 %s → 用 PoseDetector（自解码关键点）", path.name)
        return PoseDetector(
            path,
            conf=conf,
            iou=iou,
            device=device or "0",
            imgsz=imgsz,
        )
    return YoloDetector(
        path,
        conf=conf,
        iou=iou,
        device=device,
        imgsz=imgsz,
    )
 def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
-    """spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。"""
+    """spawn 出来的子进程：只跑 GPU 上的检测。这里绝不能 import rclpy。"""
    try:
        path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
-        det = YoloDetector(
+        det = build_detector(
            path,
            backend=cfg.get("backend") or "auto",
            conf=float(cfg.get("conf") or 0.25),
            iou=float(cfg.get("iou") or 0.45),
            device=str(cfg.get("device") or "0"),
@@ -336,7 +378,13 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
            dets = det.infer(image, half=bool(cfg.get("half", True)))
            gpu_ms = (time.monotonic() - t0) * 1000.0
            packed = [
-                (d.xyxy.astype(np.float32), float(d.conf), int(d.cls_id), str(d.cls_name))
+                (
                    d.xyxy.astype(np.float32),
                    float(d.conf),
                    int(d.cls_id),
                    str(d.cls_name),
                    None if d.kpts is None else np.asarray(d.kpts, dtype=np.float32),
                )
                for d in dets
            ]
            _put_drop_old(q_out, ("ok", seq, packed, gpu_ms))
@@ -399,15 +447,20 @@ class YoloInferProcess:
    def alive(self) -> bool:
        return bool(self._p is not None and self._p.is_alive())
-    def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
+    def submit(self, image_bgr: np.ndarray, seq: int = 0) -> bool:
        """把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
-        排空到队列里最新的帧，所以不会跑任何过期数据。"""
+        排空到队列里最新的帧，所以不会跑任何过期数据。
        返回是否真的进队。进不去必须告诉调用方，否则一次性会以为「已经在跑」
        而空等，把 D405 也卡住。
        """
        if not self.alive:
            raise RuntimeError("YOLO 工作进程已死亡")
        try:
            self._q_in.put((int(seq), image_bgr), timeout=0.010)
            return True
        except Exception:
-            pass
+            return False
    def wait_result(self, timeout: float = 0.045):
        """返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
@@ -444,15 +497,20 @@ class YoloInferProcess:
            return None
        seq, packed = int(msg[1]), msg[2]
        gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0
-        dets = [
+        dets = []
        for row in packed:
            # 兼容旧格式（4 元组，无关键点），免得子进程和父进程版本不一致时炸掉。
            xyxy, conf, cid, name = row[0], row[1], row[2], row[3]
            kpts = row[4] if len(row) > 4 else None
            dets.append(
                Detection(
                    xyxy=np.asarray(xyxy, dtype=float),
                    conf=float(conf),
                    cls_id=int(cid),
                    cls_name=str(name),
                    kpts=None if kpts is None else np.asarray(kpts, dtype=float),
                )
            )
            for xyxy, conf, cid, name in packed
        ]
        return seq, dets, gpu_ms
    def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0):
@@ -1,6 +1,6 @@
 # 孔定位检测配置（改 conf / 内参 / 双孔间距即可，不必改代码）
 # 权重来源：优先 site.yaml（hole_detect.weights / paths.hole_weights）；
-#           两者都缺时兜底到 models/hole_insert/detect_best_1.onnx。
+#           两者都缺时兜底到 models/yolo/gongjian0929.onnx。
 #           原先这里注释写的是 best.pt —— 该文件不存在，已修正。
 # backend: yolo | hough | two_stage（two_stage 请用 hole_detect.yaml）
@@ -12,7 +12,7 @@ perception_node:
    image_topic: ""
    camera_info_topic: ""
    depth_topic: "/d405/aligned_depth_to_color/image_raw"
-    weights: "/home/cc/ros2/models/yolo/detect_best_0.onnx"
+    weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
    conf: 0.10
    iou: 0.45
    imgsz: 640
@@ -78,6 +78,11 @@ def _launch_setup(context, *args, **kwargs):
    merged, robot = apply_active_arm(data)
    ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
    base_frame = str(
        robot.get("base_frame")
        or os.environ.get("SITE_BASE_FRAME")
        or f"{robot.get('sn')}_base_link"
    )
    handeye = str((merged.get("paths") or {}).get("handeye_file") or "").strip()
    if not handeye:
        handeye = str((robot.get("paths") or {}).get("handeye_file") or "").strip()
@@ -85,6 +90,30 @@ def _launch_setup(context, *args, **kwargs):
    cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
    cam_frame = f"{cam}_color_optical_frame"
    # ---- 手外相机（eye-to-hand）：S03 粗定位用的外参 ----
    # 名字按 site.yaml 的 cameras 注册表（role=overhead*）由**唯一实现**解析
    # （site_io.overhead_camera，节点自己取命名空间也走它）。
    sys.path.insert(0, str(root / "src" / "arm_application"))
    from arm_application.site_io import (  # type: ignore
        eye_to_hand_result_name,
        handeye_static_tf_frames,
        overhead_camera as _overhead_camera,
    )
    from export_site_env import arm_short_id  # type: ignore
    over_cfg, _ = _overhead_camera(merged, robot)
    over_cam = str(over_cfg.get("name") or "").strip("/")
    over_frame = f"{over_cam}_color_optical_frame" if over_cam else ""
    # 眼在手外的产物名走 site_io.eye_to_hand_result_name（唯一口径），
    # 不在这里自己拼（拼错/漏拼会让 S03 找不到刚标完的文件）。
    short = arm_short_id(str(robot.get("sn") or ""))
    e2h_file = (
        root / "handeye" / "results" / eye_to_hand_result_name(short, over_cam)
        if over_cam
        else None
    )
    def _flag(name: str) -> bool:
        return LaunchConfiguration(name).perform(context).strip().lower() in (
            "1", "true", "yes", "on",
@@ -137,6 +166,51 @@ def _launch_setup(context, *args, **kwargs):
            "—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh"
        )
    # ---- 手外相机外参：**基座 ← 相机**（眼在手外）----
    # 与上面那条的父子关系**不同**，两者不能混：
    #   腕部（eye-in-hand）：法兰 ← 相机（相机跟着臂动）
    #   手外（eye-to-hand）：基座 ← 相机（相机固定，不随臂动）
    # S03 反投影查的正是 `{prefix}base_link ← {相机}_color_optical_frame`
    #（gauge_assembly_node._coarse_workpiece_xy）。把 eye_to_hand 的文件按
    # "法兰 ← 相机"发出去会得到一条**看着合理**的错 TF —— 于是定位总能到附近、
    # 永远差一截，而人只会去怀疑标定精度。
    if over_cam and e2h_file and publish_handeye_tf:
        if e2h_file.is_file():
            try:
                tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(str(e2h_file))
            except Exception as exc:  # noqa: BLE001
                frames.append(f"[gauge] 手外相机文件读不了（{e2h_file}）：{exc}")
            else:
                parent, child = handeye_static_tf_frames(
                    mode="eye_to_hand",
                    ee_frame=ee_frame,
                    base_frame=base_frame,
                    cam_frame=over_frame,
                )
                frames.append(
                    f"[gauge] 手外 TF {parent} ← {child}（{e2h_file.name}）"
                )
                nodes.append(
                    Node(
                        package="tf2_ros",
                        executable="static_transform_publisher",
                        name="gauge_coarse_handeye_static_tf",
                        arguments=[
                            "--x", str(tx), "--y", str(ty), "--z", str(tz),
                            "--qx", str(qx), "--qy", str(qy), "--qz", str(qz),
                            "--qw", str(qw),
                            "--frame-id", parent,
                            "--child-frame-id", child,
                        ],
                        output="screen",
                    )
                )
        else:
            frames.append(
                f"[gauge] 手外相机 {over_cam} 没有 eye-to-hand 标定（{e2h_file.name}）"
                "—— 不发它的静态 TF，S03 会明确报缺文件并回 S04 跟踪（不假装成功）"
            )
    gauge_node = Node(
        package="arm_application",
        executable="gauge_assembly",
@@ -152,6 +226,8 @@ def _launch_setup(context, *args, **kwargs):
                "gripper_backend": backend,
                "result_file": result_file,
                "use_sim_time": use_sim_time,
                # S03 找手外外参用的确切路径（名字口径由 export_site_env 一处实现）。
                "coarse_eye_to_hand_file": str(e2h_file) if e2h_file else "",
            }
        ],
    )
@@ -36,7 +36,7 @@ def _setup(context, *args, **kwargs):
        if backend in ("two_stage", "hough_roi"):
            weights = os.environ.get("SITE_HOLE_WORKPIECE_WEIGHTS") or os.environ.get("SITE_YOLO_WEIGHTS") or ""
        else:
-            weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "hole_insert" / "detect_best_1.onnx")
+            weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "yolo" / "gongjian0929.onnx")
    return [
        LogInfo(msg=f"[hole_detect] cam={cam} backend={backend} weights={weights} cfg={cfg}"),
        Node(
@@ -77,6 +77,10 @@ def _launch_setup(context, *args, **kwargs):
    skip3 = _as_bool(LaunchConfiguration("skip_stage3").perform(context))
    preview = _as_bool(LaunchConfiguration("preview").perform(context))
    resnap = _as_bool(LaunchConfiguration("resnap").perform(context))
    # 调试开关：只验证精对准、不轻降。用途是"对准逻辑刚改完、还没确认它真能把规
    # 对到孔心"时先别让规往下走 —— 对不准就下压会把规顶在孔沿上（2026-10-08 真机
    # 顶到 63N）。默认关；验证对准时用 --no-descent 打开。
    no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context))
    target_hole = LaunchConfiguration("target_hole").perform(context).strip()
    depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
    pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
@@ -115,6 +119,8 @@ def _launch_setup(context, *args, **kwargs):
                "skip_stage1": skip1,
                "skip_stage2": skip2,
                "skip_stage3": skip3,
                # -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
                "final_descent_enable": 0 if no_descent else -1,
            }
        ],
    )
@@ -125,6 +131,7 @@ def _launch_setup(context, *args, **kwargs):
        LogInfo(
            msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
            f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
            + (" [no_descent：只对准，不轻降]" if no_descent else "")
        ),
        *nodes,
        node,
@@ -147,6 +154,8 @@ def generate_launch_description() -> LaunchDescription:
            DeclareLaunchArgument("insert_depth_mm", default_value=""),
            DeclareLaunchArgument("screw_pitch_mm", default_value=""),
            DeclareLaunchArgument("resnap", default_value="false"),
            # 只验证精对准、不轻降（见 _launch_setup 里的说明）
            DeclareLaunchArgument("no_descent", default_value="false"),
            DeclareLaunchArgument("preview", default_value="true"),
            OpaqueFunction(function=_launch_setup),
        ]
@@ -81,14 +81,13 @@ def _launch_setup(context, *args, **kwargs):
    share = Path(get_package_share_directory("arm_application"))
    site, site_path, perc_over, servo_over, robot = _load_site()
-    # launch 参数显式传入时覆盖 site 配置（空值 / 默认值的处理见下）
+    # launch 参数显式传入时覆盖；空则用 servo_overrides（已按 track_camera 选文件）。
    # 不要再拿某个臂的腕部结果路径当"空"哨兵：换臂或切到 D435 后那个判断失效，
    # 会把眼在手外的跟踪悄悄接上腕部 T。
    handeye = LaunchConfiguration("handeye_file").perform(context).strip()
-    site_he = str(servo_over.get("handeye_file") or (site.get("paths") or {}).get("handeye_file") or "")
+    site_he = str(servo_over.get("handeye_file") or "")
-    if not handeye or handeye in (
+    if not handeye:
-        "/tmp/handeye_calib/handeye_result.yaml",
+        handeye = site_he
        "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml",
    ):
        handeye = site_he or handeye
    if not Path(handeye).is_file():
        raise FileNotFoundError(
            f"手眼标定结果缺失：{handeye}。"
@@ -120,6 +119,18 @@ def _launch_setup(context, *args, **kwargs):
    tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
    # 跟踪相机的手眼父子：腕部=法兰←相机，顶视=基座←相机。不许写死法兰。
    from arm_application.site_io import handeye_static_tf_frames  # noqa: PLC0415
    he_mode = str(servo_over.get("handeye_mode") or "eye_in_hand")
    base_frame = str(robot.get("base_frame") or "")
    tf_parent, tf_child = handeye_static_tf_frames(
        mode=he_mode,
        ee_frame=ee_frame,
        base_frame=base_frame,
        cam_frame=cam_frame,
    )
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
@@ -127,8 +138,8 @@ def _launch_setup(context, *args, **kwargs):
        arguments=[
            "--x", str(tx), "--y", str(ty), "--z", str(tz),
            "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
-            "--frame-id", ee_frame,
+            "--frame-id", tf_parent,
-            "--child-frame-id", cam_frame,
+            "--child-frame-id", tf_child,
        ],
        output="screen",
    )
@@ -225,7 +236,7 @@ def _launch_setup(context, *args, **kwargs):
        LogInfo(msg=f"[site] {site_path}"),
        LogInfo(
            msg=f"[arm] {robot.get('label') or robot.get('sn')}  "
-            f"handeye={handeye}"
+            f"handeye={handeye}  mode={he_mode}  track_cam={cam_frame}"
        ),
        LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"),
    ]
@@ -257,7 +268,10 @@ def _launch_setup(context, *args, **kwargs):
    )
    if pub_tf:
        nodes.append(
-            LogInfo(msg=f"静态手眼 {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]")
+            LogInfo(
                msg=f"静态手眼 {tf_parent} → {tf_child} t=[{tx:.3f},{ty:.3f},{tz:.3f}] "
                f"（{he_mode}）"
            )
        )
        nodes.append(static_tf)
    if perception is not None:
@@ -0,0 +1,515 @@
 """跟踪点三道守卫的负向用例（数字全部来自 2026-10-08 真机 arm 89 的日志）。
 ``/tmp/pipeline_stage1.log`` 那一串假 k0（真 k0 是 conf 0.87 uv=(410,246)）::
    766.96  conf=0.58 uv=(76,75)   z=0.392        ← 贴左上角
    767.75  conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near
    768.16  conf=0.53 uv=(143,92)  z=0.376
    768.78  conf=0.73 uv=(12,30)   k0(ok=1551 near=620 借深度)
    770.07  conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度)
 每条用例都问一句"我故意弄坏它，这条会红吗"：负向基线（``test_false_k0_...``
 里带"旧逻辑"字样的几条）就是**故意按旧行为**断言的，它们红了才说明前提没变。
 """
 from __future__ import annotations
 from pathlib import Path
 from typing import Any
 import numpy as np
 import pytest
 import yaml
 from arm_application.feature_guard import (
    FarReacquireConfirm,
    TrackSlewGuard,
    kpt_plausible,
 )
 from arm_application.servo_node import ServoNode
 ROOT = Path(__file__).resolve().parents[3]
 SITE = ROOT / "config" / "site.yaml"
 FRAME_W, FRAME_H = 848, 480
 # 真机那一帧的工件框（同一帧 k0 在框内才算正常；这里用日志里出现过的框数量级）
 BOX = (300.0, 150.0, 620.0, 400.0)
 # 真 k0（must pass）
 TRUE_K0 = (410.0, 246.0)
 # 那几个假 k0（must be rejected）
 FALSE_K0_BORDER = [(76.0, 75.0), (143.0, 92.0), (12.0, 30.0), (169.0, 107.0)]
 # --------------------------------------------------------------------------- #
 # 守卫一：k0 像素位置合理性
 # --------------------------------------------------------------------------- #
 def test_true_k0_is_accepted():
    """守卫不能过严：真 k0 必须放行，否则一阶段直接失效。"""
    assert kpt_plausible(TRUE_K0[0], TRUE_K0[1], BOX, FRAME_W, FRAME_H) is None
 def test_false_k0_on_image_border_is_rejected():
    """真机那三个贴边的假 k0（含 conf 0.73 的那个）必须全部判不可信。"""
    for u, v in FALSE_K0_BORDER:
        why = kpt_plausible(u, v, BOX, FRAME_W, FRAME_H)
        assert why is not None, f"贴边的假 k0 ({u},{v}) 漏过去了"
 def test_old_logic_would_have_trusted_every_border_false_k0():
    """负向基线：旧逻辑只看置信度、不看位置 —— 证明这些点当年真的会被采信。
    ``kpt_plausible`` 若开始返回 None（= 前提变了），说明这条基线已失真，
    上面的"挡住了"就不再是在测同一件事。
    """
    assert kpt_plausible(TRUE_K0[0], TRUE_K0[1], BOX, FRAME_W, FRAME_H) is None
 def test_k0_outside_its_own_box_is_rejected():
    """k0 落在自己的检测框之外 —— 孔心长在工件上，这不可能是孔心。"""
    why = kpt_plausible(700.0, 250.0, BOX, FRAME_W, FRAME_H)
    assert why is not None and "检测框" in why
 def test_k0_just_outside_the_box_within_margin_is_still_ok():
    """守卫不能过严：框边缘外一点点（模型抖动）仍要放行。"""
    # 框宽 320 → 余量 10% = 32px；框右沿 620，取 640（外 20px）
    assert kpt_plausible(640.0, 250.0, BOX, FRAME_W, FRAME_H) is None
    # 外 40px（> 余量）就该拒
    assert kpt_plausible(660.0, 250.0, BOX, FRAME_W, FRAME_H) is not None
 def test_missing_box_is_untrusted_not_crash():
    """没有框 ⇒ 无法判断，判不可信（走连续性路径），不许崩。"""
    why = kpt_plausible(400.0, 250.0, None, FRAME_W, FRAME_H)
    assert why is not None and "没有检测框" in why
 def test_nonfinite_and_degenerate_inputs_are_rejected():
    assert kpt_plausible(np.nan, 250.0, BOX, FRAME_W, FRAME_H) is not None
    assert kpt_plausible(400.0, np.inf, BOX, FRAME_W, FRAME_H) is not None
    assert kpt_plausible(400.0, 250.0, (10.0, 10.0, 10.0, 10.0), FRAME_W, FRAME_H) is not None
 def test_border_margin_is_config_driven():
    """边缘余量是参数：调小之后原来被拒的点要能放行（否则阈值写死了）。"""
    near = (4.0, 200.0)
    assert kpt_plausible(near[0], near[1], (0.0, 100.0, 600.0, 400.0), FRAME_W, FRAME_H) is not None
    assert (
        kpt_plausible(
            near[0], near[1], (0.0, 100.0, 600.0, 400.0), FRAME_W, FRAME_H, border_px=1.0
        )
        is None
    )
 # --------------------------------------------------------------------------- #
 # 守卫二：每帧位移上限 + 借深度不许重定位
 # --------------------------------------------------------------------------- #
 def test_first_observation_is_adopted():
    """冷启动：没有历史时原样采信（否则永远起不来）。"""
    g = TrackSlewGuard(max_step_m=0.04)
    p, why = g.update([0.566, -0.172])
    assert why is None and np.allclose(p, [0.566, -0.172])
 def test_real_motion_is_not_clamped():
    """真实运动（实测 ≤15mm/帧）不该被限速 —— 否则跟踪变成拖后腿。"""
    g = TrackSlewGuard(max_step_m=0.04)
    g.update([0.566, -0.172])
    p, why = g.update([0.578, -0.168])   # 位移 13mm
    assert why is None and np.allclose(p, [0.578, -0.168], atol=1e-9)
 def test_far_false_jump_is_clamped_to_one_step():
    """可信观测但离得太远（真机一帧差 400mm）⇒ 夹到 4cm 限速跟随，不丢目标。"""
    g = TrackSlewGuard(max_step_m=0.04)
    g.update([0.686, -0.113])
    p, why = g.update([0.083, -0.515])   # 那一帧假点把臂带走的方向
    assert p is not None, "可信观测只该限速，不该整个丢掉"
    d = float(np.hypot(p[0] - 0.686, p[1] + 0.113))
    assert abs(d - 0.04) < 1e-9, f"应夹到 40mm，实际 {d*1000:.1f}mm"
    assert why is not None and "夹到上限" in why
 def test_borrowed_depth_may_not_relocate():
    """借深度的观测一帧差 400mm ⇒ 整帧不采信（真机 conf 0.73 那个假点）。"""
    g = TrackSlewGuard(max_step_m=0.04)
    g.update([0.686, -0.113])
    p, why = g.update([0.083, -0.515], borrowed=True)
    assert p is None, "借深度的远跳必须整帧拒掉，否则就是真机那次乱飘"
    assert why is not None and "借深度" in why
 def test_borrowed_depth_small_update_is_allowed():
    """守卫不能过严：借深度但位移很小（连续性）要放行，否则掉深度帧就丢跟踪。"""
    g = TrackSlewGuard(max_step_m=0.04)
    g.update([0.566, -0.172])
    p, why = g.update([0.570, -0.170], borrowed=True)   # 4.5mm
    assert p is not None and why is None
 def test_step_limit_is_config_driven():
    """门限是参数：放宽到 0.15m 后，原来被夹的 60mm 观测要能原样通过。"""
    near = [0.686, -0.053]   # 距起点 60mm
    g = TrackSlewGuard(max_step_m=0.04)
    g.update([0.686, -0.113])
    p, why = g.update(near)
    assert why is not None and tuple(np.round(p, 3)) != tuple(near)
    g2 = TrackSlewGuard(max_step_m=0.15)
    g2.update([0.686, -0.113])
    p2, why2 = g2.update(near)
    assert why2 is None and np.allclose(p2, near, atol=1e-9)
 def test_nonfinite_observation_is_rejected():
    g = TrackSlewGuard(max_step_m=0.04)
    g.update([0.5, -0.1])
    p, why = g.update([np.nan, -0.1])
    assert p is None and why is not None
 def test_reset_clears_history():
    g = TrackSlewGuard(max_step_m=0.04)
    g.update([0.686, -0.113])
    g.reset()
    p, why = g.update([0.083, -0.515])   # reset 后是冷启动 ⇒ 原样采信
    assert why is None and np.allclose(p, [0.083, -0.515])
 # --------------------------------------------------------------------------- #
 # 守卫三：远框重捕获要多帧确认
 # --------------------------------------------------------------------------- #
 def test_near_reacquire_needs_no_confirmation():
    c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
    ok, why = c.allow([400.0, 250.0], 30.0, near_px=120.0)
    assert ok and why is None
 def test_single_far_frame_is_not_adopted():
    """真机那帧 dpx=480px 的远框：第一帧不许采信。"""
    c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
    ok, why = c.allow([76.0, 75.0], 480.0, near_px=120.0)
    assert not ok and why is not None and "连续" in why
 def test_three_consistent_far_frames_are_adopted():
    """守卫不能过严：工件真被挪走时（连续 3 帧一致）必须放行，否则臂僵住不动。"""
    c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
    assert c.allow([76.0, 75.0], 480.0, near_px=120.0)[0] is False
    assert c.allow([80.0, 78.0], 478.0, near_px=120.0)[0] is False
    assert c.allow([78.0, 76.0], 479.0, near_px=120.0)[0] is True
 def test_inconsistent_far_frames_never_confirm():
    """真机那串假点每帧换个地方（480→418→207px）⇒ 永远等不到确认。"""
    c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
    for cx, cy in ((76.0, 75.0), (255.0, 248.0), (143.0, 92.0), (12.0, 30.0), (400.0, 250.0)):
        assert c.allow([cx, cy], 300.0, near_px=120.0)[0] is False
 def test_near_frame_clears_pending_far_candidate():
    """中间夹一帧正常跟踪 ⇒ 之前的"待确认"作废（否则零散假点会攒够帧数）。"""
    c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
    c.allow([76.0, 75.0], 480.0, near_px=120.0)
    c.allow([78.0, 76.0], 479.0, near_px=120.0)
    c.allow([410.0, 246.0], 20.0, near_px=120.0)    # 正常帧 → 清空
    ok, _ = c.allow([76.0, 75.0], 480.0, near_px=120.0)
    assert not ok, "正常帧之后的远框应当从 1 帧重新数"
 # --------------------------------------------------------------------------- #
 # 接线：配置 + 调用点
 # --------------------------------------------------------------------------- #
 def _site() -> dict:
    return yaml.safe_load(SITE.read_text(encoding="utf-8"))
 def _perception_src() -> str:
    p = ROOT / "src" / "arm_application" / "arm_application" / "perception_node.py"
    assert p.is_file(), f"找不到 {p}"
    return p.read_text(encoding="utf-8")
 def _fn_body(src: str, name: str) -> str:
    """取 ``def name(...)`` 的函数体（到下一个顶层 ``def`` 或文件尾）。"""
    i = src.index(f"def {name}(")
    j = src.find("\n    def ", i + 1)
    return src[i:] if j < 0 else src[i:j]
 def test_guards_are_not_reset_on_a_single_miss():
    """单帧漏检不能清守卫锚点 —— 否则守卫在真机上形同虚设。
    真机 2026-10-08 的日志里 ``检测漏检`` 几乎每隔一两帧就出现一次。若每漏一帧
    就 ``_slew.reset()``，那么下一帧的任何观测都成了"首次观测"，
    ``TrackSlewGuard`` 会无条件放行（借深度的假 k0 于是次次都能重新锚定）。
    正确做法：只在**长丢**（``miss_clear_frames`` 帧）走 ``_clear_track`` 时才冷启动。
    """
    src = _perception_src()
    # 单帧漏检分支（best is None → 计数 → 长丢判断之前）不许 reset 守卫
    i = src.index("if best is None:")
    j = src.index("if self._miss_count >= self.miss_clear_frames:", i)
    miss_branch = src[i:j]
    assert "_slew.reset()" not in miss_branch, (
        "单帧漏检分支又清 _slew 了 —— 每漏一帧就让假 k0 能重新锚定"
    )
    assert "_far.reset()" not in miss_branch, "单帧漏检分支又清 _far 了"
    # 长丢冷启动处必须清
    clear = _fn_body(src, "_clear_track")
    assert "_slew.reset()" in clear, "长丢时没清 _slew —— 真丢目标后追不回来"
    assert "_far.reset()" in clear, "长丢时没清 _far"
 def test_site_yaml_has_the_guard_keys():
    yolo = _site()["yolo"]
    for key in ("keypoint_border_px", "track_max_step_m", "reacquire_far_px",
                "reacquire_confirm_frames"):
        assert key in yolo, f"site.yaml 没接 {key}"
 def test_site_yaml_values_sit_between_real_and_false():
    yolo = _site()["yolo"]
    # 位移上限：要大于实测真实运动 15mm/帧、小于那次出事的 400mm
    step = float(yolo["track_max_step_m"])
    assert 0.015 < step < 0.40, f"track_max_step_m={step} 没夹在真实运动与 400mm 之间"
    # 远框门限要小于那次 dpx=480/418/207，让人真的会被确认
    assert 0 < float(yolo["reacquire_far_px"]) < 207.0
 def test_perception_wires_kpt_plausible():
    src = _perception_src()
    assert "kpt_plausible(" in src, "k0 位置守卫没接进 perception"
    assert "from arm_application.feature_guard import" in src or "feature_guard import" in src
 def test_perception_wires_slew_and_reacquire_guards():
    src = _perception_src()
    assert "TrackSlewGuard(" in src, "位移上限没接"
    assert "FarReacquireConfirm(" in src, "远框重捕获确认没接"
 def _export_site_env():
    """import scripts/lib/export_site_env（launch 也是这么接的）。"""
    import sys
    lib = ROOT / "scripts" / "lib"
    if str(lib) not in sys.path:
        sys.path.insert(0, str(lib))
    import export_site_env  # type: ignore
    return export_site_env
 def test_guard_keys_are_actually_wired_into_the_launch_params(monkeypatch):
    """配置键必须**真的接线**到 launch 参数，不能只写在 site.yaml 里。
    这是 R1.1 的机器化执行：节点的 declare_parameter 默认值恰好等于 site.yaml
    当前值，所以"没接线"不会报错，只会让以后改 site.yaml 毫无反应 —— 最难查的
    一类问题。判据：把 site.yaml 的值改成一个特异值，导出的参数必须跟着变。
    """
    # 清掉环境里的选臂变量，保证这里选的是 probe_robot 写的那台（可复现）。
    monkeypatch.delenv("ROBOT_SN", raising=False)
    monkeypatch.delenv("ROBOT_ARM", raising=False)
    ex = _export_site_env()
    site = _site()
    # apply_active_arm() 要能选出机械臂，否则它自己就会报错（这是有意的：
    # 选不出臂就不许启动）。这里给一个最小可解析的 robot 段，值全部来自真实
    # site.yaml，只是把待检查的键换成特异值。
    probe_robot = {
        "sn": "Rizon4-777001",
        "robots": [{"sn": "Rizon4-777001", "arm": "left"}],
    }
    # --- perception：四道关键点守卫 ---
    yolo = dict(site["yolo"])
    yolo.update({
        "keypoint_border_px": 11.0,
        "track_max_step_m": 0.033,
        "reacquire_far_px": 99.0,
        "reacquire_confirm_frames": 5,
    })
    probe = {"yolo": yolo, "robot": probe_robot}
    popt = ex.perception_overrides(probe)
    assert popt["keypoint_border_px"] == pytest.approx(11.0)
    assert popt["track_max_step_m"] == pytest.approx(0.033)
    assert popt["reacquire_far_px"] == pytest.approx(99.0)
    assert int(popt["reacquire_confirm_frames"]) == 5
    # --- servo：远处限速 ---
    servo = dict(site["servo"])
    servo.update({"slow_far_m": 0.021, "v_slow_xy": 0.067})
    sopt = ex.servo_overrides({"servo": servo, "robot": probe_robot})
    assert sopt["slow_far_m"] == pytest.approx(0.021), "slow_far_m 没接线（改了配置也不生效）"
    assert sopt["v_slow_xy"] == pytest.approx(0.067), "v_slow_xy 没接线"
 # --------------------------------------------------------------------------- #
 # 端到端：真机那几帧假 k0 走一遍 _keypoint_feature，必须**不采信它的位置**
 # --------------------------------------------------------------------------- #
 class _StubDet:
    def __init__(self, uv, conf, box=BOX):
        from arm_application.pose_detector import Detection
        self._d = Detection(
            xyxy=np.asarray(box, dtype=float), conf=0.9, cls_id=0, cls_name="workpiece",
            kpts=np.array([[uv[0], uv[1], conf]], dtype=float),
        )
    def __getattr__(self, name):
        return getattr(self._d, name)
 class _StubKpt:
    """``_keypoint_feature`` 用到的成员。"""
    def __init__(self, *, frac=None, depth_at=0.22, depth_box=0.22):
        self.logs = []
        self.keypoint_index = 0
        self.keypoint_min_conf = 0.30
        self.keypoint_border_px = 6.0
        self._kpt_box_frac = None if frac is None else np.asarray(frac, dtype=float)
        self._last_good_z = None
        self._depth_at = depth_at
        self._depth_box = depth_box
    class _L:
        def __init__(self, sink):
            self._s = sink
        def _add(self, m, **_):
            self._s.append(str(m))
        info = warn = error = debug = _add
    def get_logger(self):
        return self._L(self.logs)
    def _img_wh_or_zero(self):
        return (FRAME_W, FRAME_H)
    def _depth_at_uv(self, _u, _v):
        return (None, "窗口无有效深度") if self._depth_at is None else (self._depth_at, "win6")
    def _depth_in_box(self, _b):
        return (None, "框内无有效深度") if self._depth_box is None else (self._depth_box, "框内中位")
    def _pixel_to_cam(self, u, v, z):
        return np.array([(u - 424.0) * z / 380.0, (v - 240.0) * z / 380.0, z])
 def _kpt_feature(stub, uv, conf):
    from arm_application.perception_node import PerceptionNode
    return PerceptionNode._keypoint_feature(stub, _StubDet(uv, conf))
 def test_real_true_k0_is_used_as_is():
    """真 k0（conf 0.87 uv=(410,246)，框内、不贴边）必须原样用它的 uv。"""
    feat = _kpt_feature(_StubKpt(), TRUE_K0, 0.87)
    assert feat is not None
    assert tuple(np.round(feat.uv)) == TRUE_K0, "真 k0 的 uv 被改了 —— 守卫过严"
    assert feat.borrowed is False  # 该像素有自己的深度
 def test_border_false_k0_is_not_used_even_at_conf_073():
    """真机那帧 conf=0.73 uv=(12,30)（贴画面角）：**不许**用它的位置。
    有可信历史偏移时走"按偏移重建"，位置必须回到历史点上（不是那个角落）。
    """
    frac = np.array([0.25, -0.25])          # 锁定阶段学到的 k0 相对框偏移
    feat = _kpt_feature(_StubKpt(frac=frac), (12.0, 30.0), 0.73)
    assert feat is not None, "有可信历史就不该整帧丢掉"
    assert not np.allclose(feat.uv, [12.0, 30.0]), "贴边的假 k0 被采信了 —— 就是它把臂带走的"
    # 重建点在框上，且等于框心 + 历史偏移
    cx = 0.5 * (BOX[0] + BOX[2])
    cy = 0.5 * (BOX[1] + BOX[3])
    bw = BOX[2] - BOX[0]
    bh = BOX[3] - BOX[1]
    assert tuple(np.round(feat.uv)) == (round(cx + 0.25 * bw), round(cy - 0.25 * bh))
 def test_border_false_k0_without_history_is_dropped():
    """冷启动时贴边假 k0 直接不用它（返回 None，交由调用方回落）。"""
    feat = _kpt_feature(_StubKpt(frac=None), (12.0, 30.0), 0.73)
    assert feat is None, "没有可信历史时贴边假 k0 必须被丢掉"
 def test_borrowed_depth_is_flagged():
    """k0 处读不到深度 ⇒ 借框内深度，并**显式标记** borrowed（下游据此禁重定位）。"""
    feat = _kpt_feature(_StubKpt(depth_at=None, depth_box=0.22), TRUE_K0, 0.87)
    assert feat is not None and feat.borrowed is True
    assert "借深度" in feat.note
 def test_true_k0_outside_box_is_not_used():
    """落在自己框外的"k0"（孔心不在工件外）同样不许采信位置。"""
    frac = np.array([0.1, 0.1])
    feat = _kpt_feature(_StubKpt(frac=frac), (700.0, 250.0), 0.90)
    assert feat is not None
    assert not np.allclose(feat.uv, [700.0, 250.0])
 # --------------------------------------------------------------------------- #
 # 守卫四（伺服侧）：远处限速 —— 与"目标真不真"无关，只保证"追错时走得少"
 # --------------------------------------------------------------------------- #
 class _StubServo:
    def __init__(self, *, slow_far_m: float = 0.05, v_slow_xy: float = 0.08):
        self.slow_far_m = slow_far_m
        self.v_slow_xy = v_slow_xy
 def _slow(st, nerr: float, v_xy: float):
    return ServoNode._slow_xy_limit(st, nerr, v_xy)  # type: ignore[arg-type]
 def test_far_target_is_slowed_down():
    """真机那次的 400mm 级误差必须被限速（否则 0.7s 带走 400mm）。"""
    lim, why = _slow(_StubServo(), 0.400, 0.45)
    assert lim == pytest.approx(0.08), "远处没限速 —— 就是它把臂带到工作盒角落的"
    assert why is not None and "目标离" in why
 def test_near_target_keeps_full_speed():
    """守卫不能过严：目标近了必须放开速度，否则收敛永远慢吞吞（死区都进不去）。"""
    lim, why = _slow(_StubServo(), 0.020, 0.45)
    assert lim == pytest.approx(0.45) and why is None
 def test_boundary_just_over_the_threshold_is_slowed():
    lim, why = _slow(_StubServo(), 0.051, 0.45)
    assert lim == pytest.approx(0.08) and why is not None
    lim2, _ = _slow(_StubServo(), 0.049, 0.45)
    assert lim2 == pytest.approx(0.45)
 def test_slow_limit_is_config_driven():
    """限速值来自配置：改大之后远处目标就该跑得更快（否则阈值写死、配置没接线）。"""
    lim, _ = _slow(_StubServo(slow_far_m=0.30, v_slow_xy=0.20), 0.400, 0.45)
    assert lim == pytest.approx(0.20)
    # 比门限更近 ⇒ 完全放开
    lim2, _ = _slow(_StubServo(slow_far_m=0.50, v_slow_xy=0.20), 0.400, 0.45)
    assert lim2 == pytest.approx(0.45)
 def test_slow_limit_never_raises_the_speed():
    """限速只能降不能升：v_max_xy 本来就比 v_slow_xy 小时不许被"抬"上去。"""
    lim, _ = _slow(_StubServo(), 0.400, 0.03)
    assert lim == pytest.approx(0.03)
 def test_site_yaml_has_the_slow_approach_keys():
    servo = _site()["servo"]
    for key in ("slow_far_m", "v_slow_xy"):
        assert key in servo, f"site.yaml 没接 {key}"
    # 必须真的能拦住真机那次：门限 < 400mm、限速 < v_max_xy
    assert 0.02 < float(servo["slow_far_m"]) < 0.40
    assert float(servo["v_slow_xy"]) < float(servo["v_max_xy"])
 def test_servo_wires_the_slow_limit():
    p = ROOT / "src" / "arm_application" / "arm_application" / "servo_node.py"
    src = p.read_text(encoding="utf-8")
    assert "self._slow_xy_limit(nerr, v_xy)" in src, "限速没接进 hover 段"
    assert "def _slow_xy_limit(" in src
    assert 'declare_parameter("slow_far_m"' in src
@@ -0,0 +1,357 @@
 """跟踪点"假跳变"的两道防线（负向测试：改回去必须变红）。
 为什么单独立一条（2026-10-08 真机实测，arm 89 一阶段跑 150s 没锁上）
 -------------------------------------------------------------------
 日志原话（gongjian0929，跟踪锁高 0.370m）：
    0659.551  死区保持 |e_xy|=3.0mm              ← 其实已经锁定！误差只剩 3mm
    0659.598  k0 置信度 0.01 < 0.30，本帧不用它
    0659.604  HANDEYE snap feat_base=(0.675,-0.099)  ← 跟踪点瞬间横移 89mm
    0660.121  v=(-0.206,+0.400) |e_xy|=130mm     ← 伺服以 0.45m/s 去追这个假点
    0660.561  过期保持 —— 不跟踪冻结的检测框      ← 画面糊掉，目标彻底丢失
 根因是**两处**，各自都要能独立失败：
  1. ``perception_node._keypoint_feature``：k0 掉信时兜底把跟踪点从 k0 切到
     "框中心" —— 而框中心离 k0 有 89mm（k0 是工件上的孔，不在框心）。正确的兜底
     是**借 k0 相对框的归一化偏移**在框上重建 k0：位置连续、工件动时还能跟着动。
  2. ``servo_node._update_feat_track``：``dxy > predict_jump_m`` 时单帧就
     ``reset()`` **采信**假跳。30Hz 相机 + 工件不动，一帧跳 89mm 物理上不可能
     ⇒ 必须连续 N 帧指向同一新位置才认。
 用例全离线：用打桩对象直接调被测方法（不建 ROS 实体、不发运动指令），
 与 ``test_stage2_descent_clamp`` 同一套做法。
 """
 from __future__ import annotations
 from typing import Any, List, Optional
 import numpy as np
 from arm_application.feature_jump_guard import (
    ACCEPT,
    HOLD,
    REJECT,
    FeatureJumpGate,
 )
 from arm_application.kalman_filter import KalmanFilter3D
 from arm_application.perception_node import PerceptionNode
 from arm_application.servo_node import ServoNode
 from arm_application.pose_detector import Detection
 # 真机那次的关键数字
 _JUMP_M = 0.08  # servo.predict_jump_m
 _K0_UV = np.array([296.0, 262.0])  # 锁定瞬间的 k0
 _BOX = np.array([180.0, 120.0, 620.0, 420.0])  # 工件框
 # 真机日志里那一跳：(0.630,-0.176) → (0.675,-0.099)
 _FEAT_BEFORE = np.array([0.630, -0.176])
 _FEAT_AFTER = np.array([0.675, -0.099])
 class _StubLogger:
    """把日志写进调用方给的 sink（和被测对象共享同一个 list）。"""
    def __init__(self, sink: List[str]) -> None:
        self._sink = sink
    def _add(self, msg: str, **_: Any) -> None:
        self._sink.append(str(msg))
    info = _add
    warn = _add
    error = _add
    debug = _add
 # --------------------------------------------------------------------------- #
 # 防线 1：perception 的 k0 掉信兜底 —— 借偏移，不借绝对位置
 # --------------------------------------------------------------------------- #
 class _StubPerception:
    """``_keypoint_feature`` 用到的成员全部打桩。"""
    def __init__(
        self,
        *,
        kpt_conf: float,
        last_uv: Optional[np.ndarray],
        frac: Optional[np.ndarray],
        depth_at: Optional[float] = 0.21,
        depth_box: Optional[float] = 0.22,
    ) -> None:
        self.logs: List[str] = []
        self.keypoint_index = 0
        self.keypoint_min_conf = 0.30
        # k0 位置守卫的参数与画面尺寸（用真机 D405 的 848x480，见 feature_guard）
        self.keypoint_border_px = 6.0
        self._kpt_box_frac = None if frac is None else np.asarray(frac, dtype=float)
        self._last_uv = None if last_uv is None else np.asarray(last_uv, dtype=float)
        self._kpt_conf = float(kpt_conf)
        self._depth_at = depth_at
        self._depth_box = depth_box
    def get_logger(self) -> Any:
        return _StubLogger(self.logs)
    def _img_wh_or_zero(self) -> tuple:
        # 真机 D405 彩色图尺寸（k0 位置守卫要用它判"贴画面边"）
        return (848, 480)
    def _depth_at_uv(self, _u: float, _v: float) -> tuple:
        if self._depth_at is None:
            return None, "窗口无有效深度"
        return float(self._depth_at), "win6"
    def _depth_in_box(self, _xyxy: Any) -> tuple:
        if self._depth_box is None:
            return None, "框内无有效深度"
        return float(self._depth_box), "框内中位"
    def _pixel_to_cam(self, u: float, v: float, z: float) -> np.ndarray:
        # 只要求"同一 uv 得同一 3D 点"，用最简针孔映射即可（不参与本用例断言）
        return np.array([(u - 400.0) * z / 380.0, (v - 240.0) * z / 380.0, z])
    def det(self, kpt_uv: np.ndarray) -> Detection:
        return Detection(
            xyxy=_BOX.copy(),
            conf=0.9,
            cls_id=0,
            cls_name="workpiece",
            kpts=np.array([[kpt_uv[0], kpt_uv[1], self._kpt_conf]], dtype=float),
        )
 def _feat(stub: _StubPerception, kpt_uv: np.ndarray) -> Any:
    """直接调被测方法（打桩对象冒充 self）。"""
    return PerceptionNode._keypoint_feature(stub, stub.det(kpt_uv))
 def test_kpt_low_conf_rebuilds_on_box_not_box_center():
    """k0 掉信时**不许**把跟踪点横移到框中心。"""
    # 锁定阶段记下的 k0 相对框偏移：k0 在框心右侧偏上
    frac = np.array([0.25, -0.25])
    stub = _StubPerception(kpt_conf=0.05, last_uv=_K0_UV, frac=frac)
    box_cx = 0.5 * (_BOX[0] + _BOX[2])
    box_cy = 0.5 * (_BOX[1] + _BOX[3])
    bw = _BOX[2] - _BOX[0]
    bh = _BOX[3] - _BOX[1]
    feat = _feat(stub, kpt_uv=np.array([150.0, 60.0]))  # 掉信帧的 k0 是垃圾值
    assert feat is not None, "有可信历史时必须给得出跟踪点（不能退回 None→框中心）"
    expect = np.array([box_cx + frac[0] * bw, box_cy + frac[1] * bh])
    assert np.allclose(feat.uv, expect, atol=1e-6)
    # 负向：绝不能等于框中心
    assert not np.allclose(feat.uv, [box_cx, box_cy], atol=1.0), (
        f"跟踪点落回框中心了 uv={feat.uv} 框心=({box_cx},{box_cy})"
    )
    assert "保持" in feat.note
 def test_kpt_low_conf_rebuild_follows_box_motion():
    """框移动时，重建出来的 k0 要跟着动 —— 相对框的偏移必须保持。"""
    frac = np.array([0.25, -0.25])
    stub = _StubPerception(kpt_conf=0.02, last_uv=_K0_UV, frac=frac)
    moved_box = _BOX + np.array([40.0, 0.0, 40.0, 0.0])  # 整框右移 40px
    d = stub.det(np.array([150.0, 60.0]))
    d.xyxy = moved_box
    feat = PerceptionNode._keypoint_feature(stub, d)
    box_cx = 0.5 * (moved_box[0] + moved_box[2])
    box_cy = 0.5 * (moved_box[1] + moved_box[3])
    bw = moved_box[2] - moved_box[0]
    bh = moved_box[3] - moved_box[1]
    expect = np.array([box_cx + frac[0] * bw, box_cy + frac[1] * bh])
    assert np.allclose(feat.uv, expect, atol=1e-6)
    # 相对框心的偏移（归一化后）没变 —— 这才是"跟着走且不跳"的含义
    got_frac = np.array(
        [(feat.uv[0] - box_cx) / bw, (feat.uv[1] - box_cy) / bh]
    )
    assert np.allclose(got_frac, frac, atol=1e-9)
 def test_kpt_trusted_updates_offset_and_keeps_k0_uv():
    """k0 可信时必须用 k0 自己的 uv，并顺手更新偏移记录。"""
    stub = _StubPerception(kpt_conf=0.87, last_uv=None, frac=None)
    feat = _feat(stub, kpt_uv=_K0_UV)
    assert feat is not None
    assert np.allclose(feat.uv, _K0_UV, atol=1e-6)
    assert stub._kpt_box_frac is not None, "可信帧必须记下 k0 相对框的偏移"
    box_cx = 0.5 * (_BOX[0] + _BOX[2])
    bw = _BOX[2] - _BOX[0]
    assert abs(float(stub._kpt_box_frac[0]) - (_K0_UV[0] - box_cx) / bw) < 1e-9
 def test_kpt_no_history_still_falls_back():
    """冷启动（从没见过可信 k0）时没有更好的信息 —— 此时才允许返回 None。"""
    stub = _StubPerception(kpt_conf=0.05, last_uv=None, frac=None)
    assert _feat(stub, kpt_uv=np.array([150.0, 60.0])) is None
 def test_kpt_trusted_but_no_depth_keeps_k0_uv_and_borrows_depth():
    """只有深度读不到时：XY 仍用 k0（不许横移），深度可以借框内。"""
    stub = _StubPerception(kpt_conf=0.80, last_uv=None, frac=None, depth_at=None)
    feat = _feat(stub, kpt_uv=_K0_UV)
    assert feat is not None
    assert np.allclose(feat.uv, _K0_UV, atol=1e-6), "深度不可用时把 XY 也弄丢了"
    assert abs(feat.z - 0.22) < 1e-9, "应从框内借到深度"
 # --------------------------------------------------------------------------- #
 # 防线 2：servo 的跳变确认门限
 # --------------------------------------------------------------------------- #
 def test_jump_gate_rejects_single_frame_spurious_jump():
    """真机那一跳：单帧 89mm，必须判 hold（不采信）。"""
    gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
    assert float(np.hypot(*(_FEAT_AFTER - _FEAT_BEFORE))) > _JUMP_M  # 前提：确实是大跳
    assert gate.update(_FEAT_AFTER, _FEAT_BEFORE) == FeatureJumpGate.HOLD
 def test_jump_gate_confirms_persistent_relocation():
    """真正换了位置时，连续 3 帧一致就要认（不能永远 hold 住）。"""
    gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
    new = np.array([0.760, -0.180])
    assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.HOLD
    assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.HOLD
    assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.JUMP
 def test_jump_gate_near_is_passthrough():
    """正常小位移不该被拦住（真实运动 ≤15mm/帧）。"""
    gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
    for d in (0.005, -0.010, 0.015):
        assert gate.update(
            np.array([_FEAT_BEFORE[0] + d, _FEAT_BEFORE[1]]), _FEAT_BEFORE
        ) == FeatureJumpGate.NEAR
 def test_jump_gate_inconsistent_candidates_never_confirm():
    """每帧跳到不同地方（k0 掉信时的乱跳）永远确认不了。"""
    gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
    for p in ([0.675, -0.099], [0.570, -0.046], [0.700, -0.250], [0.520, -0.020]):
        assert gate.update(np.array(p), _FEAT_BEFORE) == FeatureJumpGate.HOLD
    assert gate.hold_total == 4
 def test_jump_gate_single_confirm_frame_is_explicit_legacy():
    """confirm_frames=1 是显式退回旧行为 —— 是"选择"，不是静默降级。"""
    gate = FeatureJumpGate(_JUMP_M, confirm_frames=1)
    assert gate.update(_FEAT_AFTER, _FEAT_BEFORE) == FeatureJumpGate.JUMP
 class _StubServo:
    """``_update_feat_track`` 用到的成员全部打桩。
    ``ws_min/ws_max`` 取 site.yaml 现行值（法兰工作盒），特征点在盒外时会被判误检。
    """
    def __init__(self) -> None:
        self.logs: List[str] = []
        self.predict_jump_m = _JUMP_M
        self.feat_ws_margin_m = 0.05
        # config/site.yaml 的 servo.workspace_xyz_min/max
        self.ws_min = np.array([0.08, -0.55, 0.18], dtype=float)
        self.ws_max = np.array([0.85, 0.55, 0.65], dtype=float)
        self._feat_kf = KalmanFilter3D(q_pos=2.0e-3, q_vel=1.5e-1, r_pos=4.0e-4)
        self._jump_gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
    def get_logger(self) -> Any:
        return _StubLogger(self.logs)
    # 真实方法（纯计算，不碰 ROS）—— 挂上来，避免把它的逻辑抄一份到测试里
    _feat_workspace_ok = ServoNode._feat_workspace_ok
 def _update(stub: _StubServo, p_base: np.ndarray, *, was_lost: bool, now_m: float) -> str:
    return ServoNode._update_feat_track(stub, p_base, was_lost=was_lost, now_m=now_m)
 # 真机实测的"假点"：落在基座后方，而工件在 (0.5,-0.12)
 _BOGUS_FAR = np.array([-0.102, -0.648, -0.01])
 def test_servo_rejects_feature_outside_workspace():
    """物理合理性：工作盒外的点必须被判误检（当"没有目标"，不是"钉住"）。"""
    stub = _StubServo()
    p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
    assert _update(stub, p0, was_lost=True, now_m=1.0) == ACCEPT
    # 真机那一串假点：(-0.102,-0.648) 在工件反方向、x 甚至为负
    assert _update(stub, _BOGUS_FAR, was_lost=False, now_m=1.033) == REJECT
    assert "工作盒外" in " ".join(stub.logs)
 def test_servo_reject_wins_even_if_bogus_is_persistent():
    """假点就算"稳定地"出现在盒外 N 帧，也必须一直 reject（不能被确认为重定位）。"""
    stub = _StubServo()
    p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
    _update(stub, p0, was_lost=True, now_m=1.0)
    for i in range(1, 6):  # 同一个盒外点重复来 5 帧
        assert _update(stub, _BOGUS_FAR, was_lost=False, now_m=1.0 + 0.033 * i) == REJECT
    # 位置必须还钉在原处（没有被假点带跑）
    assert np.allclose(stub._feat_kf.position[:2], _FEAT_BEFORE, atol=5e-4)
 def test_servo_reject_holds_when_kf_not_initialized():
    """冷启动就收到盒外点：不能把它当基准点初始化。"""
    stub = _StubServo()
    assert _update(stub, _BOGUS_FAR, was_lost=True, now_m=1.0) == REJECT
    assert not stub._feat_kf.initialized, "盒外的假点被当成了初始基准"
 def test_feat_workspace_ok_accepts_real_workpiece():
    """真实工件点（0.5,-0.12）绝不能被误判成盒外。"""
    stub = _StubServo()
    assert ServoNode._feat_workspace_ok(stub, np.array([0.500, -0.120, 0.022])) is True
    # 盒边界附近（含余量内）也放行
    assert ServoNode._feat_workspace_ok(stub, np.array([0.08, -0.55, 0.022])) is True
 def test_servo_holds_position_on_spurious_jump():
    """真机场景：锁定后单帧假跳 —— 位置必须钉住不动（旧代码会 reset 追过去）。"""
    stub = _StubServo()
    p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
    assert _update(stub, p0, was_lost=True, now_m=1.0) == ACCEPT  # 首次捕获
    held_pos = stub._feat_kf.position.copy()
    assert np.allclose(held_pos[:2], _FEAT_BEFORE, atol=1e-6)
    jumped = np.array([_FEAT_AFTER[0], _FEAT_AFTER[1], -0.008])
    assert _update(stub, jumped, was_lost=False, now_m=1.033) == HOLD, "必须判 hold"
    assert np.allclose(
        stub._feat_kf.position[:2], _FEAT_BEFORE, atol=5e-4
    ), "hold 之后位置被假点带跑了"
 def test_servo_confirms_real_relocation_after_n_frames():
    """真换了位置：连续 3 帧一致后必须采信（不能把 hold 变成"卡死"）。"""
    stub = _StubServo()
    p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
    _update(stub, p0, was_lost=True, now_m=1.0)
    new = np.array([0.760, -0.180, -0.007])
    assert _update(stub, new, was_lost=False, now_m=1.033) == HOLD
    assert _update(stub, new, was_lost=False, now_m=1.066) == HOLD
    assert _update(stub, new, was_lost=False, now_m=1.099) == ACCEPT, "第 3 帧该采信了"
    assert np.allclose(stub._feat_kf.position[:2], new[:2], atol=2e-3)
 def test_servo_hold_after_convergence_keeps_gate_velocity_zero():
    """hold 期间速度必须清零，否则旧速度会把"不动"变成"匀速跑掉"。"""
    stub = _StubServo()
    # 给 KF 灌一个明显不为零的速度
    p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
    _update(stub, p0, was_lost=True, now_m=1.0)
    for i, dx in enumerate((0.004, 0.008, 0.012), start=1):
        _update(
            stub,
            np.array([_FEAT_BEFORE[0] + dx, _FEAT_BEFORE[1], -0.007]),
            was_lost=False,
            now_m=1.0 + 0.033 * i,
        )
    assert float(np.hypot(*stub._feat_kf.velocity[:2])) > 1e-3, "前提：速度非零"
    jumped = np.array([_FEAT_AFTER[0], _FEAT_AFTER[1], -0.008])
    assert _update(stub, jumped, was_lost=False, now_m=1.2) == HOLD
    assert float(np.hypot(*stub._feat_kf.velocity[:2])) < 1e-9, "hold 时速度没清零"
@@ -0,0 +1,200 @@
 """腕部 FT 兜底不许"静默失效"：数据源没出数时必须喊出来，且配置必须指向有数的话题。
 为什么单独立一条（2026-10-08 真机实测）
 --------------------------------------
 二阶段轻降把"规尖碰工件"的兜底押在腕部 FT 上：``abs(fz − 基线) > 2.5N ⇒ 停``。
 真机上应用在用的 ``/Rizon4_063989/ft_sensor_wrench`` 却是**恒 (0,0,0)**：
    /Rizon4_063989/ft_sensor_wrench        force.z = 0.0          ← 应用在用
    /Rizon4_063989/external_wrench_in_tcp  force.z = -53.84       ← 才有数（负载）
 于是 ``abs(0 − 0) > 2.5`` 永远为假 —— **接触兜底永远触发不了**，而日志里一切正常：
 话题收得到、回调在跑、``wrench_live``（只看"够不够新"）还报"活"。这是最危险的
 静默失效：轻降会一直往下走而没有任何力保护。
 两处修（互补，都要有）：
 1. 配置：真机 ``servo.ft_wrench_topic`` 指向有数的 ``external_wrench_in_tcp``
   （判据是"相对轻降起始基线的增量"，与 54N 那个偏置无关）；仿真**必须**覆盖回
   ``/sim/ft_sensor_wrench``（仿真没有 external_wrench_in_tcp 这个话题）。
 2. 守卫：``gauge_fsm.wrench_all_zero`` —— 带负载的 FT 不可能六个分量**同时**恰好为
   0，所以"全零 > 5s"= 数据源没出数，节点必须报 error（一次）。
 这些用例全是**离线**的：``_on_wrench`` 只用到少量成员，用打桩对象即可。
 """
 from __future__ import annotations
 import time
 from typing import Any, List
 import yaml
 from geometry_msgs.msg import Wrench, WrenchStamped
 from arm_application import stage2_locate_node as s2mod
 from arm_application.gauge_fsm import wrench_all_zero
 from arm_application.stage2_locate_node import Stage2Locate
 ROOT = __import__("pathlib").Path(__file__).resolve().parents[3]
 SITE = ROOT / "config" / "site.yaml"
 class _Logger:
    def __init__(self, sink: List[str]) -> None:
        self._sink = sink
    def _add(self, msg: str) -> None:
        self._sink.append(str(msg))
    info = _add
    warn = _add
    error = _add
 class _Stub:
    """``_on_wrench`` 用到的成员全部打桩。"""
    def __init__(self) -> None:
        self.logs: List[str] = []
        self._logger = _Logger(self.logs)
        self._ft_fz = None
        self._t_ft = None
        self._ft_zero_since = None
        self._ft_zero_warned = False
        self.ft_wrench_topic = "/Rizon4_063989/ft_sensor_wrench"
    def get_logger(self) -> Any:
        return self._logger
 def _wrench_msg(*, fz: float = 0.0, tx: float = 0.0) -> WrenchStamped:
    m = WrenchStamped()
    m.wrench = Wrench()
    m.wrench.force.x = 0.0
    m.wrench.force.y = 0.0
    m.wrench.force.z = float(fz)
    m.wrench.torque.x = float(tx)
    m.wrench.torque.y = 0.0
    m.wrench.torque.z = 0.0
    return m
 # --------------------------------------------------------------------------- #
 # wrench_all_zero：区分"安静"与"没出数"
 # --------------------------------------------------------------------------- #
 def test_all_zero_is_flagged():
    assert wrench_all_zero(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) is True
 def test_a_real_load_is_not_all_zero():
    """真机 external_wrench_in_tcp 的读数（≈负载 54N）不能被判成"没出数"。"""
    assert wrench_all_zero(0.036, 0.32, -53.84, 0.0, 0.0, 0.0) is False
 def test_tiny_noise_is_not_all_zero():
    """很小但活的读数（噪声量级）也不算"没出数" —— 否则会误报。"""
    assert wrench_all_zero(1.0e-3, 0.0, 0.0, 0.0, 0.0, 0.0) is False
 def test_torque_alone_is_enough_to_prove_it_is_alive():
    assert wrench_all_zero(0.0, 0.0, 0.0, 0.02, 0.0, 0.0) is False
 # --------------------------------------------------------------------------- #
 # 节点守卫：全零 > 5s 报 error（一次），有数则安静
 # --------------------------------------------------------------------------- #
 def test_all_zero_for_over_5s_raises_error_once():
    st = _Stub()
    # 让它一开始就已经"全零了 6 秒"（不 sleep，直接构造状态）
    st._ft_zero_since = time.monotonic() - 6.0
    Stage2Locate._on_wrench(st, _wrench_msg())  # type: ignore[arg-type]
    errs = [m for m in st.logs if "全为 0" in m]
    assert len(errs) == 1, f"数据源恒零却{'没报' if not errs else '报多了'} —— 静默失效"
    assert "判不到接触" in errs[0], "要说清后果：接触兜底永远不会触发"
    assert "ft_sensor_wrench" in errs[0], "要打出话题名，现场才知道换哪个"
    # 第二次不该再刷屏
    Stage2Locate._on_wrench(st, _wrench_msg())  # type: ignore[arg-type]
    assert len([m for m in st.logs if "全为 0" in m]) == 1, "全零告警只许喊一次"
 def test_a_live_reading_never_warns_and_is_cached():
    st = _Stub()
    Stage2Locate._on_wrench(st, _wrench_msg(fz=-53.84))  # type: ignore[arg-type]
    assert not [m for m in st.logs if "全为 0" in m], "有数还告警 = 误报"
    assert st._ft_fz == -53.84, "轴向力必须缓存下来（_contact_detected 要用）"
    assert st._t_ft is not None, "要记收到时刻"
 def test_losing_data_after_a_live_reading_restarts_the_clock():
    """先有数、后全零：计时要从"变成全零那一刻"重新起算，不能沿用旧起点。"""
    st = _Stub()
    Stage2Locate._on_wrench(st, _wrench_msg(fz=-53.84))  # type: ignore[arg-type]
    assert st._ft_zero_since is None
    Stage2Locate._on_wrench(st, _wrench_msg())  # 转全零
    assert st._ft_zero_since is not None, "转全零要起计时"
 # --------------------------------------------------------------------------- #
 # 源码守卫：这两处都不能被悄悄删掉
 # --------------------------------------------------------------------------- #
 def test_stage2_wires_the_all_zero_guard():
    src = (
        ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py"
    ).read_text(encoding="utf-8")
    assert "wrench_all_zero(" in src, "全零守卫没接进 stage2 的 FT 回调"
    assert "from arm_application.gauge_fsm import" in src and "wrench_all_zero" in src
 # --------------------------------------------------------------------------- #
 # 配置接线：真机指向有数的话题，仿真不许跟着用
 # --------------------------------------------------------------------------- #
 def _robots() -> dict:
    site = yaml.safe_load(SITE.read_text(encoding="utf-8"))
    return {r["sn"]: r for r in site["robot"]["robots"]}
 def test_real_arms_point_at_a_topic_that_actually_has_data():
    """真机 FT 源必须是 external_wrench_in_tcp —— ft_sensor_wrench 实测恒零。"""
    robots = _robots()
    for sn in ("Rizon4-063989", "Rizon4-063991"):
        if sn not in robots:
            continue
        topic = robots[sn]["servo"].get("ft_wrench_topic")
        assert topic, f"{sn} 没配 servo.ft_wrench_topic —— 会退回恒零的 ft_sensor_wrench"
        assert topic.endswith("/external_wrench_in_tcp"), (
            f"{sn} 的 FT 源是 {topic}；真机只有 external_wrench_in_tcp 有数"
        )
        assert sn.replace("-", "_") in topic, "话题名里的 sn 要用下划线（与 broadcaster 一致）"
 def test_sim_overrides_the_real_ft_topic():
    """仿真必须显式覆盖 —— `*servo_left` 带下来的是真机话题，仿真里不存在。"""
    topic = _robots()["sim"]["servo"].get("ft_wrench_topic")
    assert topic == "/sim/ft_sensor_wrench", (
        f"仿真 FT 话题是 {topic}；仿真由 sim_wrist_ft 发 /sim/ft_sensor_wrench"
    )
 def test_node_reads_the_topic_from_site_config_not_hardcoded():
    """话题必须可配（读 servo.ft_wrench_topic），不许把真机话题写死在代码里。
    用语法树取字符串字面量来判（不是文本匹配）—— 这样解释这件事的注释不算数。
    """
    import ast
    src = (
        ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py"
    ).read_text(encoding="utf-8")
    assert 'servo.get("ft_wrench_topic")' in src, "没从 site 配置读话题"
    literals = [
        n.value
        for n in ast.walk(ast.parse(src))
        if isinstance(n, ast.Constant) and isinstance(n.value, str)
    ]
    assert not any("Rizon4_063989" in s for s in literals), (
        "真机 sn/话题被写进了代码字符串（应当只在 site.yaml）"
    )
    # 兜底必须是"按 sn 推导"，而不是某个具体话题
    assert 'f"/{robot_ns}/ft_sensor_wrench"' in src, "兜底不再是按 sn 推导的话题"
@@ -18,6 +18,8 @@ from arm_application.gauge_fsm import (
    STATE_DOC,
    TERMINAL,
    TRANSITIONS,
    ENTRY_HIGH,
    ENTRY_OK,
    Event,
    FsmError,
    GaugeContext,
@@ -31,6 +33,7 @@ from arm_application.gauge_fsm import (
    descend_needed_m,
    descend_progress,
    descend_tol_m,
    hole_entry_verdict,
    relief_tol_m,
    tool_clearance_m,
    wrench_live,
@@ -540,6 +543,63 @@ def test_descend_needed_measures_shoulder_not_tip():
    assert descend_needed_m(0.655, 0.243, 0.412, 0.030) - tip_only == pytest.approx(0.030)
 def test_hole_entry_verdict_flags_gauge_landing_on_the_surface():
    """2026-09-28 实测：同一命令三跑里坏一跑（DONE / ABORT / DONE）。
    坏的那跑：k1 装好了，k2 只下压 0.90mm 就"接触"（正常要 28.6~31.9mm）——
    因为工件在 k1 之后被工具推走了 ~20mm，而 k2 的孔位是 S05 一次性定下的。
    不拦的代价是干转 5 轮 + 最后报一句"转轴偏差…先确认工具系有没有装转接件"，
    把人引去查工具系（那几个都是好的）。
    """
    needed = 0.0319  # 台肩落面还需下压 31.9mm（实测）
    # 坏那跑：0.90 / 31.9 = 3% ⇒ 必须报"没进孔"
    assert hole_entry_verdict(0.0009, needed) == ENTRY_HIGH
    # 好那跑：28.6 / 31.9 = 90% ⇒ 必须放行（否则白扔一整个孔）
    assert hole_entry_verdict(0.0286, needed) == ENTRY_OK
    # 另一跑 k1：31.45 / 31.9 ⇒ 放行
    assert hole_entry_verdict(0.03145, needed) == ENTRY_OK
 def test_hole_entry_verdict_never_flags_the_healthy_band():
    """健康的比值区间（台肩倒角没完全落面、板厚公差）绝不能被判成"没进孔"。
    这是"宁可漏报也不能误报"那一边：误报一次就白扔一整个孔，
    而漏报只是回到旧行为（多烧几轮 + 一句误导）。
    """
    needed = 0.032
    for ratio in (0.85, 0.90, 0.95, 1.00, 1.10):
        assert hole_entry_verdict(needed * ratio, needed) == ENTRY_OK, (
            f"比值 {ratio} 是正常范围，不该判成没进孔"
        )
    # 明确该报的一侧
    for ratio in (0.0, 0.03, 0.10, 0.30, 0.49):
        assert hole_entry_verdict(needed * ratio, needed) == ENTRY_HIGH, (
            f"比值 {ratio} 差着一个台肩高度，必须报"
        )
 def test_hole_entry_verdict_stays_quiet_when_geometry_is_unusable():
    """``needed_m <= 0`` 时不许抢着报第二个错。
    那种情况上面那条几何自检（tool_clearance_m / descend_needed_m）已经在报了，
    这里再报一次只会把真正的第一因埋掉。
    """
    assert hole_entry_verdict(0.0005, 0.0) == ENTRY_OK
    assert hole_entry_verdict(0.0005, -0.01) == ENTRY_OK
 def test_hole_lost_event_aborts_and_has_an_honest_label():
    """事件本身与它的中文标签都必须是实话。
    复用 FORCE_OVER_LIMIT 会让结论报成"力超安全上限"，而这种情况下力根本没超限 ——
    又是一句误导。所以单列 HOLE_LOST，并且标签要能一眼看出是孔位问题。
    """
    assert resolve_next(State.CONTACT, Event.HOLE_LOST, GaugeContext()) == State.ABORT
    label = EVENT_DOC[Event.HOLE_LOST]
    assert "孔" in label, "标签要指明是孔位问题"
    assert "力" not in label, "别把它说成力的问题 —— 力是好的"
 def test_descend_budget_has_margin_and_floor():
    """预算要有余量（容纳对位残差/板厚公差），且再小也有下限。
@@ -273,33 +273,72 @@ def test_roundtrip_is_rotation_matrix_with_det_one():
    assert np.allclose(R @ R.T, np.eye(3), atol=1e-8), "必须是正交阵"
-def test_roundtrip_eye_to_hand_recovers_known_transform():
+def _synthesize_views_eye_to_hand(
-    """eye_to_hand：相机固定在基座，X = cam → base。
+    X_true: np.ndarray, T_target2gripper: np.ndarray, n: int, seed: int = 7
 ):
    """由已知 X_true 生成 n 组**眼在手外**的视图观测。
-    此时坐标链变成 target → camera 与 gripper → base 相互独立，
+    眼在手外的坐标链：相机固定在工位外不动，**标定板装在法兰上随臂动**。
-    求解器内部会把 gripper2base 求逆，这条分支容易写错，单独测。
+        T_t2b = T_g2b · T_t2g      （板在基座 = 法兰在基座 · 板在法兰，T_t2g 固定）
        T_t2b = X · T_t2c          （X = T_cam2base）
    所以给定任意的 T_g2b，可反推出该视图下应该观测到的 T_t2c：
        T_t2c = inv(X) · T_g2b · T_t2g
    **为什么板必须在法兰上**：相机与基座都不动，唯一会变的就是带着板走的法兰；
    板若像眼在手上那样摆在台面上不动，T_t2c 就恒定不变，方程直接退化。
    """
-    rng = np.random.default_rng(3)
+    rng = np.random.default_rng(seed)
    X_true = _rand_transform(rng)  # T_cam2base
    Rg, tg, Rc, tc = [], [], [], []
-    for _ in range(12):
+    Xinv = np.linalg.inv(X_true)
-        T_g2b = _rand_transform(rng)          # 法兰在基座
+    for _ in range(n):
-        T_t2c = _rand_transform(rng)          # 板在相机（固定相机，板随臂动）
+        T_g2b = _rand_transform(rng)
-        # 板在基座 = T_cam2base · T_t2c（板相对基座是固定的物理事实）
+        T_t2c = Xinv @ T_g2b @ T_target2gripper
        _ = X_true @ T_t2c
        Rg.append(T_g2b[:3, :3])
        tg.append(T_g2b[:3, 3].reshape(3, 1))
        Rc.append(T_t2c[:3, :3])
        tc.append(T_t2c[:3, 3].reshape(3, 1))
    return Rg, tg, Rc, tc
 def test_roundtrip_recovers_known_transform_eye_to_hand():
    """眼在手外：X = cam → base，**与真值比对**（不是只查正交/det）。
    这条分支内部把 gripper2base 求了逆。求逆方向或坐标链写错时，结果照样是一个
    合法的旋转矩阵 —— 只查 det/正交的测试**抓不到**，所以必须比真值。
    """
    rng = np.random.default_rng(3)
    X_true = _rand_transform(rng)             # T_cam2base
    T_target2gripper = _rand_transform(rng)   # 板刚性固定在法兰上（每视图不变）
    Rg, tg, Rc, tc = _synthesize_views_eye_to_hand(X_true, T_target2gripper, 14)
    # eye_to_hand 下 OpenCV 仍是解 AX=XB，X 语义变为 cam→base；
    # 这里只验证"能解出正交且 det=1 的结果"，不比对真值
    # （板随臂动时需要 T_t2b 与 T_g2b 的联动，构造更复杂，
    #  真正的语义正确性由 eye_in_hand 那条测试把关坐标链）
    R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True)
    X_est = Rt_to_T(R, t)
    assert np.allclose(X_est, X_true, atol=1e-6), (
        "eye_to_hand 没解回已知的 T_cam2base —— 求逆方向或坐标链错了"
    )
    R = np.asarray(R, float)
-    assert R.shape == (3, 3)
+    assert np.isclose(np.linalg.det(R), 1.0, atol=1e-8), "det 必须是 +1，不是镜像"
-    assert np.isclose(np.linalg.det(R), 1.0, atol=1e-6)
+
 def test_eye_to_hand_flag_actually_changes_the_answer():
    """负向：同一份眼在手外数据当成眼在手上解，**必须解出不同的东西**。
    给上面那条把关：万一哪天 `eye_to_hand` 分支被改成"其实什么也没做"
    （两个模式走同一条路），上面那条测试就会失去意义 —— 而这条会红。
    """
    rng = np.random.default_rng(3)
    X_true = _rand_transform(rng)
    T_target2gripper = _rand_transform(rng)
    Rg, tg, Rc, tc = _synthesize_views_eye_to_hand(X_true, T_target2gripper, 14)
    R_ok, t_ok = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True)
    R_bad, t_bad = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=False)
    assert np.allclose(Rt_to_T(R_ok, t_ok), X_true, atol=1e-6)
    assert not np.allclose(Rt_to_T(R_bad, t_bad), X_true, atol=1e-3), (
        "眼在手上分支也解出了同一个真值 —— 说明 eye_to_hand 这个开关根本没起作用"
    )
 def test_calibration_fails_loudly_on_degenerate_input():
@@ -0,0 +1,404 @@
 """眼在手外 / 眼在手上必须按模式分开：板尺寸、相机、结果文件名、TF 父子。
 这些不是风格问题。两种装法用两块不同的物理板、两台不同的相机，解出的
 矩阵语义也不同（cam→法兰 vs cam→基座）。共用一份配置的后果不是报错，
 而是"标定跑完、结果系统性偏"——现场几乎联想不到是板尺寸/相机/文件名
 串了。所以这里每条测试都配一条**负向**：故意走错模式必须红。
 """
 from __future__ import annotations
 import os
 import subprocess
 import sys
 from pathlib import Path
 import pytest
 import yaml
 ROOT = Path(__file__).resolve().parents[3]
 SCRIPTS_LIB = ROOT / "scripts" / "lib"
 SCRIPT = ROOT / "scripts" / "run_handeye.sh"
 sys.path.insert(0, str(SCRIPTS_LIB))
 import export_site_env  # type: ignore  # noqa: E402
 from arm_application.site_io import (  # noqa: E402
    eye_to_hand_result_name,
    handeye_static_tf_frames,
    overhead_camera,
    resolve_eye_to_hand_file,
 )
 def _site(**over) -> dict:
    """最小可 flatten 的虚构站点。调用方可覆盖任意段。"""
    base = {
        "robot": {
            "sn": "Rizon4-777001",
            "robots": [
                {
                    "sn": "Rizon4-777001",
                    "arm": "left",
                    "label": "左臂01",
                    "camera": {"name": "cam_a", "serial": "99900001"},
                },
                {
                    "sn": "Rizon4-777002",
                    "arm": "right",
                    "label": "右臂02",
                    "camera": {"name": "cam_b", "serial": "99900002"},
                },
            ],
        },
        "cameras": {
            "d435": {
                "name": "d435",
                "role": "overhead_eye_to_hand",
                "serial": "99900003",
                "width": 1280,
                "height": 720,
            },
            "sim_d435": {
                "name": "sim_d435",
                "role": "overhead_eye_to_hand",
                "arm": "sim",
                "serial": "",
            },
        },
        "handeye": {
            "mode": "eye_in_hand",
            "boards": {
                "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
                "eye_to_hand": {"cols": 11, "rows": 8, "square_size_m": 0.020},
            },
        },
    }
    for k, v in over.items():
        if isinstance(v, dict) and isinstance(base.get(k), dict):
            merged = dict(base[k])
            merged.update(v)
            base[k] = merged
        else:
            base[k] = v
    return base
 def _flat(monkeypatch, site: dict, sn: str = "Rizon4-777001") -> dict:
    monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
    monkeypatch.setenv("ROBOT_SN", sn)
    return export_site_env.flatten(site)
 # ---------------------------------------------------------------------------
 # 板尺寸按模式
 # ---------------------------------------------------------------------------
 class TestHandeyeBoardPerMode:
    def test_two_modes_keep_different_boards(self, monkeypatch):
        flat = _flat(monkeypatch, _site())
        assert flat["SITE_HANDEYE_BOARD_COLS_EYE_IN_HAND"] == "8"
        assert flat["SITE_HANDEYE_BOARD_ROWS_EYE_IN_HAND"] == "11"
        assert flat["SITE_HANDEYE_SQUARE_EYE_IN_HAND"] == "0.015"
        assert flat["SITE_HANDEYE_BOARD_SRC_EYE_IN_HAND"] == "boards"
        assert flat["SITE_HANDEYE_BOARD_COLS_EYE_TO_HAND"] == "11"
        assert flat["SITE_HANDEYE_BOARD_ROWS_EYE_TO_HAND"] == "8"
        assert flat["SITE_HANDEYE_SQUARE_EYE_TO_HAND"] == "0.02"
        assert flat["SITE_HANDEYE_BOARD_SRC_EYE_TO_HAND"] == "boards"
    def test_current_mode_board_follows_handeye_mode(self, monkeypatch):
        site = _site()
        site["handeye"]["mode"] = "eye_to_hand"
        flat = _flat(monkeypatch, site)
        assert flat["SITE_HANDEYE_BOARD_COLS"] == "11"
        assert flat["SITE_HANDEYE_SQUARE_M"] == "0.02"
    def test_legacy_top_level_keys_still_work(self):
        he = {"board_cols": 9, "board_rows": 6, "square_size_m": 0.025}
        board, src = export_site_env.handeye_board(he, "eye_in_hand")
        assert src == "legacy"
        assert board == {"cols": 9, "rows": 6, "square_size_m": 0.025}
    def test_missing_board_is_default_not_silent_guess_disguised_as_config(self):
        """没配过这块板 → 来源必须是 default。run_handeye.sh 见 default 会拦下。
        如果这里返回 boards / legacy，脚本会以为"配过了"，带着内置 8×11/15mm
        去标 A4 板 —— 标定照样跑完，结果系统性偏。
        """
        board, src = export_site_env.handeye_board({}, "eye_to_hand")
        assert src == "default"
        # 默认值本身仍给出来（给 show 打印），但来源必须让调用方能拦。
        assert board["cols"] == 8
    def test_unknown_mode_is_default(self):
        he = {
            "boards": {
                "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
            }
        }
        _, src = export_site_env.handeye_board(he, "eye_on_foot")
        assert src == "default"
    def test_negative_shared_keys_cannot_serve_both_modes(self):
        """负向：只有一组顶层键时，两种模式会读到**同一块板**。
        这正是旧配置的失败模式。新配置（boards 段）必须让这条不成立。
        """
        he_old = {"board_cols": 8, "board_rows": 11, "square_size_m": 0.015}
        a, _ = export_site_env.handeye_board(he_old, "eye_in_hand")
        b, _ = export_site_env.handeye_board(he_old, "eye_to_hand")
        assert a == b, "旧口径下两种模式本就共用一块板（这条钉住旧逻辑会红的前提）"
        he_new = {
            "boards": {
                "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
                "eye_to_hand": {"cols": 11, "rows": 8, "square_size_m": 0.020},
            }
        }
        a2, _ = export_site_env.handeye_board(he_new, "eye_in_hand")
        b2, _ = export_site_env.handeye_board(he_new, "eye_to_hand")
        assert a2 != b2
        assert a2["square_size_m"] == 0.015
        assert b2["square_size_m"] == 0.020
 # ---------------------------------------------------------------------------
 # 手外相机按 role + arm
 # ---------------------------------------------------------------------------
 class TestOverheadCamera:
    def test_real_arm_picks_shared_d435_not_sim(self):
        site = _site()
        cam, others = overhead_camera(site, {"arm": "left"})
        assert cam.get("name") == "d435"
        assert others == []
    def test_sim_arm_picks_sim_d435(self):
        site = _site()
        cam, _ = overhead_camera(site, {"arm": "sim"})
        assert cam.get("name") == "sim_d435"
    def test_no_overhead_returns_empty(self):
        cam, others = overhead_camera({"cameras": {"d405": {"role": "left_wrist"}}})
        assert cam == {}
        assert others == []
    def test_ambiguous_cameras_surface_the_others(self):
        """两台都没标 arm → 取第一台，但其余名字必须返回，让调用方出声。"""
        site = {
            "cameras": {
                "a": {"name": "a", "role": "overhead_eye_to_hand"},
                "b": {"name": "b", "role": "overhead_eye_to_hand"},
            }
        }
        cam, others = overhead_camera(site, {"arm": "left"})
        assert cam.get("name") in ("a", "b")
        assert len(others) == 1
        assert others[0] != cam.get("name")
 # ---------------------------------------------------------------------------
 # 结果文件名
 # ---------------------------------------------------------------------------
 class TestEyeToHandResultName:
    def test_name_carries_arm_and_camera(self):
        assert eye_to_hand_result_name("89", "d435") == "89_d435_eye_to_hand.yaml"
    def test_two_arms_do_not_share_a_file(self, monkeypatch):
        site = _site()
        left = _flat(monkeypatch, site, sn="Rizon4-777001")
        right = _flat(monkeypatch, site, sn="Rizon4-777002")
        assert left["SITE_HANDEYE_FILE_EYE_TO_HAND"] != right["SITE_HANDEYE_FILE_EYE_TO_HAND"]
        assert left["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("01_d435_eye_to_hand.yaml")
        assert right["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("02_d435_eye_to_hand.yaml")
    def test_eye_in_hand_file_is_untouched(self, monkeypatch):
        """换模式不能改腕部标定那份路径 —— S05 精拍定位一直读它。"""
        site = _site()
        site["handeye"]["mode"] = "eye_to_hand"
        flat = _flat(monkeypatch, site)
        assert flat["SITE_HANDEYE_FILE"].endswith("01_eye_in_hand.yaml")
        assert flat["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("01_d435_eye_to_hand.yaml")
    def test_resolve_prefers_explicit_over_legacy(self, tmp_path: Path):
        explicit = tmp_path / "89_d435_eye_to_hand.yaml"
        legacy = tmp_path / "handeye" / "results" / "d435_eye_to_hand.yaml"
        legacy.parent.mkdir(parents=True)
        explicit.write_text("explicit\n")
        legacy.write_text("legacy\n")
        got = resolve_eye_to_hand_file(
            tmp_path, explicit=str(explicit), camera_name="d435"
        )
        assert got == explicit
    def test_resolve_falls_back_to_camera_named_file(self, tmp_path: Path):
        legacy = tmp_path / "handeye" / "results" / "d435_eye_to_hand.yaml"
        legacy.parent.mkdir(parents=True)
        legacy.write_text("legacy\n")
        got = resolve_eye_to_hand_file(tmp_path, explicit="", camera_name="d435")
        assert got == legacy
    def test_resolve_missing_returns_none_not_a_guess(self, tmp_path: Path):
        assert resolve_eye_to_hand_file(tmp_path, camera_name="d435") is None
    def test_resolve_relative_explicit_is_under_root_not_cwd(self, tmp_path: Path):
        """相对路径必须按仓库根解析。跟 CWD 走会在换目录启动时静默找不到。"""
        (tmp_path / "handeye" / "results").mkdir(parents=True)
        target = tmp_path / "handeye" / "results" / "89_d435_eye_to_hand.yaml"
        target.write_text("ok\n")
        got = resolve_eye_to_hand_file(
            tmp_path,
            explicit="handeye/results/89_d435_eye_to_hand.yaml",
            camera_name="d435",
        )
        assert got == target
 # ---------------------------------------------------------------------------
 # TF 父子
 # ---------------------------------------------------------------------------
 class TestHandeyeStaticTfFrames:
    def test_eye_in_hand_is_flange_parent(self):
        parent, child = handeye_static_tf_frames(
            mode="eye_in_hand",
            ee_frame="Rizon4-063989_flange",
            base_frame="Rizon4-063989_base_link",
            cam_frame="d405_left_color_optical_frame",
        )
        assert parent == "Rizon4-063989_flange"
        assert child == "d405_left_color_optical_frame"
    def test_eye_to_hand_is_base_parent(self):
        parent, child = handeye_static_tf_frames(
            mode="eye_to_hand",
            ee_frame="Rizon4-063989_flange",
            base_frame="Rizon4-063989_base_link",
            cam_frame="d435_color_optical_frame",
        )
        assert parent == "Rizon4-063989_base_link"
        assert child == "d435_color_optical_frame"
    def test_negative_wrong_mode_puts_camera_under_flange(self):
        """负向：眼在手外的文件若按眼在手上发 TF，父系会变成法兰。
        这正是旧 launch 的失败模式。新函数必须让这条不成立。
        """
        wrong, _ = handeye_static_tf_frames(
            mode="eye_in_hand",
            ee_frame="flange",
            base_frame="base",
            cam_frame="d435_color_optical_frame",
        )
        right, _ = handeye_static_tf_frames(
            mode="eye_to_hand",
            ee_frame="flange",
            base_frame="base",
            cam_frame="d435_color_optical_frame",
        )
        assert wrong == "flange"
        assert right == "base"
        assert wrong != right
 # ---------------------------------------------------------------------------
 # run_handeye.sh --mode 真的切到对应的板 / 相机 / 文件
 # ---------------------------------------------------------------------------
 def _show(mode: str, site: Path) -> str:
    env = os.environ.copy()
    env["SITE_CONFIG"] = str(site)
    env["ROBOT_SN"] = "Rizon4-063989"
    proc = subprocess.run(
        ["bash", str(SCRIPT), "--arm", "89", "show", "--mode", mode],
        cwd=str(ROOT),
        env=env,
        text=True,
        capture_output=True,
        check=False,
    )
    return proc.stdout + proc.stderr
@pytest.fixture(scope="module")
 def _real_site() -> Path:
    """脚本走 load_site_config.sh，必须是一份能选出 89 臂的真实形状配置。"""
    return ROOT / "config" / "site.yaml"
 class TestRunHandeyeScriptMode:
    def test_eye_to_hand_uses_d435_and_a4_board(self, _real_site: Path):
        out = _show("eye_to_hand", _real_site)
        assert "mode=eye_to_hand" in out
        assert "camera=d435" in out
        assert "11x8 inner, square=0.02 m" in out
        assert "89_d435_eye_to_hand.yaml" in out
        # 负向：不能再去开腕部相机。
        assert "camera=d405" not in out
    def test_eye_in_hand_keeps_wrist_camera_and_a5_board(self, _real_site: Path):
        out = _show("eye_in_hand", _real_site)
        assert "mode=eye_in_hand" in out
        assert "d405" in out
        assert "8x11 inner, square=0.015 m" in out
        assert "89_eye_in_hand.yaml" in out
    def test_missing_board_blocks_calibrate(self, tmp_path: Path):
        """没配 eye_to_hand 板尺寸时，calibrate 必须当场红，不能去赌默认值。"""
        site = {
            "robot": {
                "sn": "Rizon4-063989",
                "robots": [
                    {
                        "sn": "Rizon4-063989",
                        "arm": "left",
                        "camera": {"name": "d405_left", "serial": "1"},
                    }
                ],
            },
            "handeye": {
                "mode": "eye_in_hand",
                "boards": {
                    "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
                },
            },
            "cameras": {
                "d435": {
                    "name": "d435",
                    "role": "overhead_eye_to_hand",
                    "serial": "9",
                }
            },
        }
        p = tmp_path / "site.yaml"
        p.write_text(yaml.safe_dump(site, allow_unicode=True), encoding="utf-8")
        env = os.environ.copy()
        env["SITE_CONFIG"] = str(p)
        env["ROBOT_SN"] = "Rizon4-063989"
        proc = subprocess.run(
            ["bash", str(SCRIPT), "--arm", "89", "calibrate", "--mode", "eye_to_hand"],
            cwd=str(ROOT),
            env=env,
            text=True,
            capture_output=True,
            check=False,
        )
        assert proc.returncode != 0
        assert "没有配置 [eye_to_hand] 用的标定板尺寸" in (proc.stdout + proc.stderr)
    def test_illegal_mode_exits_immediately(self):
        proc = subprocess.run(
            ["bash", str(SCRIPT), "--arm", "89", "show", "--mode", "eye_on_foot"],
            cwd=str(ROOT),
            text=True,
            capture_output=True,
            check=False,
        )
        assert proc.returncode != 0
        assert "eye_in_hand 或 eye_to_hand" in (proc.stdout + proc.stderr)
@@ -15,7 +15,7 @@ from __future__ import annotations
 from arm_application.hole_detector import _default_model_path, weights_from_site
-CANONICAL = "detect_best_1.onnx"
+CANONICAL = "gongjian0929.onnx"
 # ---------------------------------------------------------------------------
@@ -0,0 +1,50 @@
 """``OnnxWorkpieceDetector`` 必须**当场拒绝**非检测模型，而不是解出垃圾框。
 为什么值得一条测试（2026-10-08 实测）
 -------------------------------------
 把一个 ``task=pose`` 的模型（输出 ``[1, 4+nc+3K, 8400]``）喂给这个"只认
 ``[1, N, 6]``"的解码器，会得到 12 个**看起来像检测结果**的东西：
    conf=70.870  cls=class_73  xyxy=[28.7, 0.0, 75.8, 0.0]
    conf=41.092  cls=class_47  xyxy=[11.0, 0.0, 35.4, 0.0]
    ...
    conf=0.324   cls=workpiece xyxy=[ 0.2, 0.0,  0.3, 0.0]
 坐标被当成置信度（>1）、类号不存在、框是退化的（``y1 == y2``）—— 但没有异常、
 没有告警，流水线照跑。这类"静默失效"正是本仓库花最多代价在防的东西，所以在
 ``__init__`` 里硬失败。
 """
 from __future__ import annotations
 from pathlib import Path
 import pytest
 pytest.importorskip("onnxruntime", reason="没装 onnxruntime，属于环境缺依赖")
 from arm_application.onnx_workpiece import OnnxWorkpieceDetector  # noqa: E402
 ROOT = Path(__file__).resolve().parents[3]
 YOLO_DIR = ROOT / "models" / "yolo"
 POSE_ONNX = YOLO_DIR / "gongjian0929.onnx"       # task=pose, 输出 [1,14,8400]
 DETECT_ONNX = YOLO_DIR / "detect_best_0.onnx"    # task=detect, end2end, 输出 [1,N,6]
 def test_pose_model_is_refused_loudly():
    """带关键点的 pose 模型进了检测解码器 ⇒ 必须报错，且报错要指出正确做法。"""
    if not POSE_ONNX.is_file():
        pytest.skip(f"没有 {POSE_ONNX.name}（模型不在本机）")
    with pytest.raises(RuntimeError) as ei:
        OnnxWorkpieceDetector(POSE_ONNX, conf=0.25, imgsz=640)
    msg = str(ei.value)
    assert "不是 6" in msg, f"报错要说出真实输出维度，实际：{msg}"
    # 必须告诉人该往哪走，否则只会被当成"模型坏了"
    assert "pose" in msg.lower()
 def test_end2end_detect_model_still_loads():
    """守卫不能过严：真正的 end2end 检测模型必须照常加载。"""
    if not DETECT_ONNX.is_file():
        pytest.skip(f"没有 {DETECT_ONNX.name}（模型不在本机）")
    OnnxWorkpieceDetector(DETECT_ONNX, conf=0.25, imgsz=640)
@@ -0,0 +1,195 @@
 """D405 丢了切 D435 一次性、以及臂静止进二阶段：纯函数必须能红。
 错乘法兰 / 把框心冒充 k0 / conf≥门槛还去切 / 晃一下刚好满 3 秒 ——
 这些在真机上都不报错，只是位置错或过早对孔。所以每条都配负向。
 """
 from __future__ import annotations
 from types import SimpleNamespace
 import numpy as np
 import pytest
 from arm_application.overhead_oneshot import (
    StillLock,
    best_det_conf,
    depth_raw_to_m,
    k0_uv_from_det,
    map_uv_to_depth,
    oneshot_k0_base,
    select_oneshot_det,
    wrist_needs_handoff,
 )
 def _det(*, conf=0.8, cx=100.0, cy=80.0, kpts=None):
    xyxy = np.array([cx - 20, cy - 15, cx + 20, cy + 15], dtype=float)
    return SimpleNamespace(conf=conf, xyxy=xyxy, cx=cx, cy=cy, kpts=kpts)
 class TestHandoff:
    def test_none_and_low_conf_need_handoff(self):
        assert wrist_needs_handoff(None, 0.5) is True
        assert wrist_needs_handoff(0.49, 0.5) is True
        assert wrist_needs_handoff(float("nan"), 0.5) is True
    def test_conf_at_or_above_threshold_stays_on_wrist(self):
        assert wrist_needs_handoff(0.5, 0.5) is False
        assert wrist_needs_handoff(0.87, 0.5) is False
    def test_threshold_is_the_caller_value_not_hardcoded(self):
        # 负向：如果函数内部写死 0.5，改门槛 0.7 时 0.6 会判错
        assert wrist_needs_handoff(0.6, 0.7) is True
        assert wrist_needs_handoff(0.6, 0.5) is False
 class TestSelect:
    def test_best_conf_none_on_empty(self):
        assert best_det_conf([]) is None
        assert best_det_conf(None) is None
    def test_select_rejects_below_threshold(self):
        dets = [_det(conf=0.3), _det(conf=0.49)]
        assert select_oneshot_det(dets, 0.5) is None
    def test_select_picks_highest_above_threshold(self):
        lo = _det(conf=0.55, cx=1)
        hi = _det(conf=0.9, cx=2)
        got = select_oneshot_det([lo, hi], 0.5)
        assert got is hi
 class TestK0:
    def test_missing_kpts_is_not_bbox_center(self):
        d = _det(cx=200, cy=150, kpts=None)
        assert k0_uv_from_det(d) is None
    def test_low_kpt_conf_rejected(self):
        kpts = np.array([[10.0, 20.0, 0.1]], dtype=float)
        assert k0_uv_from_det(_det(kpts=kpts), kpt_min_conf=0.30) is None
    def test_good_k0(self):
        kpts = np.array([[11.0, 22.0, 0.9]], dtype=float)
        assert k0_uv_from_det(_det(kpts=kpts)) == (11.0, 22.0)
 class TestOneshotK0Base:
    def _depth(self, z=0.70, h=10, w=10):
        return np.full((h, w), z, dtype=float)
    def _T(self, xyz):
        T = np.eye(4)
        T[:3, 3] = xyz
        return T
    def test_eye_to_hand_ignores_placeholder_flange(self):
        kpts = np.array([[5.0, 5.0, 0.99]], dtype=float)
        det = _det(conf=0.8, cx=5, cy=5, kpts=kpts)
        T = self._T([0.4, -0.5, 0.7])
        got, is_k0 = oneshot_k0_base(
            det,
            self._depth(0.80),
            fx=1.0,
            fy=1.0,
            cx=5.0,
            cy=5.0,
            T_cam_to_base=T,
            min_z_m=0.25,
            max_z_m=1.80,
        )
        assert is_k0 is True
        np.testing.assert_allclose(got, [0.4, -0.5, 1.5], atol=1e-9)
    def test_wrong_mode_would_shift_by_flange(self):
        """守卫：同一点用眼在手上公式会差出法兰平移 —— 调用方绝不能走那条。"""
        from arm_application.handeye_lib import cam_point_to_base
        T_he = self._T([0.4, -0.5, 0.7])
        T_ee = self._T([0.6, 0.0, 0.3])
        p = np.array([0.0, 0.0, 0.8])
        right = cam_point_to_base(p, T_he, T_ee, "eye_to_hand")
        wrong = cam_point_to_base(p, T_he, T_ee, "eye_in_hand")
        assert np.linalg.norm(right - wrong) > 0.5
    def test_low_det_conf_returns_none(self):
        det = _det(conf=0.2)
        assert (
            oneshot_k0_base(
                det,
                self._depth(),
                fx=1,
                fy=1,
                cx=5,
                cy=5,
                T_cam_to_base=np.eye(4),
                min_z_m=0.25,
                max_z_m=1.80,
                min_det_conf=0.5,
            )
            is None
        )
    def test_depth_out_of_range_returns_none(self):
        kpts = np.array([[5.0, 5.0, 0.99]], dtype=float)
        det = _det(conf=0.9, kpts=kpts)
        assert (
            oneshot_k0_base(
                det,
                self._depth(0.05),
                fx=1,
                fy=1,
                cx=5,
                cy=5,
                T_cam_to_base=np.eye(4),
                min_z_m=0.25,
                max_z_m=1.80,
            )
            is None
        )
 class TestDepthScale:
    def test_uint16_mm_becomes_meters(self):
        raw = np.full((8, 8), 700, dtype=np.uint16)
        m = depth_raw_to_m(raw, "16UC1")
        assert m is not None
        assert m[0, 0] == pytest.approx(0.70)
    def test_unknown_encoding_returns_none(self):
        assert depth_raw_to_m(np.ones((8, 8)), "rgb8") is None
    def test_map_uv_scales_when_size_differs(self):
        u, v = map_uv_to_depth(100, 50, (200, 100), (100, 50))
        assert (u, v) == (50.0, 25.0)
 class TestStillLock:
    def test_three_seconds_still_locks(self):
        s = StillLock(still_sec=3.0, still_move_m=0.015)
        p = np.array([0.50, -0.12])
        assert s.update(p, 0.0, tracking_ok=True) is False
        assert s.update(p + 0.005, 2.9, tracking_ok=True) is False
        assert s.update(p + 0.005, 3.0, tracking_ok=True) is True
    def test_large_move_resets(self):
        s = StillLock(still_sec=3.0, still_move_m=0.015)
        p = np.array([0.50, -0.12])
        s.update(p, 0.0, tracking_ok=True)
        assert s.update(p + np.array([0.020, 0.0]), 2.0, tracking_ok=True) is False
        assert s.update(p + np.array([0.020, 0.0]), 4.9, tracking_ok=True) is False
        assert s.update(p + np.array([0.020, 0.0]), 5.0, tracking_ok=True) is True
    def test_lost_tracking_resets(self):
        s = StillLock(still_sec=3.0, still_move_m=0.015)
        p = np.array([0.50, -0.12])
        s.update(p, 0.0, tracking_ok=True)
        assert s.update(p, 4.0, tracking_ok=False) is False
        assert s.update(p, 4.1, tracking_ok=True) is False
    def test_overhead_move_does_not_lock(self):
        """tracking_ok=False（顶视迁臂）即使法兰不动也不许进二阶段。"""
        s = StillLock(still_sec=3.0, still_move_m=0.015)
        p = np.array([0.50, -0.12])
        assert s.update(p, 0.0, tracking_ok=False) is False
        assert s.update(p, 10.0, tracking_ok=False) is False
@@ -0,0 +1,234 @@
 """一阶段跟踪切到顶置 D435：相机、手眼文件、TF 父子、点变换都必须对。
 把眼在手外的 T（cam→基座）再乘法兰，定位总能到附近、永远差一截。
 所以这里每条都配负向：走错模式 / 借腕部量程 / 非法角色必须红。
 """
 from __future__ import annotations
 import sys
 from pathlib import Path
 import numpy as np
 import pytest
 ROOT = Path(__file__).resolve().parents[3]
 SCRIPTS_LIB = ROOT / "scripts" / "lib"
 sys.path.insert(0, str(SCRIPTS_LIB))
 import export_site_env  # type: ignore  # noqa: E402
 from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T  # noqa: E402
 from arm_application.site_io import (  # noqa: E402
    handeye_static_tf_frames,
    track_camera_role,
    track_handeye_mode,
 )
 from test_handeye_mode import _site  # noqa: E402
@pytest.fixture(autouse=True)
 def _isolate_arm(monkeypatch):
    """别让本机 env 里的 ROBOT_SN=89 把虚构站点解析到真机相机。"""
    monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
    monkeypatch.setenv("ROBOT_SN", "Rizon4-777001")
 def _track_site(**over) -> dict:
    site = _site()
    site["camera"] = {"name": "d405_left"}
    site["paths"] = {"handeye_file": "/tmp/wrist_eye_in_hand.yaml"}
    site["yolo"] = {"max_depth_m": 0.55, "min_depth_m": 0.04, "feature_min_range_m": 0.14}
    site["servo"] = {
        "xy_track": "hover",
        "lock_still_sec": 3.0,
        "lock_still_move_m": 0.015,
    }
    site["perception"] = {
        "track_camera": "wrist",
        "wrist_handoff_conf": 0.5,
        "overhead": {
            "min_depth_m": 0.25,
            "max_depth_m": 1.80,
            "feature_min_range_m": 0.30,
            "oneshot_conf": 0.5,
            "cooldown_s": 1.5,
            "frame_max_age_s": 0.4,
            "oneshot_hold_s": 8.0,
        },
    }
    for k, v in over.items():
        if isinstance(v, dict) and isinstance(site.get(k), dict):
            merged = dict(site[k])
            merged.update(v)
            site[k] = merged
        else:
            site[k] = v
    return site
 def _overhead_ok() -> dict:
    return _track_site(
        perception={
            "track_camera": "overhead",
            "overhead": {
                "min_depth_m": 0.25,
                "max_depth_m": 1.80,
                "feature_min_range_m": 0.30,
            },
        }
    )
 class TestTrackCameraRole:
    def test_missing_defaults_to_wrist(self):
        assert track_camera_role({}) == "wrist"
        assert track_camera_role({"perception": {}}) == "wrist"
    def test_illegal_value_raises_not_fallback(self):
        with pytest.raises(ValueError, match="wrist 或 overhead"):
            track_camera_role({"perception": {"track_camera": "d405"}})
    def test_overhead_maps_to_eye_to_hand(self):
        assert track_handeye_mode("overhead") == "eye_to_hand"
        assert track_handeye_mode("wrist") == "eye_in_hand"
 class TestPerceptionServoWiring:
    def test_overhead_uses_d435_not_wrist_camera(self):
        p = export_site_env.perception_overrides(_overhead_ok())
        assert p["camera_source"] == "d435"
        assert p["camera_frame"] == "d435_color_optical_frame"
        assert p["depth_topic"] == "/d435/aligned_depth_to_color/image_raw"
        assert "d405" not in p["camera_source"]
        assert "d405" not in p["depth_topic"]
    def test_overhead_does_not_borrow_wrist_depth_range(self):
        p = export_site_env.perception_overrides(_overhead_ok())
        assert p["max_depth_m"] == pytest.approx(1.80)
        assert p["min_depth_m"] == pytest.approx(0.25)
        assert p["feature_min_range_m"] == pytest.approx(0.30)
        # 负向：腕部 0.55 绝不能漏过来
        assert p["max_depth_m"] != pytest.approx(0.55)
    def test_overhead_handeye_file_is_e2h_not_wrist(self):
        s = export_site_env.servo_overrides(_overhead_ok())
        assert s["handeye_mode"] == "eye_to_hand"
        assert s["handeye_file"].endswith("01_d435_eye_to_hand.yaml")
        assert "wrist_eye_in_hand" not in s["handeye_file"]
        parent, child = handeye_static_tf_frames(
            mode=s["handeye_mode"],
            ee_frame="flange",
            base_frame="base",
            cam_frame=s["camera_frame"],
        )
        assert parent == "base"
        assert child == "d435_color_optical_frame"
    def test_wrist_track_keeps_arm_camera_and_wrist_file(self):
        site = _track_site(perception={"track_camera": "wrist"})
        p = export_site_env.perception_overrides(site)
        s = export_site_env.servo_overrides(site)
        assert p["camera_source"] == "cam_a"
        assert s["handeye_mode"] == "eye_in_hand"
        assert s["handeye_file"] == "/tmp/wrist_eye_in_hand.yaml"
        assert s["handeye_file"].endswith("eye_in_hand.yaml")
    def test_overhead_without_range_raises(self):
        site = _track_site(perception={"track_camera": "overhead", "overhead": {}})
        with pytest.raises(RuntimeError, match="perception.overhead"):
            export_site_env.perception_overrides(site)
    def test_wrist_hybrid_wires_oneshot_without_overwriting_wrist_range(self):
        site = _track_site(perception={"track_camera": "wrist"})
        p = export_site_env.perception_overrides(site)
        s = export_site_env.servo_overrides(site)
        assert p["camera_source"] == "cam_a"
        assert p["max_depth_m"] == pytest.approx(0.55)
        assert p["overhead_oneshot_enable"] is True
        assert p["wrist_handoff_conf"] == pytest.approx(0.5)
        assert p["overhead_max_depth_m"] == pytest.approx(1.80)
        assert p["overhead_handeye_file"].endswith("01_d435_eye_to_hand.yaml")
        assert "d405" not in p["overhead_camera_source"]
        assert s["handeye_mode"] == "eye_in_hand"
        assert s["lock_still_sec"] == pytest.approx(3.0)
        assert s["wrist_handoff_conf"] == pytest.approx(0.5)
        assert s["overhead_hold_s"] == pytest.approx(8.0)
    def test_wrist_hybrid_missing_handoff_conf_raises(self):
        site = _track_site()
        del site["perception"]["wrist_handoff_conf"]
        with pytest.raises(RuntimeError, match="wrist_handoff_conf"):
            export_site_env.perception_overrides(site)
    def test_wrist_hybrid_missing_still_keys_raises(self):
        site = _track_site()
        del site["servo"]["lock_still_sec"]
        with pytest.raises(RuntimeError, match="lock_still_sec"):
            export_site_env.servo_overrides(site)
    def test_overhead_lookat_raises(self):
        site = _overhead_ok()
        site["servo"] = {"xy_track": "lookat"}
        with pytest.raises(RuntimeError, match="lookat"):
            export_site_env.servo_overrides(site)
    def test_flatten_track_vars_follow_role(self):
        flat = export_site_env.flatten(_overhead_ok())
        assert flat["SITE_TRACK_CAMERA"] == "overhead"
        assert flat["SITE_TRACK_CAM_NAME"] == "d435"
        assert flat["SITE_CAM_NAME"] == "cam_a"  # 腕部精拍路不变
        assert flat["SITE_TRACK_HANDEYE_MODE"] == "eye_to_hand"
        assert flat["SITE_TRACK_HANDEYE_FILE"].endswith("01_d435_eye_to_hand.yaml")
        assert "wrist_eye_in_hand" not in flat["SITE_TRACK_HANDEYE_FILE"]
 class TestCamPointToBase:
    def test_eye_to_hand_ignores_flange(self):
        T_he = np.eye(4)
        T_he[:3, 3] = [0.4, -0.5, 0.7]
        T_ee = np.eye(4)
        T_ee[:3, 3] = [0.6, 0.0, 0.3]
        p = np.array([0.0, 0.0, 0.8])
        got = cam_point_to_base(p, T_he, T_ee, "eye_to_hand")
        np.testing.assert_allclose(got, [0.4, -0.5, 1.5], atol=1e-9)
        # 负向：若误乘法兰，点会多出法兰平移
        wrong = cam_point_to_base(p, T_he, T_ee, "eye_in_hand")
        assert np.linalg.norm(got - wrong) > 0.5
    def test_eye_in_hand_still_left_multiplies_ee(self):
        T_he = np.eye(4)
        T_he[:3, 3] = [0.0, 0.0, 0.12]
        T_ee = np.eye(4)
        T_ee[:3, 3] = [0.5, -0.1, 0.3]
        p = np.array([0.01, 0.0, 0.2])
        got = cam_point_to_base(p, T_he, T_ee, "eye_in_hand")
        np.testing.assert_allclose(got, [0.51, -0.1, 0.62], atol=1e-9)
    def test_illegal_mode_raises(self):
        with pytest.raises(ValueError, match="eye_in_hand 或 eye_to_hand"):
            cam_to_base_T(np.eye(4), np.eye(4), "eye_on_foot")
 class TestRealSiteBranchDefault:
    """main 上 site.yaml 必须是 D405 主跟踪 + D435 一次性，跟踪外参仍是腕部。"""
    def test_site_yaml_hybrid_wrist_plus_d435_oneshot(self, monkeypatch):
        monkeypatch.setenv("SITE_CONFIG", str(ROOT / "config" / "site.yaml"))
        monkeypatch.setenv("ROBOT_SN", "Rizon4-063989")
        data = export_site_env.load_site()
        p = export_site_env.perception_overrides(data)
        s = export_site_env.servo_overrides(data)
        assert p["camera_source"] in ("d405_left", "d405")
        assert p["max_depth_m"] == pytest.approx(0.55)
        assert p["wrist_handoff_conf"] == pytest.approx(0.5)
        assert p["overhead_oneshot_enable"] is True
        assert p["overhead_handeye_file"].endswith("89_d435_eye_to_hand.yaml")
        assert p["overhead_max_depth_m"] == pytest.approx(1.80)
        assert s["handeye_mode"] == "eye_in_hand"
        assert s["handeye_file"].endswith("89_eye_in_hand.yaml")
        assert s["lock_still_sec"] == pytest.approx(3.0)
        assert s["lock_still_move_m"] == pytest.approx(0.015)
        # 负向：跟踪外参不能被换成顶视文件
        assert "eye_to_hand" not in s["handeye_file"]
        assert "eye_in_hand" not in p["overhead_handeye_file"]
@@ -0,0 +1,180 @@
 """一阶段"重新捕获"的两道守卫：置信度门槛 + 特征点相机量程下限。
 为什么单拎出来测（2026-10-08 真机，arm 89，一阶段把臂带离工件 500mm）
 --------------------------------------------------------------------
 ``/tmp/pipeline_stage1.log`` 原话：
    0659.551  死区保持 |e_xy|=3.0mm                 ← 已经收敛到 3mm
    0659.598  k0 置信度 0.00 < 0.30，本帧不用它
    0659.6xx  重新捕获 conf=0.15 dpx=503            ← 503px 外、conf 0.15 的假框
              feat_base=(0.231,-0.484) z=-0.021     ← 跟踪点瞬间横移 427mm
    0670.985  === 一阶段已锁定 ===                  ← "锁定"在离工件 500mm 处
 视觉模型（真 k0 conf 0.87）与控制（伺服收敛到 3mm）都正常，坏的是逻辑：
 ``pick_best`` 的"重新捕获"分支把门槛接成了 ``min_conf=0.12``，于是 0.15 的远框
 被原样采信、perception 自己 ``kf.reset()`` 跳过去；伺服侧的 FeatureJumpGate 只能
 拦一帧（假框帧间稳定，3 帧后照样认）。拦不住还有一个更根本的原因：那个假目标
 就在**相机前 0.08m**（爪上工具/相机座，随臂一起动），跟踪其误差恒为 0 —— 伺服
 "收敛"得完美，锁的却是自己。所以还要一道**量纲是米**的判据。
 前两条是**负向**用例：把门槛调回旧值，第一条必须红。
 """
 from __future__ import annotations
 from pathlib import Path
 import numpy as np
 import pytest
 import yaml
 from arm_application.yolo_detector import Detection, pick_best
 # 测试在 src/<包>/test/ 下 → 仓库根是 parents[3]
 ROOT = Path(__file__).resolve().parents[3]
 SITE = ROOT / "config" / "site.yaml"
 HANDEYE = ROOT / "handeye" / "results" / "89_eye_in_hand.yaml"
 # 真机实测数字（见本文件头与 site.yaml 的对应注释）——
 # 写死在这里是**故意**的：它们是被测事实，不是配置。
 REAL_K0_CONF = 0.87          # 真 k0 的置信度
 FALSE_BOX_CONF = 0.15        # 被误采信的那个假框
 FALSE_BOX_CONF_HI = 0.47     # 那串假框的实测上沿
 # 相机光轴视距的两条带 [m]（p_cam.z）
 ROBOT_NEAR_BAND = (0.078, 0.087)   # 随臂一起动：爪上工具 / 相机座
 WORKPIECE_RANGE_BAND = (0.19, 0.29)  # 工件面在锁高下
 # 假框：503px 外、尺寸和真工件框同量级（否则会被 min_area 先挡掉，测不到本意）
 PREV = np.array([300.0, 300.0, 460.0, 440.0])       # 上一帧框，中心 (380, 370)
 FALSE_FAR = np.array([800.0, 20.0, 960.0, 160.0])   # 中心 (880, 90) → 503px 外
 def _det(xyxy: np.ndarray, conf: float) -> Detection:
    return Detection(xyxy=np.asarray(xyxy, dtype=float), conf=float(conf),
                     cls_id=0, cls_name="workpiece")
 def _pick(conf: float, reacquire_conf: float):
    return pick_best(
        [_det(FALSE_FAR, conf)],
        prev_uv=np.array([380.0, 370.0]),
        prev_xyxy=PREV,
        max_track_px=400.0,
        reacquire_conf=reacquire_conf,
        min_conf=0.12,
        lock_iou=0.05,
    )
 def test_old_wiring_would_adopt_the_false_box():
    """负向基线：门槛=min_conf（旧接线）时，conf 0.15 的 503px 远框**会被采信**。
    这条是"这个检查能红"的证明 —— 它若开始返回 None，说明前提变了，
    下面"新接线挡得住"就不再是在测同一件事。
    """
    best = _pick(FALSE_BOX_CONF, reacquire_conf=0.12)
    assert best is not None, "旧接线本该采信这个假框 —— 前提变了，用例已失真"
    assert best.conf == pytest.approx(FALSE_BOX_CONF)
 def test_new_wiring_rejects_the_false_box():
    """新接线：门槛 0.55 > 假框上沿 0.47 ⇒ 低置信度远框一律不采信。"""
    assert _pick(FALSE_BOX_CONF, reacquire_conf=0.55) is None
 def test_new_wiring_rejects_whole_false_band():
    """实测那串假框 conf 0.13~0.47，全带都要挡住（不能只挡 0.15 那一个）。"""
    for conf in (0.13, 0.25, 0.38, FALSE_BOX_CONF_HI):
        assert _pick(conf, reacquire_conf=0.55) is None, f"conf={conf} 漏过去了"
 def test_high_conf_relocation_is_still_adopted():
    """守卫不能过严：真 k0（0.87）级的重定位必须仍然立刻生效。
    否则"工件确实被挪了"时臂会僵在原地不动 —— 那是另一种失效。
    """
    best = _pick(REAL_K0_CONF, reacquire_conf=0.55)
    assert best is not None and best.conf == pytest.approx(REAL_K0_CONF)
 # --------------------------------------------------------------------------- #
 # 接线：site.yaml 真的把这两个门槛接到了位，且取值区间有实测/几何依据
 # --------------------------------------------------------------------------- #
 def _site() -> dict:
    assert SITE.is_file(), f"找不到站点配置 {SITE}"
    return yaml.safe_load(SITE.read_text(encoding="utf-8"))
 def _left_arm89(site: dict) -> dict:
    for entry in site["robot"]["robots"]:
        if str(entry.get("label", "")) == "左臂89":
            return entry
    pytest.fail("site.yaml 里找不到 左臂89 的条目")
 def test_site_yaml_wires_reacquire_conf_between_false_band_and_real_k0():
    yolo = _site()["yolo"]
    # 键必须存在 —— 缺了会静默退回 min_conf（代码默认值兜着，配置看着"改了"却没用）
    assert "reacquire_conf" in yolo, "site.yaml 没接 reacquire_conf（会退回 min_conf）"
    v = float(yolo["reacquire_conf"])
    assert FALSE_BOX_CONF_HI < v < REAL_K0_CONF, (
        f"reacquire_conf={v} 必须高于假框带 {FALSE_BOX_CONF_HI}、低于真 k0 {REAL_K0_CONF}"
    )
 def test_feature_min_range_sits_between_robot_and_workpiece():
    """量程下限必须夹在"机械臂自身"与"工件面"两条实测带之间。"""
    site = _site()
    assert "feature_min_range_m" in site["yolo"], "site.yaml 没接 feature_min_range_m"
    gate = float(site["yolo"]["feature_min_range_m"])
    assert ROBOT_NEAR_BAND[1] < gate < WORKPIECE_RANGE_BAND[0], (
        f"量程下限 {gate} 必须高于随臂近带 {ROBOT_NEAR_BAND[1]}、"
        f"低于工件面 {WORKPIECE_RANGE_BAND[0]}"
    )
 def test_gauge_rod_extends_below_camera_so_near_band_is_the_robot():
    """近带为什么会被占满：规尖比相机更靠下 ⇒ 规杆整段都在相机近场。"""
    arm = _left_arm89(_site())
    tip = float(arm["gauge_assembly"]["tool_tip_below_flange_m"])
    doc = yaml.safe_load(HANDEYE.read_text(encoding="utf-8"))
    cam_z = abs(float(doc["translation_m"][2]))   # 相机在法兰 Z 下方的距离
    assert tip > cam_z + 0.05, (
        f"规尖 {tip:.3f} 应比相机 {cam_z:.3f} 明显更靠下 —— 否则近带论证不成立"
    )
 def test_feature_min_range_is_not_looser_than_depth_floor():
    """不许把量程下限调到解码下限以下 —— 那等于这道守卫没接。"""
    yolo = _site()["yolo"]
    assert float(yolo["feature_min_range_m"]) > float(yolo["min_depth_m"])
    assert float(yolo["feature_min_range_m"]) < float(yolo["max_depth_m"])
 # --------------------------------------------------------------------------- #
 # 接线（代码侧）：site.yaml 写了键还不够，必须真的传进 pick_best / 量程判断
 # --------------------------------------------------------------------------- #
 def _perception_src() -> str:
    p = ROOT / "src" / "arm_application" / "arm_application" / "perception_node.py"
    assert p.is_file(), f"找不到 {p}"
    return p.read_text(encoding="utf-8")
 def test_perception_wires_reacquire_conf_into_pick_best():
    src = _perception_src()
    assert "reacquire_conf=self.reacquire_conf" in src, "参数没传进 pick_best，配置等于没接"
    # 旧接线不许回来：这就是 2026-10-08 出事的写法
    assert "reacquire_conf=self.min_conf" not in src, "重新捕获门槛又退回 min_conf 了"
    assert 'declare_parameter("reacquire_conf"' in src
 def test_perception_uses_feature_min_range_not_min_depth_for_features():
    src = _perception_src()
    assert 'declare_parameter("feature_min_range_m"' in src
    assert "self.feature_min_range = max(" in src, "量程下限没有取严（可能被调得比 min_depth 还松）"
    # 特征点量程判断必须用 feature_min_range，不能用回 min_depth
    assert "self.feature_min_range < z < self.max_depth" in src
    assert "(meters > self.feature_min_range)" in src
    assert "(meters2 > self.feature_min_range)" in src
    assert "min_depth=self.min_depth," not in src, "还有特征测量在用 min_depth"
@@ -0,0 +1,280 @@
 """pose 模型支持（k0 跟踪 / k1,k2 对孔）的测试。
 刻意**不依赖仓库里的模型二进制**（`models/**/*.onnx` 都不进 git）：
 ONNX 用 `onnx.helper` 现场造，解码用合成张量。所以纯逻辑都能离线跑，
 GPU / TensorRT 都不需要 —— engine 那条路只能真机验（`weights.sh doctor --check-engines`）。
 要挡住的那类 bug（2026-09-29 实测踩到的）：
 ultralytics 按 `task="detect"` 去解 pose 的 14 通道输出，会吐 300 个垃圾框，
 `conf` 直接是坐标值（600+）—— 现场表现是"跟踪锁到空气上"，在真机上很危险。
 """
 from __future__ import annotations
 import numpy as np
 import pytest
 from arm_application.hole_detect_hough import _keypoint_holes
 from arm_application.model_labels import SOURCE_SIDECAR, LabelSet
 from arm_application.pose_detector import (
    PoseDetector,
    is_pose_model,
    keypoint_roles,
    letterbox,
    nms_xyxy,
    read_pose_info,
 )
 from arm_application.yolo_detector import Detection
 # --------------------------------------------------------------------------
 # fixture 工具
 # --------------------------------------------------------------------------
 def _make_onnx(path, metadata):
    """造一个只有 metadata 的最小 ONNX（图内容无所谓，只测 metadata 解析）。"""
    pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）")
    import onnx
    from onnx import TensorProto, helper
    inp = helper.make_tensor_value_info("images", TensorProto.FLOAT, [1, 3, 8, 8])
    out = helper.make_tensor_value_info("output0", TensorProto.FLOAT, [1, 14, 4])
    graph = helper.make_graph(
        [helper.make_node("Identity", ["images"], ["output0"])], "g", [inp], [out]
    )
    model = helper.make_model(graph)
    model.opset_import[0].version = 13
    for k, v in metadata.items():
        model.metadata_props.add(key=k, value=v)
    onnx.save(model, str(path))
    return path
 def _bare_detector(*, kpt_count=3, conf=0.25, iou=0.45, max_det=20, class_names=None):
    """只装解码需要的属性 —— 绕开 __init__ 里的 engine 加载（要 GPU）。"""
    det = object.__new__(PoseDetector)
    det.kpt_count = kpt_count
    det.conf = conf
    det.iou = iou
    det.max_det = max_det
    det.class_ids = None
    det.class_names = class_names
    det._labels = LabelSet(names={0: "workpiece"}, source=SOURCE_SIDECAR, path=None)
    return det
 def _raw(boxes, kpts, confs, n_anchor=4, nc=1):
    """拼一个 (1, 4+nc+3K, A) 的合成输出。boxes=xywh，kpts=[[(x,y,c),...], ...]"""
    k = kpts.shape[1]
    ch = 4 + nc + 3 * k
    o = np.zeros((ch, n_anchor), dtype=np.float32)
    n = boxes.shape[0]
    o[0:4, :n] = boxes.T
    o[4:4 + nc, :n] = confs[None, :] if nc > 1 else confs[None, :]
    o[4 + nc:, :n] = kpts.reshape(n, -1).T
    return o[None]
 # --------------------------------------------------------------------------
 # 模型识别：从旁挂 .onnx 的 metadata 判断是不是 pose
 # --------------------------------------------------------------------------
 def test_pose_model_is_recognized(tmp_path):
    _make_onnx(
        tmp_path / "m.onnx",
        {
            "task": "pose",
            "kpt_shape": "[3, 3]",
            "kpt_names": "{0: ['0', '1', '2']}",
            "names": "{0: 'workpiece'}",
        },
    )
    info = read_pose_info(tmp_path / "m.onnx")
    assert info is not None
    assert info["kpt_count"] == 3
    assert info["names"] == {0: "workpiece"}
    assert is_pose_model(tmp_path / "m.onnx")
 def test_detect_model_is_not_pose(tmp_path):
    _make_onnx(tmp_path / "d.onnx", {"task": "detect", "names": "{0: 'workpiece'}"})
    assert read_pose_info(tmp_path / "d.onnx") is None
    assert not is_pose_model(tmp_path / "d.onnx")
 def test_pose_without_kpt_shape_is_not_guessed(tmp_path):
    """没有 kpt_shape 就别猜关键点个数 —— 猜错是"静默错位"，比报错难查得多。"""
    _make_onnx(tmp_path / "p.onnx", {"task": "pose", "names": "{0: 'workpiece'}"})
    assert read_pose_info(tmp_path / "p.onnx") is None
 def test_engine_reads_pose_info_from_sibling_onnx(tmp_path):
    """engine 没有 metadata（TRT 10 Python API 就没有接口），要靠同名的 .onnx。"""
    _make_onnx(
        tmp_path / "m.onnx", {"task": "pose", "kpt_shape": "[3, 3]", "names": "{0: 'workpiece'}"}
    )
    (tmp_path / "m.engine").write_bytes(b"not a real engine")
    assert is_pose_model(tmp_path / "m.engine")
 def test_pose_info_missing_onnx_returns_none(tmp_path):
    (tmp_path / "m.engine").write_bytes(b"not a real engine")
    assert read_pose_info(tmp_path / "m.engine") is None
 # --------------------------------------------------------------------------
 # letterbox：坐标还原全靠它，错了整幅图的框都会偏
 # --------------------------------------------------------------------------
 def test_letterbox_preserves_aspect_and_reports_padding():
    img = np.zeros((480, 848, 3), dtype=np.uint8)
    out, gain, (dw, dh) = letterbox(img, 640)
    assert out.shape == (640, 640, 3)
    # 848x480 → gain = 640/848；等比缩到 640x362，纵向补 139
    assert gain == pytest.approx(640 / 848, rel=1e-6)
    assert dw == pytest.approx(0.0, abs=0.5)
    assert dh == pytest.approx(139.0, abs=1.0)
 def test_letterbox_square_input_is_untouched():
    img = np.zeros((640, 640, 3), dtype=np.uint8)
    out, gain, (dw, dh) = letterbox(img, 640)
    assert out.shape == (640, 640, 3)
    assert gain == pytest.approx(1.0)
    assert (dw, dh) == (0.0, 0.0)
 # --------------------------------------------------------------------------
 # NMS
 # --------------------------------------------------------------------------
 def test_nms_suppresses_overlap_and_keeps_highest():
    boxes = np.array([[0, 0, 100, 100], [5, 5, 105, 105], [500, 500, 600, 600]], dtype=float)
    scores = np.array([0.9, 0.8, 0.7])
    keep = nms_xyxy(boxes, scores, 0.5, max_det=10)
    assert list(keep) == [0, 2]
 def test_nms_respects_max_det():
    boxes = np.array([[0, 0, 10, 10], [100, 100, 110, 110], [200, 200, 210, 210]], dtype=float)
    assert len(nms_xyxy(boxes, np.array([0.9, 0.8, 0.7]), 0.5, max_det=2)) == 2
 # --------------------------------------------------------------------------
 # 解码：框 + 关键点，并去 letterbox 还原到原图
 # --------------------------------------------------------------------------
 def test_decode_returns_box_and_keypoints_unletterboxed():
    det = _bare_detector()
    boxes = np.array([[320.0, 320.0, 100.0, 80.0]])
    kpts = np.array([[[300.0, 300.0, 0.9], [360.0, 300.0, 0.8], [280.0, 310.0, 0.7]]])
    raw = _raw(boxes, kpts, np.array([0.90]))
    out = det._decode(raw, gain=0.5, dw=10.0, dh=20.0)
    assert len(out) == 1
    d = out[0]
    assert d.cls_name == "workpiece"
    assert d.conf == pytest.approx(0.90, abs=1e-5)
    # ((cx±w/2) - dw) / gain
    assert list(np.round(d.xyxy, 3)) == [520.0, 520.0, 720.0, 680.0]
    assert d.kpts.shape == (3, 3)
    assert list(np.round(d.kpts[0][:2], 3)) == [580.0, 560.0]
    assert d.kpts[0][2] == pytest.approx(0.9, abs=1e-5)
    assert list(np.round(d.kpts[1][:2], 3)) == [700.0, 560.0]
    assert list(np.round(d.kpts[2][:2], 3)) == [540.0, 580.0]
 def test_decode_drops_low_conf_anchors():
    det = _bare_detector(conf=0.5)
    boxes = np.array([[100.0, 100.0, 20.0, 20.0], [400.0, 400.0, 20.0, 20.0]])
    kpts = np.zeros((2, 3, 3), dtype=float)
    raw = _raw(boxes, kpts, np.array([0.9, 0.1]))
    out = det._decode(raw, gain=1.0, dw=0.0, dh=0.0)
    assert len(out) == 1
    assert out[0].conf == pytest.approx(0.9, abs=1e-5)
 def test_decode_raises_when_channels_do_not_match_kpt_count():
    """通道数和 kpt_shape 对不上时必须报错，不能默默解出一堆垃圾。"""
    det = _bare_detector(kpt_count=3)
    raw = np.zeros((1, 6, 4), dtype=np.float32)  # 6 通道装不下 3 个关键点
    with pytest.raises(RuntimeError, match="通道数不对"):
        det._decode(raw, gain=1.0, dw=0.0, dh=0.0)
 def test_decode_of_detect_style_output_would_be_garbage():
    """回归：把 pose 的 14 通道输出当检测解 → conf 会变成 600+ 的坐标值。
    这条测的是"我们**没有**走那条路"：同一份合成输出，按 pose 解出来的 conf
    必须在 (0,1]，绝不能是坐标量级。
    """
    det = _bare_detector()
    boxes = np.array([[600.0, 640.0, 100.0, 80.0]])
    kpts = np.array([[[610.0, 300.0, 0.9], [620.0, 300.0, 0.8], [630.0, 310.0, 0.7]]])
    raw = _raw(boxes, kpts, np.array([0.9]))
    out = det._decode(raw, gain=1.0, dw=0.0, dh=0.0)
    assert len(out) == 1
    assert 0.0 < out[0].conf <= 1.0
 # --------------------------------------------------------------------------
 # k1/k2：模型语义顺序，**不按图像 x 交换**
 # --------------------------------------------------------------------------
 def _det_with_kpts(kpts, conf=0.9):
    return Detection(
        xyxy=np.array([300.0, 300.0, 500.0, 400.0]),
        conf=conf,
        cls_id=0,
        cls_name="workpiece",
        kpts=np.asarray(kpts, dtype=float),
    )
 def _kw(**over):
    cfg = dict(conf_thr=0.25, min_kconf=0.30, radius_px=12.0, min_dist_px=15.0)
    cfg.update(over)
    return cfg
 def test_keypoint_holes_keeps_model_semantic_order():
    """k1 在画面右边、k2 在左边时，**不能**交换。
    Hough 路径的 k1/k2 是"图像左/右"（位置定义）；pose 的 k1/k2 是训练时标注的
    固定物理孔。按 x 交换会让 target_hole=k1 在工件转个角度后指向**另一个**孔 ——
    现场表现为"有时对左孔、有时对右孔"，非常难查。
    """
    det = _det_with_kpts([[400, 350, 1.0], [470, 340, 1.0], [330, 345, 1.0]])
    out = _keypoint_holes([det], **_kw())
    assert out is not None
    (k1x, k1y), (k2x, k2y), r1, r2 = out
    assert (k1x, k1y) == (470.0, 340.0), "k1 必须还是模型的 k1（画面右）"
    assert (k2x, k2y) == (330.0, 345.0), "k2 必须还是模型的 k2（画面左）"
    assert (r1, r2) == (12.0, 12.0)
 def test_keypoint_holes_rejects_low_confidence():
    det = _det_with_kpts([[400, 350, 1.0], [470, 340, 0.10], [330, 345, 0.05]])
    assert _keypoint_holes([det], **_kw()) is None
 def test_keypoint_holes_rejects_coincident_points():
    det = _det_with_kpts([[400, 350, 1.0], [401, 350, 1.0], [402, 350, 1.0]])
    assert _keypoint_holes([det], **_kw(min_dist_px=15.0)) is None
 def test_keypoint_holes_returns_none_for_plain_detection():
    """普通检测模型没有 kpts → 必须返回 None，让 Hough 老路径接手。"""
    det = Detection(xyxy=np.array([0.0, 0.0, 10.0, 10.0]), conf=0.9, cls_id=0, cls_name="workpiece")
    assert _keypoint_holes([det], **_kw()) is None
 def test_keypoint_holes_skips_low_conf_detection():
    det = _det_with_kpts([[400, 350, 1.0], [470, 340, 1.0], [330, 345, 1.0]], conf=0.05)
    assert _keypoint_holes([det], **_kw(conf_thr=0.25)) is None
 # --------------------------------------------------------------------------
 # 角色约定
 # --------------------------------------------------------------------------
 def test_keypoint_roles_defaults_to_k0_track_k1k2_holes():
    assert keypoint_roles(3) == (0, (1, 2))
 def test_keypoint_roles_handles_models_with_fewer_points():
    assert keypoint_roles(1) == (0, ())
    assert keypoint_roles(2) == (0, (1,))
@@ -0,0 +1,263 @@
 """粗居中的两条误检守卫：**框被画面切** + **目标点离谱/盒外**。
 为什么单独立一条（2026-10-08 真机 arm 89，二阶段 exit=4）
 --------------------------------------------------------
 ``/tmp/run_all3.log`` 原话（一阶段锁定 ✓、二阶段悬停到位 ✓，死在这一步）::
    悬停到位: 法兰 Z=0.320m（目标 0.320，差 +0mm） 残余倾角 0.37deg
    snap_coarse_0: 工件 bbox=(0, 142, 97, 222) 中心=(48,182)
                   base=(0.280,-0.747)[深度] 夹爪=(0.336,-0.543) 偏差=211.5mm
      目标超出工作盒，已截断: (0.280,-0.747,0.320) → (0.280,-0.545,0.320)
      plan coarse Δ=(-56,-2,-0)mm            ← 把臂带离工件 56mm
    snap_coarse_1: 工件 bbox=[0, 0, 847, 479]（整幅画面）偏差=55.9mm
    snap_fine_0..8: 两孔基座间距 105.8 / 210.0 / 213.2 / …mm
                    （期望 32–85mm）丢弃本拍
    二阶段失败 exit=4，停在当前位姿
 工件其实在 (0.5,-0.12) 一带。两个缺陷叠在一起：
  1. ``bbox=(0,142,97,222)`` 的 ``x0=0`` —— 被画面**左边缘切掉**的半框。框中心
     根本不在工件上，深度反投影因此给出盒外的 (0.280,-0.747)；
  2. 这个盒外目标**没有被判误检**，而是"截断到盒边"后照样移动 —— 越界只被夹到
     盒边，臂还是走了。这就是规矩里禁的"宽容降级"：把"没检查"伪装成"检查通过"。
     一挪错，精定位就再也看不到 k1/k2，而流程把它报成"孔检测失败"（exit=4）。
 修法：``_bbox_on_border`` 弃用贴边框、``_coarse_target_reject`` 在**移动前**
 否掉盒外/离谱目标（判误检 ⇒ 原地不动，交给精定位）；以及 YOLO 无可信框但精定位
 已看到 k1/k2 时**真的不移动**（旧代码嘴上说"跳过居中"却仍然算偏差横移）。
 这些用例全是**离线**的：两个被判的函数都是纯逻辑，用打桩对象即可，不建 ROS 实体
 （与 ``test_stage2_descent_clamp`` 同一套做法）。
 """
 from __future__ import annotations
 from pathlib import Path
 from typing import Any, List, Optional
 import numpy as np
 import pytest
 import yaml
 from arm_application.stage2_locate_node import Stage2Locate
 # 测试在 src/<包>/test/ 下 → 仓库根是 parents[3]
 ROOT = Path(__file__).resolve().parents[3]
 SITE = ROOT / "config" / "site.yaml"
 # ---- 真机实测数字（见本文件头，故意写死：它们是被测事实，不是配置）----
 FRAME_W, FRAME_H = 848, 480            # D405 彩色图尺寸（bbox [0,0,847,479] 可反推）
 BAD_BBOX_CLIPPED = (0, 142, 97, 222)   # 左边缘切掉的半框 —— 事故那一帧
 DEGENERATE_BBOX = (0, 0, 847, 479)     # 孔检测器兜底给的整幅画面
 GOOD_BBOX = (300, 150, 520, 330)       # 一个正常的、不贴边的框
 BAD_TARGET_BASE = np.array([0.280, -0.747, 0.320])   # 那帧反投影出来的点（盒外）
 GAUGE_XY = np.array([0.336, -0.543])                 # 那帧的夹爪轴线 XY
 BAD_ERR_M = 0.2115       # 日志里的"偏差=211.5mm"
 CLAMPED_Y = -0.545       # 日志里的"截断"结果
 # 工作盒（site.yaml: robots[].servo.workspace_xyz_*）
 WS_MIN = np.array([0.08, -0.55, 0.18])
 WS_MAX = np.array([0.85, 0.55, 0.65])
 class _StubLogger:
    def __init__(self, sink: List[str]) -> None:
        self._sink = sink
    def _add(self, msg: str) -> None:
        self._sink.append(str(msg))
    info = _add
    warn = _add
    error = _add
 class _Stub:
    """两个被判函数只用到这几个成员，其余全部打桩。"""
    def __init__(
        self,
        *,
        ws_min: Any = WS_MIN,
        ws_max: Any = WS_MAX,
        max_offset_m: float = 0.12,
        border_px: int = 2,
    ) -> None:
        self.logs: List[str] = []
        self.ws_min = ws_min
        self.ws_max = ws_max
        self.coarse_max_offset_m = max_offset_m
        self.coarse_border_px = border_px
    def get_logger(self) -> Any:
        return _StubLogger(self.logs)
 def _reject(st: _Stub, p: Any, g: Any, max_offset_m: Optional[float] = None) -> Optional[str]:
    return Stage2Locate._coarse_target_reject(  # type: ignore[arg-type]
        st, np.asarray(p, dtype=float), np.asarray(g, dtype=float), max_offset_m
    )
 def _on_border(st: _Stub, bbox: Any, w: int = FRAME_W, h: int = FRAME_H) -> bool:
    return Stage2Locate._bbox_on_border(st, bbox, w, h)  # type: ignore[arg-type]
 # --------------------------------------------------------------------------- #
 # 守卫一：被画面边缘切掉的框一律弃用
 # --------------------------------------------------------------------------- #
 def test_clipped_bbox_is_rejected():
    """负向基线：事故那一帧的框贴左边（x0=0），必须判"不可用"。"""
    assert _on_border(_Stub(), BAD_BBOX_CLIPPED) is True
 def test_degenerate_full_frame_bbox_is_rejected():
    """整幅画面当工件（四边全贴）也必须弃用 —— 它的"中心"就是画面中心，毫无信息。"""
    assert _on_border(_Stub(), DEGENERATE_BBOX) is True
 def test_normal_bbox_is_kept():
    """守卫不能过严：正常的、不贴边的框必须仍然可用，否则粗居中彻底废掉。"""
    assert _on_border(_Stub(), GOOD_BBOX) is False
 def test_bbox_touching_any_single_edge_is_rejected():
    """四条边各自贴边都要挡住（只挡左边就漏掉了真机 coarse_move 1 那种整幅图）。"""
    for bbox in (
        (0, 100, 300, 300),
        (100, 0, 300, 300),
        (100, 100, FRAME_W - 1, 300),
        (100, 100, 300, FRAME_H - 1),
    ):
        assert _on_border(_Stub(), bbox) is True, f"{bbox} 贴边却没被挡住"
 def test_border_margin_is_config_driven_not_hardcoded():
    """余量必须来自配置：margin=0 时 3px 贴边的框算贴边，margin 大时更靠内也算贴边。"""
    assert _on_border(_Stub(border_px=2), (1, 100, 300, 300)) is True
    assert _on_border(_Stub(border_px=0), (3, 100, 300, 300)) is False
    assert _on_border(_Stub(border_px=8), (6, 100, 300, 300)) is True
 def test_none_bbox_is_rejected():
    assert _on_border(_Stub(), None) is True
 # --------------------------------------------------------------------------- #
 # 守卫二：目标点在移动前就被否掉（而不是"截断到盒边照样走"）
 # --------------------------------------------------------------------------- #
 def test_old_behaviour_clamped_the_target_and_therefore_relocated_the_arm():
    """负向基线：证明"截断"不是安全兜底，而是一条真实的横移指令。
    这条是"这个检查能红"的前提证明 —— 如果哪天 ``_clamp_ws`` 改成直接拒绝，
    下面的用例就不是在测同一件事了。
    """
    st = _Stub()
    clamped = Stage2Locate._clamp_ws(st, BAD_TARGET_BASE)  # type: ignore[arg-type]
    assert clamped[1] == pytest.approx(CLAMPED_Y, abs=1e-6), "夹具前提变了"
    moved_mm = float(np.hypot(clamped[0] - BAD_TARGET_BASE[0],
                              clamped[1] - BAD_TARGET_BASE[1])) * 1000.0
    assert moved_mm > 190.0, (
        f"截断只把目标挪了 {moved_mm:.1f}mm —— 与真机日志不符，用例已失真"
    )
 def test_bad_target_is_rejected_outside_workspace():
    """真机那一帧的目标 (0.280,-0.747) 在盒外（y < -0.55），必须判误检。"""
    why = _reject(_Stub(), BAD_TARGET_BASE, GAUGE_XY)
    assert why is not None, "盒外目标没被挡住 —— 就是它把臂带走 56mm 的"
    assert "工作盒" in why, f"拒绝原因要说清是盒外（实际：{why}）"
 def test_bad_target_is_rejected_even_if_inside_workspace():
    """把目标夹进盒内之后，211.5mm 的偏移本身也必须挡住 —— 两条判据解耦。
    这样即使以后放宽工作盒，第二道判据仍然拦得住"坏深度反投影出来的点"。
    """
    inside = np.array([0.206, CLAMPED_Y, 0.320])   # 盒内（x/y 都在界内），但偏移 130mm
    err = float(np.hypot(inside[0] - GAUGE_XY[0], inside[1] - GAUGE_XY[1]))
    assert err > 0.12, "夹具前提变了：这个点已经不是大偏移"
    assert WS_MIN[0] < inside[0] < WS_MAX[0] and WS_MIN[1] < inside[1] < WS_MAX[1]
    why = _reject(_Stub(), inside, GAUGE_XY)
    assert why is not None, f"盒内但偏移 {err*1000:.1f}mm 的目标漏过去了"
    assert "上限" in why, f"拒绝原因要说清是超偏移上限（实际：{why}）"
 def test_good_target_is_accepted():
    """守卫不能过严：一阶段刚锁定时"臂在工件上方、差几厘米"的正常量必须放行。"""
    p = np.array([GAUGE_XY[0] + 0.030, GAUGE_XY[1] + 0.012, 0.320])
    assert _reject(_Stub(), p, GAUGE_XY) is None
 def test_offset_limit_is_config_driven():
    """上限是配置项：放开后同一个点必须放行，否则阈值是写死的、配置等于没接。"""
    assert _reject(_Stub(max_offset_m=0.12), BAD_TARGET_BASE, GAUGE_XY) is not None
    # 偏移上限放到 10m ⇒ 只剩"盒外"这条在拦；说明两条判据不是同一条的复述
    why = _reject(_Stub(max_offset_m=10.0), BAD_TARGET_BASE, GAUGE_XY)
    assert why is not None and "工作盒" in why
 def test_offset_guard_alone_blocks_a_point_that_is_inside_the_box():
    """只放宽工作盒（盒内）时，偏移这条必须独立生效。"""
    wide = _Stub(ws_min=np.array([-9.0, -9.0, 0.0]), ws_max=np.array([9.0, 9.0, 9.0]))
    why = _reject(wide, BAD_TARGET_BASE, GAUGE_XY)
    assert why is not None and "上限" in why
 def test_nonfinite_target_is_rejected():
    assert _reject(_Stub(), [np.nan, -0.5, 0.32], GAUGE_XY) is not None
    assert _reject(_Stub(), [0.3, np.inf, 0.32], GAUGE_XY) is not None
 # --------------------------------------------------------------------------- #
 # 接线：配置 + 调用点（写了键却没接线 = 等于没检查）
 # --------------------------------------------------------------------------- #
 def _site() -> dict:
    assert SITE.is_file(), f"找不到站点配置 {SITE}"
    return yaml.safe_load(SITE.read_text(encoding="utf-8"))
 def _stage2_src() -> str:
    p = ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py"
    assert p.is_file(), f"找不到 {p}"
    return p.read_text(encoding="utf-8")
 def test_site_yaml_has_the_two_guard_keys():
    # 路径与源码同源：stage2_locate_node 读的是 site["hole_detect"]["stage2"]
    stage2 = (_site().get("hole_detect") or {}).get("stage2") or {}
    assert "coarse_max_offset_m" in stage2, "site.yaml 没接 coarse_max_offset_m"
    assert "coarse_border_px" in stage2, "site.yaml 没接 coarse_border_px"
    off = float(stage2["coarse_max_offset_m"])
    # 必须小于真机那次出事的 211.5mm、又大于合理粗居中的厘米级位移
    assert 0.03 < off < BAD_ERR_M, (
        f"coarse_max_offset_m={off} 必须夹在'合理粗居中'与'那次 211.5mm'之间"
    )
 def test_best_bbox_wires_the_border_guard():
    src = _stage2_src()
    assert "self._bbox_on_border(bbox, w, h)" in src, "贴边守卫没接到 _best_bbox"
    assert "def _bbox_on_border(" in src
    # 旧写法不许回来：贴边的框直接当检测结果用
    assert "return tuple(int(v) for v in best.xyxy)" not in src, (
        "又直接采信检测框了（没有过贴边守卫）"
    )
 def test_coarse_loop_rejects_before_moving():
    src = _stage2_src()
    assert "self._coarse_target_reject(p_base, gxy)" in src, "误检守卫没接在移动前"
    assert "def _coarse_target_reject(" in src
    # 旧写法不许回来：拿孔检测器的整幅 bbox 当粗居中目标，然后照样移动
    assert 'bbox = holes.get("bbox")' not in src, (
        "又在咬孔检测器的 bbox 当粗居中目标了（真机那帧是整幅画面）"
    )
    assert "skip_coarse = True" in src, "'跳过粗居中'必须真的不移动"
 def test_guard_reason_is_logged_with_both_numbers():
    """判误检必须把"目标点 vs 夹爪"两个数都喊出来，否则现场查不出是谁离谱。"""
    src = _stage2_src()
    assert "粗定位目标判误检" in src
    assert "**不移动**" in src, "日志要说清处置方式（不移动），别让人以为它动了"
@@ -0,0 +1,357 @@
 """轻降的下降量必须由**几何**决定，深度测量只许做交叉核对。
 两段真机事故，方向相反，都指向同一个结论：那套"规杆外环形"的间隙读数不能用。
 --------------------------------------------------------------------------
 2026-09-29（**多降**，撞工件）::
    轻降准备: 规尖-工件间隙 141.0mm → 目标 2.0mm，需下降 139.0mm
 按几何真值（悬停 0.320 − 工具长 0.245 − 工件面 0.022）尖端只在面上 **53mm**，
 它多降约 86mm，把通止规撞进孔，工件被推走 5mm（k1 x: 0.4907 → 0.4967）。
 2026-10-08（**少降**，永远到不了 2mm）::
    轻降被几何夹逼：测量出的间隙 144.0mm 与几何不符（…= 75.0mm）
    …
    已达安全下限间隙 -7.0mm，停止下降（不接触）
    === 轻降完成: 规尖悬在孔面上方 ≈18.0mm （目标 2.0mm，不接触）===
 这次读数反着错：悬停时报 144mm（几何真值 75mm），只降 24mm 后又翻成 **−7mm**，
 ``SAFE_FLOOR`` 于是把轻降停在离面约 50mm 处 —— **永远到不了 2mm**。
 结论：同一套读数在两个方向上各错一次、幅度 150mm 量级、还随位姿乱跳。现在
 ``_descend_above_hole`` **只**用几何算下降量（法兰 FK − 工具长 − 工件面 − final_gap），
 深度读数只在偏差 >10mm 时喊一声，**不参与任何决策**。安全由三条物理/几何机制保证：
  1. 几何预算上限 ``descent_budget_m``（与 ``gauge_fsm.hover_z_guard`` 同源）；
  2. 腕部 FT 接触检测 ⇒ 停 + 回退 1mm；
  3. 任一步 ``_goto`` 没走到位 ⇒ 停，保持当前高度。
 这些用例全是**离线**的：``_descend_above_hole`` 只用到少量成员，用打桩对象即可，
 不建任何 ROS 实体（与 ``test_stage2_no_follow`` 同一套做法）。
 """
 from __future__ import annotations
 from typing import Any, List, Optional
 import numpy as np
 from arm_application.stage2_locate_node import Stage2Locate
 # 与真机那次一致的一组数（site.yaml 现行值）
 _HOVER_Z = 0.320
 _TOOL_LEN = 0.245
 _SURFACE_Z = 0.022
 _FINAL_GAP = 0.002
 # ⇒ 几何间隙 = 0.320 − 0.245 − 0.022 = 53mm；需下降 = 51mm
 _GAP0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
 _BUDGET = _GAP0 - _FINAL_GAP
 # 真机实测的两个坏读数（工件面被量成 242mm 而不是 ~175mm，差 86mm）
 _BAD_Z_WP = 0.242
 _TIP_D = 0.101
 _BAD_GAP0 = _BAD_Z_WP - _TIP_D  # = 141mm
 class _StubLogger:
    """把日志写进调用方给的 sink —— 必须与 ``_StubStage2.logs`` 是**同一个** list，
    否则断言永远看不到日志（踩过一次：两个 list 各写各的）。"""
    def __init__(self, sink: List[str]) -> None:
        self._sink = sink
    def _add(self, msg: str) -> None:
        self._sink.append(str(msg))
    info = _add
    warn = _add
    error = _add
 class _StubStage2:
    """``_descend_above_hole`` 用到的成员全部打桩。
    ``z_wp`` 固定不变 = 复现"环形深度读数一直错/乱跳"的场景。
    """
    def __init__(
        self,
        *,
        z_wp: Optional[float] = _BAD_Z_WP,
        budget: Optional[float] = _BUDGET,
        tool_len: float = _TOOL_LEN,
        surface_z: float = _SURFACE_Z,
        contact_after_steps: Optional[int] = None,
        zwp_slope: Optional[float] = None,
        ft_fz: Optional[float] = 3.0,
    ) -> None:
        self.logs: List[str] = []
        self._logger = _StubLogger(self.logs)
        self.final_descent_enable = True
        self.final_gap_m = _FINAL_GAP
        self.final_descent_speed = 0.005
        self.vert_tol_rad = 0.01
        self.hover_z = _HOVER_Z
        self.tool_tip_below_flange_m = tool_len
        self.surface_z_m = surface_z
        self.descent_budget_m = budget
        self.descent_final_gap_m: Optional[float] = None
        self._z_wp = z_wp
        # zwp_slope ≠ None 时复现 2026-10-08 实测那种"读数随下降乱掉"：
        # 真实工件面深度只随下降 1:1 减小，但那个环形读数掉得**快很多倍**
        # （实测：降 24mm 就从 +144mm 间隙翻到 −7mm）。斜率 6 正是这个倍率。
        self._zwp_slope = zwp_slope
        self._cur_z = _HOVER_Z
        self._ft_fz = ft_fz
        self._contact_after = contact_after_steps
        # 记录每一次实际下发的下降步长，用来核对累计行程
        self.steps_mm: List[float] = []
        self.pins = 0
    # --- 被测函数会调到的钩子 ---
    def _state(self, _name: str) -> None:
        return None
    def _measure_gauge_state(self) -> Any:
        if self._zwp_slope is not None and self._z_wp is not None:
            z_wp = self._z_wp - self._zwp_slope * (_HOVER_Z - self._cur_z)
        else:
            z_wp = self._z_wp
        return _TIP_D, np.ones((4, 4), dtype=bool), z_wp
    def _contact_detected(self, _base_fz: float) -> bool:
        # ``contact_after_steps`` 步之后"碰到工件"（复现 FT 兜底）
        return self._contact_after is not None and len(self.steps_mm) >= self._contact_after
    def _pin(self, _q: Any) -> None:
        self.pins += 1
    def _goto(self, _q: Any, Tee: Any, target: Any, **_kw: Any) -> Any:
        Tee = np.array(Tee, dtype=float)
        step_mm = (Tee[2, 3] - float(target[2])) * 1000.0
        # 记录"下降步"（正数）；回退步（负数）不计入下降行程
        if step_mm > 0.0:
            self.steps_mm.append(step_mm)
        Tee[2, 3] = float(target[2])
        self._cur_z = float(target[2])
        return True, np.zeros(7), Tee
    def get_logger(self) -> Any:
        return self._logger
 def _run(st: _StubStage2) -> Any:
    # ``_descend_above_hole`` 循环开头有 ``rclpy.ok()`` 兜底（运行期 Ctrl-C 时退出），
    # 而 ``rclpy.ok()`` 在没 init 过时恒为 False —— 不初始化的话函数会在第一行就返回。
    import rclpy
    if not rclpy.ok():
        rclpy.init()
    Tee0 = np.eye(4)
    Tee0[2, 3] = _HOVER_Z
    ok, _q, Tee = Stage2Locate._descend_above_hole(  # type: ignore[arg-type]
        st, np.zeros(7), Tee0
    )
    return ok, Tee, st
 # --------------------------------------------------------------------------- #
 # 核心回归：坏读数**不能**再决定下降量（两个方向都不行）
 # --------------------------------------------------------------------------- #
 def test_bad_reading_no_longer_shortens_the_descent():
    """2026-10-08 的回归：读数随下降乱掉时，也要**降满几何预算**到 2mm。
    旧逻辑里 ``SAFE_FLOOR`` 会被这个读数触发 —— 实测它降 24mm 就从 +144mm 翻到
    −7mm（``zwp_slope`` 复现这个倍率），于是轻降被停在离面 ~50mm 处，**永远到不了
    2mm**。这就是本次要修的现象。
    """
    ok, Tee, st = _run(_StubStage2(zwp_slope=6.0))
    assert ok is True
    total_mm = sum(st.steps_mm)
    assert abs(total_mm - _BUDGET * 1000) < 1.0, (
        f"应降满几何预算 {_BUDGET * 1000:.1f}mm 到 2mm 目标，实际只降了 {total_mm:.1f}mm "
        "—— 又被那个乱跳的读数叫停了"
    )
    gap_final_mm = (Tee[2, 3] - _TOOL_LEN - _SURFACE_Z) * 1000
    assert abs(gap_final_mm - _FINAL_GAP * 1000) < 1.0, (
        f"最终几何间隙 {gap_final_mm:.1f}mm，应当到 {_FINAL_GAP * 1000:.1f}mm"
    )
    assert Tee[2, 3] >= _SURFACE_Z + _TOOL_LEN, "规尖已低于工件面 —— 撞了"
 def test_bad_reading_cannot_drive_it_into_the_workpiece():
    """2026-09-29 的回归：读数往**大**错时，下降量也不许超过几何预算。
    那次读数报 141mm ⇒ 想降 139mm；真值只有 53mm ⇒ 多降 86mm 撞进工件。
    """
    # 读数报 141mm，同时把 budget 也按真值给（51mm）
    ok, Tee, st = _run(_StubStage2(z_wp=_BAD_Z_WP))
    assert ok is True
    total_mm = sum(st.steps_mm)
    assert total_mm <= _BUDGET * 1000 + 0.5, (
        f"下降了 {total_mm:.1f}mm > 几何预算 {_BUDGET * 1000:.1f}mm —— "
        "又是那个「多降 86mm 撞工件」的 bug"
    )
    assert Tee[2, 3] >= _SURFACE_Z + _TOOL_LEN - 1e-9, (
        f"法兰停在 {Tee[2, 3]:.4f}，规尖 {Tee[2, 3] - _TOOL_LEN:.4f} 低于工件面 {_SURFACE_Z}"
    )
 def test_disagreement_is_announced_with_both_numbers():
    """深度与几何不符时必须喊出来，并把两个数都打出来。
    否则现场只看到"下降量变了"，查不出是测量坏了还是配置写错了 —— 而"测距方法
    待修"这件事必须留在日志里，不能被静默吞掉。
    """
    _ok, _Tee, st = _run(_StubStage2())
    hit = [m for m in st.logs if "深度测距与几何不符" in m]
    assert hit, "读数与几何差 88mm 却一声不吭 —— 静默比报错难查得多"
    text = " ".join(hit)
    assert f"{_BAD_GAP0 * 1000:.1f}mm" in text, "要打出测量值，才看得出它离谱"
    assert f"{_GAP0 * 1000:.1f}mm" in text, "要打出几何值，才能核对配置"
    assert "按几何下降" in text, "要说清本次是按几何走的（读数不参与决策）"
 def test_missing_gauge_visibility_still_descends_on_geometry():
    """规不在视野（测不到）时**仍按几何下降** —— 下降量本来就不依赖规是否可见。
    旧逻辑在这里直接"跳过轻降、停在悬停高"，等于轻降功能随机失效。
    """
    st = _StubStage2(z_wp=None)  # 测不到工件面
    ok, Tee, st = _run(st)
    assert ok is True
    total_mm = sum(st.steps_mm)
    assert abs(total_mm - _BUDGET * 1000) < 1.0, "测不到就该按几何降满，而不是跳过"
    assert any("深度核对跳过" in m for m in st.logs), "跳过了核对也要讲一声"
 # --------------------------------------------------------------------------- #
 # 缺几何 ⇒ 不许降（绝不用测距去赌）
 # --------------------------------------------------------------------------- #
 def test_missing_geometry_refuses_to_descend():
    """工具长/工件面没配时**必须不下降**并报错，不能退回"按测量降"。
    2026-09-29 撞工件就是"没有几何、只有测距"时发生的。旧测试甚至把这个危险
    行为钉成了"legacy 行为"（按测量降 139mm）—— 那条是错的，已改。
    """
    st = _StubStage2(budget=None)
    ok, Tee, st = _run(st)
    assert ok is False, "缺几何必须判失败（保持悬停），不能假装完成"
    assert not st.steps_mm, "缺几何时一步都不许降"
    assert abs(Tee[2, 3] - _HOVER_Z) < 1e-9, "臂必须停在原位"
    assert any("轻降缺少几何" in m for m in st.logs), "缺几何要说清缺什么、怎么补"
 def test_zero_tool_length_refuses_to_descend():
    """工具长为 0（没标定）同样不许降 —— 那是"拿 0 当尺子"。"""
    st = _StubStage2(tool_len=0.0)
    ok, _Tee, st = _run(st)
    assert ok is False and not st.steps_mm
 # --------------------------------------------------------------------------- #
 # 真安全机制仍在：FT 接触、goto 失败
 # --------------------------------------------------------------------------- #
 def test_ft_contact_stops_and_backs_off():
    """腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。"""
    st = _StubStage2(contact_after_steps=2)
    ok, Tee, st = _run(st)
    assert ok is True, "接触后停住即算安全收工"
    assert any("腕部力突变" in m for m in st.logs), "碰到工件却没喊"
    # 只降了 2 步就被拦住（远没走满预算）
    assert sum(st.steps_mm) < _BUDGET * 1000 - 5.0, "力突变后还在继续降"
    # 回退 1mm：最终位置应高于"最后一步的下发目标"
    assert Tee[2, 3] > _HOVER_Z - _BUDGET, "回退没生效"
 def test_every_step_logs_the_ft_force():
    """每一步都要打出 FT 轴向力 —— 这是"停住是碰到还是没跟上"的唯一直接证据。
    2026-10-08 排查"轻降在离面 ~4mm 处停住"时全靠这一行；没有它只能猜。
    """
    _ok, _Tee, st = _run(_StubStage2())
    ft_lines = [m for m in st.logs if "FT_fz=" in m]
    assert ft_lines, "每步的 FT 读数没打进日志 —— 排查时无从判断是否接触"
    assert any("基线" in m for m in ft_lines), "要给出基线与增量，才看得出力有没有变"
 def test_missing_ft_baseline_is_announced():
    """轻降开始前没收到 FT 时要明说'接触兜底不生效'（不许静默少一道保险）。"""
    _ok, _Tee, st = _run(_StubStage2(ft_fz=None))
    assert any("接触兜底不生效" in m for m in st.logs), (
        "FT 没数据却一声不吭 —— 会让人以为有接触兜底"
    )
 def test_goto_failure_stops_the_descent():
    """某一步走不到位（被挡/碰到东西）⇒ 停，保持当前高度，并判失败。"""
    st = _StubStage2()
    # 从第 3 步起 _goto 报失败
    orig = st._goto
    calls = {"n": 0}
    def _goto(q, Tee, target, **kw):
        calls["n"] += 1
        if calls["n"] >= 3:
            return False, q, Tee
        return orig(q, Tee, target, **kw)
    st._goto = _goto  # type: ignore[assignment]
    ok, Tee, st = _run(st)
    assert ok is False, "走不到位必须判失败（安全优先）"
    assert any("轻降中途未到位" in m for m in st.logs)
    assert Tee[2, 3] > _SURFACE_Z + _TOOL_LEN, "走不到位时也不许低于工件面"
 # --------------------------------------------------------------------------- #
 # 不许回退到"把目标值当结果汇报"
 # --------------------------------------------------------------------------- #
 def test_wired_final_gap_is_reported_from_geometry_not_target():
    """收尾要汇报**几何实算**的间隙，不能把目标值（2.0mm）念一遍。
    旧日志就出现过"停在离面 18mm"却同时打印"悬在孔面上方 ~2.0mm"，现场会以为
    轻降做到了（R1.1：不许把没做到说成做到）。
    """
    _ok, _Tee, st = _run(_StubStage2())
    assert st.descent_final_gap_m is not None, "没记录最终间隙"
    assert abs(st.descent_final_gap_m * 1000 - _FINAL_GAP * 1000) < 1.0
    assert any("轻降完成" in m and "几何" in m for m in st.logs), (
        "收尾日志要标明这是几何量，且给出可核对的分量"
    )
 def test_source_no_longer_uses_depth_to_stop_the_descent():
    """守卫测试：源码里不许再出现"用深度间隙叫停轻降"的逻辑。
    这是**反向**证据 —— 只要有人把 SAFE_FLOOR / _measure_z_wp 叫停逻辑加回来，
    这条立刻红（比"单元测试碰巧还能过"更直接）。用语法树判据（不是文本匹配），
    这样解释这件事的注释/文档字符串不会把它误判成"逻辑又回来了"。
    """
    import ast
    from pathlib import Path
    src = (
        Path(__file__).resolve().parents[1]
        / "arm_application"
        / "stage2_locate_node.py"
    ).read_text(encoding="utf-8")
    tree = ast.parse(src)
    fn = next(
        n
        for n in ast.walk(tree)
        if isinstance(n, ast.FunctionDef) and n.name == "_descend_above_hole"
    )
    names = {n.id for n in ast.walk(fn) if isinstance(n, ast.Name)}
    attrs = {n.attr for n in ast.walk(fn) if isinstance(n, ast.Attribute)}
    assert "SAFE_FLOOR" not in names, "深度安全下限又回来了 —— 它就是那个误停的元凶"
    assert "_measure_z_wp" not in attrs, "又在循环里重测深度当停止条件了"
    # 交叉核对必须仍在（不是把测量悄悄删掉了）
    assert "_measure_gauge_state" in attrs, "连交叉核对都没了 —— 那就没法发现测距坏了"
@@ -0,0 +1,308 @@
 """精对准的「工具位置」必须取自**视觉量到的规尖**，不能再用法兰轴线。
 为什么（2026-10-08 真机 arm 89）
 --------------------------------
 规是**夹在爪里**的零件，它相对法兰轴线可以整体偏出约 10mm。实测（同一帧、同一
 条手眼链，n=8）：
 * 规杆轴线相对法兰 = **(+10.7, −0.1) mm**（σ≈0.5mm）；最深像素法 (+8.8, −1.8) mm
 * 而 ``hover_offset_ee_xy_m`` 写的是 ``[0, 0]``（"平行爪同轴"）⇒ 精对准把**法兰**
  对到孔上 ⇒ 规尖实际离孔心 ~10mm ⇒ 轻降时 φ6 规尖骑在 φ7 孔的**孔沿**上，
  腕力从 −1N 蹦到 −31N→−63N 卡死。现场看到的是"轻降在离面 4mm 停住"。
 而孔检测本身重复性极好（k1 基座坐标 σ=(0.17,0.04)mm）—— 精度不是瓶颈，
 "对的是谁"才是。所以改成在同一帧里同时解「规尖」与「孔」，两者共用手眼链。
 真机复跑还暴露了第二件事：**工具长读数会跳**（192~223mm）——规尖细、反光，掠射角
 下深度容易丢，丢了以后"最深有效像素"往上跑、工具长只会变短。逐拍重算偏移会把
 "规杆略有倾斜"引入的 ±0.2~0.4mm 抖动带进残差，残差在 0.44~0.85mm 之间来回荡、
 收不进 0.25mm。偏移是**刚体量**，所以**量准一份就冻结**，并且只取"端头看得全"
 （工具长最长）那一簇。
 这些用例全是**离线**的：只用到少量成员，用打桩对象即可。
 """
 from __future__ import annotations
 from typing import Any, Dict, Optional, Tuple
 import numpy as np
 from arm_application.stage2_locate_node import Stage2Locate
 _INTR = {"fx": 400.0, "fy": 400.0, "cx": 400.0, "cy": 240.0}
 _TEE = np.eye(4)
 _TEE[0, 3] = 0.50
 _TEE[1, 3] = -0.10
 _TEE[2, 3] = 0.55
 # 相机朝下、相对竖直前倾约 30°（≈真机那个 29.7° 的视角）。
 # 用旋转而不是纯平移——纯平移会让"更深"在基座里变成**更长**的工具长，与真机相反，
 # 那样这条链就测不出"端头丢像素 ⇒ 工具长变短"这件事。
 _R_CB = np.array([[1.0, 0.0, 0.0], [0.0, -0.866, -0.5], [0.0, 0.5, -0.866]])
 _T_CB = np.array([0.50, -0.10, 0.45])
 def _rod_frame(v1: int = 300, u0: int = 400, v0: int = 100, near: float = 0.06,
               far: float = 0.12, half: int = 8, w: int = 800, h: int = 480) -> np.ndarray:
    """造一帧含"规杆"的深度图：中央竖带，深度随 v 线性变深（越靠下越深）。
    深度只随 ``v`` 的**绝对位置**变（用固定的 200px 量程），所以 ``v1`` 截短 = 规尖
    那几个像素丢了 —— 正好复现真机那个"最深有效像素往上跑、工具长变短"。
    """
    depth = np.full((h, w), 0.50, np.float32)
    for v in range(v0, v1 + 1):
        f = min(1.0, (v - v0) / 200.0)
        depth[v, u0 - half : u0 + half] = near + f * (far - near)
    return depth
 class _StubLogger:
    def __init__(self) -> None:
        self.msgs: list = []
    def _add(self, msg: str) -> None:
        self.msgs.append(str(msg))
    info = _add
    warn = _add
    error = _add
 class _Stub:
    """被测方法用到的成员。"""
    tip_min_m = 0.04
    tip_max_m = 0.60
    tip_near_m = 0.15
    tip_band_frac = 0.25
    tip_min_px = 150
    tip_len_min = 0.15
    tip_len_max = 0.32
    tip_off_tol = 0.003
    tip_calib_n = 4
    tool_tip_below_flange_m = 0.19
    def __init__(self, t_cb: Tuple[float, float, float] = tuple(_T_CB)) -> None:
        self._t_cb = np.array(t_cb, float)
        self._tip_off: Optional[np.ndarray] = None
        self._tip_samp: list = []
        self._tip_len_m: Optional[float] = None
        self._logger = _StubLogger()
    def get_logger(self) -> Any:
        return self._logger
    # 真实实现（纯数学）——换掉就等于没测到那条链
    def _px_to_cam(self, u: float, v: float, z: float, intr: Dict[str, float]) -> np.ndarray:
        return Stage2Locate._px_to_cam(self, u, v, z, intr)  # type: ignore[arg-type]
    def _cam_to_base(self, p_cam: np.ndarray) -> np.ndarray:
        return _R_CB @ np.asarray(p_cam, float) + self._t_cb
 def _call(st: _Stub, depth: np.ndarray):
    return Stage2Locate._tip_offset_from_depth(  # type: ignore[arg-type]
        st, depth, _INTR, _TEE
    )
 def _feed(st: _Stub, depth: np.ndarray) -> Tuple[Optional[np.ndarray], Optional[float]]:
    """走一遍"量一帧 → 攒样本 → 试冻结"的完整流程。"""
    off, tlen, _why = _call(st, depth)
    if off is not None and tlen is not None:
        st._tip_samp.append((off, tlen))
        Stage2Locate._refresh_tip_offset(st)  # type: ignore[arg-type]
    return off, tlen
 # --------------------------------------------------------------------------- #
 # 正常路径
 # --------------------------------------------------------------------------- #
 def test_measures_the_rod_tip_offset():
    """规杆在中央、深度合理时，要真的量出「规尖相对法兰」的基座 XY 偏移 + 工具长。"""
    off, tlen, why = _call(_Stub(), _rod_frame())
    assert off is not None and tlen is not None, f"没量到规尖：{why}"
    # 规杆在 u=400=cx 的正中央 ⇒ 基座 X 偏移应≈0
    assert abs(float(off[0])) < 0.001, f"规杆居中，X 偏移应≈0，实际 {off[0]*1000:+.2f}mm"
    assert abs(float(off[1])) < 0.15, f"Y 偏移 {off[1]*1000:.1f}mm 离谱"
    assert 0.15 <= tlen <= 0.32, f"工具长 {tlen*1000:.0f}mm 不在合法区间"
    assert "工具长" in why, "说明里要给出反投影出的工具长（可核对）"
 def test_offset_is_stable_frame_to_frame():
    """同一姿态连拍：偏移是刚体量，重复帧之间必须几乎不动。"""
    st = _Stub()
    off1, _t1, _ = _call(st, _rod_frame())
    assert off1 is not None
    off2, _t2, why = _call(st, _rod_frame())
    assert off2 is not None, f"重复帧被误判：{why}"
    assert float(np.linalg.norm(off2 - off1)) < 1e-6
 # --------------------------------------------------------------------------- #
 # 判据各自都要能真的失败（负向用例）
 # --------------------------------------------------------------------------- #
 def test_rejects_when_nothing_is_near():
    """近端什么都没有（规不在视野）⇒ 必须判失败，不能返回一个假偏移。"""
    depth = np.full((480, 800), 0.50, np.float32)  # 全在远端
    off, tlen, why = _call(_Stub(), depth)
    assert off is None and tlen is None, "近端没物体却量出了偏移 —— 那是拿台面当规"
    assert "近端" in why
 def test_rejects_when_rod_is_outside_the_center_band():
    """物体在画面边上（不是夹在爪里的规）⇒ 必须拒绝。"""
    off, _tlen, why = _call(_Stub(), _rod_frame(u0=60))
    assert off is None, "画面边上的东西不是规，不许采信"
    assert "近端" in why
 def test_rejects_implausible_tool_length():
    """反投影出的工具长荒谬（量到台面/工件）⇒ 必须拒绝。
    把相机系平移的 z 抬到 0.75 ⇒ 反投影的基座 z 高于法兰 ⇒ 工具长为负。
    """
    st = _Stub(t_cb=(0.50, -0.10, 0.75))
    off, _tlen, why = _call(st, _rod_frame())
    assert off is None, "工具长为负也敢采信 —— 会把臂往工件里带"
    assert "工具长" in why and "不是规" in why
 # --------------------------------------------------------------------------- #
 # 冻结：偏移是刚体量，量准一次就定住
 # --------------------------------------------------------------------------- #
 def test_freezes_offset_after_consistent_samples():
    """攒够一致样本后必须**冻结**，且冻结后不再被后续样本改动。
    为什么要冻结：逐拍重算会把"规杆略有倾斜"引入的 ±0.2~0.4mm 抖动带进残差
    （2026-10-08 真机残差 0.44~0.85mm 来回荡、收不进 0.25mm）。
    """
    st = _Stub()
    for _ in range(st.tip_calib_n):
        _feed(st, _rod_frame())
    assert st._tip_off is not None, "样本一致却没冻结"
    frozen = st._tip_off.copy()
    assert any("已冻结" in m for m in st._logger.msgs), "冻结了要讲一声（否则现场不知道基准何时定的）"
    for _ in range(3):  # 再喂几帧：冻结后不许改
        _feed(st, _rod_frame(u0=402))
    assert np.allclose(st._tip_off, frozen), "冻结后又被改了 —— 那冻结就没意义"
 def test_does_not_freeze_when_samples_disagree_without_a_majority():
    """样本分成**势均力敌的两派**时绝不许冻结 —— 宁可继续用退路 + 报失败。
    （若有一派能凑够 ``tip_calib_n`` 拍一致，那按多数派冻结是对的：外点就是靠这个
    剔掉的 —— 见 test_freezes_offset_after_consistent_samples。）
    这是 2026-10-08 那次真机"残差 0.15mm 的假达标"的守卫：只要偏移量不准，
    对准结果就必须判不可信（见 test_success_is_refused_without_tip_offset）。
    """
    st = _Stub()
    good = _rod_frame()          # 偏移 ≈ x=0
    far = _rod_frame(u0=470)     # 挪到别的 u ⇒ x 偏移差 ~21mm
    for i in range(st.tip_calib_n):  # 各一半，谁都不够多数派
        _feed(st, good if i % 2 == 0 else far)
    assert st._tip_off is None, "两派势均力敌（各 2 拍）竟然冻住了 —— 定了个假基准"
 def test_reports_tool_length_from_the_consistent_samples():
    """冻结时要并记工具长，并在与 site.yaml 差 >3mm 时告警（夹持变动的唯一线索）。"""
    st = _Stub()
    st.tool_tip_below_flange_m = 0.30  # 故意与量到的 ~0.19m 差很多
    for _ in range(st.tip_calib_n):
        _feed(st, _rod_frame())
    assert st._tip_off is not None
    assert st._tip_len_m is not None, "冻结了却没记工具长"
    warn = [m for m in st._logger.msgs if "site.yaml 的工具长" in m]
    assert warn, "量的工具长与配置差一大截却一声不吭 —— 夹持变动的线索就这么丢了"
 # --------------------------------------------------------------------------- #
 # 守卫测试：精对准必须真的用规尖，且保留退路
 # --------------------------------------------------------------------------- #
 def test_align_loop_uses_visual_tip_with_flange_fallback():
    """源码守卫（语法树判据）：精对准循环里
      · 必须调用 ``_tip_offset_from_depth``（视觉规尖）；
      · 必须调用 ``_refresh_tip_offset``（量准一次就冻结）；
      · 必须仍能用 ``_gauge_xy`` 兜底（规尖量不到时不至于完全没方向）；
      · 结果里要标出这一拍用的是哪个（``gauge_source``），否则现场分不清。
    """
    import ast
    from pathlib import Path
    src = (
        Path(__file__).resolve().parents[1]
        / "arm_application"
        / "stage2_locate_node.py"
    ).read_text(encoding="utf-8")
    tree = ast.parse(src)
    attrs = {n.attr for n in ast.walk(tree) if isinstance(n, ast.Attribute)}
    assert "_tip_offset_from_depth" in attrs, "精对准没用视觉规尖 —— 又回到拿法兰对孔了"
    assert "_refresh_tip_offset" in attrs, "偏移没走『量准一次就冻结』的路子 —— 残差会被抖动拖住"
    assert "_gauge_xy" in attrs, "连退路都没了：规尖一时量不到就完全没方向"
    consts = {n.value for n in ast.walk(tree) if isinstance(n, ast.Constant)}
    assert "gauge_source" in consts, "结果里没标这一拍用的是规尖还是法兰，现场没法核对"
    assert "align_trusted" in consts, "结果里没标这次对准可不可信，现场没法核对"
 def test_success_is_refused_without_tip_offset():
    """**残差达标但规尖偏移没量到**时，必须判失败 —— 不许把没做到说成做到。
    2026-10-08 真机复跑就栽在这：``tip_offset_m=None``、残差 0.15mm 看着漂亮，
    其实那一拍用的是**法兰轴线**（退路），法兰压在 k1 上、规尖偏出 ~11mm。
    没有这条守卫，日志会报"二阶段 OK 残差 0.15mm"，而规根本没对上孔。
    判据走**语法树**而不是文本匹配：必须存在一个"残差达标"的分支，其（含嵌套的）
    分支体里同时写了 ``align_trusted`` 与 ``fail_reason``。文本匹配会被方法里
    别处的 `_tip_off is None` 蒙混过去（试过，真的会漏）。
    """
    import ast
    from pathlib import Path
    src = (
        Path(__file__).resolve().parents[1]
        / "arm_application"
        / "stage2_locate_node.py"
    ).read_text(encoding="utf-8")
    fn = next(
        n
        for n in ast.walk(ast.parse(src))
        if isinstance(n, ast.FunctionDef) and n.name == "_run_locate"
    )
    def _consts(node: ast.AST) -> set:
        return {x.value for x in ast.walk(node) if isinstance(x, ast.Constant)}
    tol_branches = [
        n for n in ast.walk(fn) if isinstance(n, ast.If) and "align_tol" in ast.unparse(n.test)
    ]
    assert tol_branches, "没找到『残差达标』那个分支 —— 结构变了，这条守卫要跟着改"
    guarded = [
        n for n in tol_branches if {"align_trusted", "fail_reason"} <= _consts(n)
    ]
    assert guarded, (
        "『残差达标』分支里没有『规尖没量到 ⇒ align_trusted=False + fail_reason』—— "
        "假达标（对的是法兰）又会蒙混成成功"
    )
 def test_default_is_tip_vision_on_in_site():
    """site.yaml 必须默认开视觉规尖，且容差收到 0.25mm。
    容差是关键：0.7mm 会让"规偏 10mm"这种系统偏差在容差里被放过去
    （原来残差 0.69mm 看着"通过"，其实是拿法兰对的孔）。
    """
    from pathlib import Path
    import yaml
    site = yaml.safe_load(
        (Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text(encoding="utf-8")
    )
    st2 = ((site.get("hole_detect") or {}).get("stage2") or {})
    assert st2.get("align_tip_vision") is True, "默认必须用视觉规尖"
    assert float(st2.get("align_tol_m")) <= 0.00025 + 1e-9, (
        f"align_tol_m={st2.get('align_tol_m')} 太松：会放过系统性的对孔偏差"
    )
    assert int(st2.get("align_max_iters")) >= 12, "收紧容差后迭代上限要跟上"
@@ -14,6 +14,7 @@ from rclpy.node import Node
 from arm_msgs.action import FollowJointTrajectory
 from arm_msgs.srv import SetNamedPose
 from std_srvs.srv import Trigger
 DEFAULT_JOINTS = [
    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
@@ -64,53 +65,77 @@ class NamedPoseServer(Node):
        self.create_service(
            SetNamedPose, 'set_named_pose', self._on_set, callback_group=self._cb
        )
        # ``/home``（std_srvs/Trigger）：契约里的"回初始位"入口。
        #
        # **为什么这里也要有一个**：真机上 ``/home`` 原本由 ``arm_driver_flexiv``
        # 提供（它只是个包装，内部还是调 ``/set_named_pose``），但 real 模式的
        # bringup 起的是 ``flexiv_bringup + named_pose_server + arm_status_node``，
        # **没有任何 launch 文件会起 arm_driver_flexiv** —— 那个驱动还会自己开
        # RDK 连接，与 ros2_control 的 flexiv_hardware 抢独占（RDK 是单客户端）。
        # 于是真机上 ``/home`` 根本不存在，通止规装配的 S00 必检因此永远过不了
        # （``gauge_boot.py`` 的 ``gauge_home`` 项 target=/home，
        # ``gauge_assembly_node`` 的 ``_call_trigger("/home")`` 同）。
        # 放在这里最合适：本节点已经握着 ``/set_named_pose`` 的实现与 JTC 客户端，
        # 不需要再起一个进程、也不碰 RDK。
        self.declare_parameter('home_pose', 'home')
        self.declare_parameter('home_speed', 0.30)
        self.create_service(
            Trigger, 'home', self._on_home, callback_group=self._cb
        )
        self.get_logger().info(
            f'命名位姿 {list(self.poses)} joints={self.joint_names}'
        )
-    def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response):
+    def _go_to_pose(self, name: str, velocity_scale: float):
-        name = request.pose_name
+        """把 ``name`` 这个命名位姿走完。返回 ``(成功?, 说明)``。
        两个服务（``/set_named_pose`` 与 ``/home``）共用这一段：分两份写的话，
        以后只改一处就会出现"手动回位能动、装配回位不能动"这类难查的偏差。
        """
        if name not in self.poses:
-            response.success = False
+            return False, f'未知位姿"{name}"。可用: {list(self.poses)}'
            response.message = f'未知位姿"{name}"。可用: {list(self.poses)}'
            return response
        if not self._client.wait_for_server(timeout_sec=3.0):
-            response.success = False
+            return False, 'follow_joint_trajectory action 不可用'
            response.message = 'follow_joint_trajectory action 不可用'
            return response
        goal = FollowJointTrajectory.Goal()
        goal.joint_names = self.joint_names
        goal.num_points = 1
        goal.positions = list(self.poses[name])
-        duration = max(0.5, 1.0 / max(float(request.velocity_scale), 0.1))
+        duration = max(0.5, 1.0 / max(float(velocity_scale), 0.1))
        goal.time_from_start = [duration]
-        goal.velocity_scale = float(request.velocity_scale)
+        goal.velocity_scale = float(velocity_scale)
        self.get_logger().info(f'-> 位姿 [{name}] T={duration:.2f}s')
        send_fut = self._client.send_goal_async(goal)
        if not _wait_future(send_fut, 5.0):
-            response.success = False
+            return False, '等待目标被接受超时'
            response.message = '等待目标被接受超时'
            return response
        handle = send_fut.result()
        if handle is None or not handle.accepted:
-            response.success = False
+            return False, '目标被拒绝'
            response.message = '目标被拒绝'
            return response
        res_fut = handle.get_result_async()
        if not _wait_future(res_fut, 60.0):
-            response.success = False
+            return False, '等待轨迹结果超时'
            response.message = '等待轨迹结果超时'
            return response
        wrapped = res_fut.result()
        if wrapped is None:
-            response.success = False
+            return False, '结果为空'
            response.message = '结果为空'
            return response
        result = wrapped.result
-        response.success = bool(result.success)
+        return bool(result.success), (result.message or f'已移动到位姿 {name}')
-        response.message = result.message or f'已移动到位姿 {name}'
+
    def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response):
        response.success, response.message = self._go_to_pose(
            request.pose_name, float(request.velocity_scale)
        )
        return response
    def _on_home(self, _request: Trigger.Request, response: Trigger.Response):
        """``/home``：回到 ``home_pose`` 指定的命名位姿（默认 ``home``）。"""
        pose = str(self.get_parameter('home_pose').value)
        speed = float(self.get_parameter('home_speed').value)
        ok, message = self._go_to_pose(pose, speed)
        if not ok:
            self.get_logger().error(f'回初始位失败（pose={pose}）: {message}')
        response.success = ok
        response.message = message
        return response
@@ -10,11 +10,19 @@
 #include <chrono>
 #include <cmath>
 #include <cstdint>
 #include <cstdlib>
 #include <exception>
 #include <string>
 #include <thread>
 #include <vector>
 // RDK 网卡白名单的本地核对（见下面 resolve_network_interface_whitelist 的说明）
 #include <arpa/inet.h>
 #include <ifaddrs.h>
 #include <net/if.h>
 #include <netinet/in.h>
 #include <unistd.h>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/clock.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
@@ -159,6 +167,118 @@ void wait_for_operational(flexiv::rdk::Robot& robot, rclcpp::Logger logger)
    }
 }
 // ===== RDK 网卡白名单（2026-09-29 真机踩到）=====
 //
 // 现象：`rdk::Robot(sn)`（不传白名单 = 扫本机全部网卡）在**换网卡之后**连不上
 // 机器人，报 "Failed to connect to [Rizon4-063989]"；而
 // `rdk::Robot(sn, {"192.168.2.10"})` 立刻连上。同一时刻
 // `scripts/rdk_switch_tool.py`（Python RDK，钉了网卡）报 "RDK 正常" —— 所以
 // 不是机器人不在网，是本机多网卡时 RDK 的「全部网卡」发现选错了源地址：
 // 2026-09-29 改线（两臂 → 交换机 → USB-C 网卡）之后，enp4s0 上还挂着旧 Flexiv
 // 网段的 192.168.100.10 / 192.168.101.10，wlo1 也有地址。
 //
 // 取值优先级（三个都没有就退回 RDK 默认，行为与打这个补丁之前**完全一致**）：
 //   1. hardware 参数 `network_interface`（逗号/分号/空白分隔，网卡名或 IP 都行）
 //   2. 环境变量 `FLEXIV_RDK_NETWORK_INTERFACE`
 //   3. 环境变量 `SITE_PC_IP` —— scripts/lib/export_site_env.py 从 site.yaml 的
 //      robots[].pc_ip 导出，语义正是「本机在这一网段的地址」，就是 RDK 要的值
 //
 // ⚠️ 候选值**必须**先核对「本机确实持有」，否则 RDK 会把整张白名单过滤掉、
 //    报 "All whitelist interfaces were filtered out"，现象和「不传白名单」
 //    一样难查（现场笔记见 scripts/test_rdk_python.sh）。
 std::vector<std::string> split_whitelist(const std::string& raw)
 {
    std::vector<std::string> out;
    std::string cur;
    for (const char c : raw) {
        if (c == ',' || c == ';' || c == ' ' || c == '\t' || c == '\n' || c == '\r') {
            if (!cur.empty()) {
                out.push_back(cur);
                cur.clear();
            }
        } else {
            cur.push_back(c);
        }
    }
    if (!cur.empty()) {
        out.push_back(cur);
    }
    return out;
 }
 /// 本机当前持有的全部 IPv4 地址（不含回环）。
 std::vector<std::string> local_ipv4_addresses()
 {
    std::vector<std::string> out;
    struct ifaddrs* ifs = nullptr;
    if (getifaddrs(&ifs) != 0) {
        return out;
    }
    for (struct ifaddrs* it = ifs; it != nullptr; it = it->ifa_next) {
        if (it->ifa_addr == nullptr || it->ifa_addr->sa_family != AF_INET) {
            continue;
        }
        if ((it->ifa_flags & IFF_LOOPBACK) != 0) {
            continue;
        }
        char buf[INET_ADDRSTRLEN] = {0};
        auto* sin = reinterpret_cast<struct sockaddr_in*>(it->ifa_addr);
        if (inet_ntop(AF_INET, &sin->sin_addr, buf, sizeof(buf)) != nullptr) {
            out.emplace_back(buf);
        }
    }
    freeifaddrs(ifs);
    return out;
 }
 /// 网卡名是否存在（RDK 的白名单也接受网卡名）。
 bool is_local_interface(const std::string& name)
 {
    return !name.empty() && access(("/sys/class/net/" + name).c_str(), F_OK) == 0;
 }
 std::vector<std::string> resolve_network_interface_whitelist(
    const hardware_interface::HardwareInfo& info)
 {
    const auto from_env = [](const char* key) {
        const char* v = std::getenv(key);
        return v == nullptr ? std::string{} : std::string{v};
    };
    std::vector<std::string> candidates;
    const auto it = info.hardware_parameters.find("network_interface");
    if (it != info.hardware_parameters.end()) {
        candidates = split_whitelist(it->second);
    }
    if (candidates.empty()) {
        candidates = split_whitelist(from_env("FLEXIV_RDK_NETWORK_INTERFACE"));
    }
    if (candidates.empty()) {
        candidates = split_whitelist(from_env("SITE_PC_IP"));
    }
    if (candidates.empty()) {
        return {};
    }
    const auto local_ips = local_ipv4_addresses();
    std::vector<std::string> resolved;
    for (const auto& c : candidates) {
        const bool held = is_local_interface(c)
            || std::find(local_ips.begin(), local_ips.end(), c) != local_ips.end();
        if (held) {
            resolved.push_back(c);
        } else {
            // 这里不用 private 的成员 getLogger()（匿名命名空间里访问不到）；
            // 它本身就是 `rclcpp::get_logger("FlexivHardwareInterface")`。
            RCLCPP_WARN(rclcpp::get_logger("FlexivHardwareInterface"),
                "RDK 网卡白名单项 '%s' 既不是本机网卡名也不是本机地址 —— 已忽略"
                "（传不存在的项会让 RDK 过滤掉整张白名单，反而连不上机器人）",
                c.c_str());
        }
    }
    return resolved;
 }
 }
 namespace flexiv_hardware {
@@ -331,7 +451,21 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init(
    try {
        RCLCPP_INFO(getLogger(), "Connecting to robot %s ...", robot_sn.c_str());
        // 钉住网卡再连：不钉的话本机多网卡时 RDK 会发现不到机器人
        // （取不到有效值时退回 RDK 默认，行为与补丁前一致）。
        const auto network_whitelist = resolve_network_interface_whitelist(info_);
        if (network_whitelist.empty()) {
            RCLCPP_INFO(getLogger(),
                "RDK 网卡白名单为空 —— 用 RDK 默认（扫本机全部网卡）");
            robot_ = std::make_unique<flexiv::rdk::Robot>(robot_sn);
        } else {
            std::string joined;
            for (const auto& item : network_whitelist) {
                joined += (joined.empty() ? "" : ", ") + item;
            }
            RCLCPP_INFO(getLogger(), "RDK 网卡白名单: [%s]", joined.c_str());
            robot_ = std::make_unique<flexiv::rdk::Robot>(robot_sn, network_whitelist);
        }
    } catch (const std::exception& e) {
        RCLCPP_FATAL(getLogger(), "Could not connect to robot");
        RCLCPP_FATAL(getLogger(), e.what());