应用层修复：S00 自检假阴性、S07 触觉假断流、S07 下探静默卡死

三处都是"看着像硬件问题、其实是判据问题"的假警报： 1. S00 自检假阴性（gauge_boot.py）探测等待时用 time.sleep 不 spin → 订阅回调根本没机会执行，于是**健康的硬件也会被判"没有数据"**。改成等待期间持续 spin。 2. S07 触觉假断流（stage3_search_node.py）判据原本是 |fnormal| > 1.0，但力本来就可能是零或很小（真机轻触、仿真刚接触）→ 明明在发数据却判"触觉断流"。改成"数据够新（TACTILE_MAX_AGE_S 内）+ connected" —— 判活看的是 **有没有在发**，不是**数值大不大**。 3. S07 下探静默卡死（gauge_fsm.py / gauge_assembly_node.py） _goto 的到位容差 ≥ 步长时，每一步都被判成"已在容差内"，臂一步都不走，而外层只看到"没进度"→ 死循环到超时，日志里看不出原因。新增 descend_tol_m / descend_iter_budget / descend_progress：容差从步长推导（不该手工调大），并显式判"连续 N 步零位移"。容差 ≥ 步长的反例已进 site 回归测试，防止再犯。 4. site 分段化（export_site_env.py）：89/91 两臂的 servo/handeye/点位各一段，避免"改 91 影响 89"。另：run_unit_tests.sh 作为统一测试入口（932→933 项）， devices/sim/gazebo/README.md 同步。 Co-authored-by: Cursor <cursoragent@cursor.com>
2026-09-24 14:43:19 +08:00
parent b29339e522
commit 36a59c81bf
21 changed files with 1573 additions and 99 deletions
@@ -246,6 +246,7 @@ UserCommands 系统）会让**所有**模型都 spawn 不进去（见 [坑 2](#
 | 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致告警 |
 | 相机图幅（影响像素尺度） | `config/cameras.yaml` 的 `width`/`height` **且** SDF 的 `image_width/height` | ✔ 启动时自动比对（这是最容易漏的一处，见 4.3） |
 | 触觉标度/轴向/基线 | `config/tactile.yaml` | ✔ 适配器每次启动都读 |
 | 感知来源（一阶段目标点 + 二阶段孔位） | `site.yaml` → `perception.source` | ✔ 仿真那台写 `truth`：一阶段由 `sim_truth_perception` 按场景位姿算目标点，二阶段由 `arm_sim_gazebo.truth_holes` 按场景几何投影孔位（见 §5 坑 13） |
 | 装配的悬高/速度 | **复用真机那套**（`servo.*` / `hole_detect.stage2.*`） | ✔ 有意共用 |
 | 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 的 `<gravity>` |
 | 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。改要动三处：`urdf/arm.sim.urdf.xacro` 的 `arm_prefix`、`config/controllers.yaml` 的 `sim_joint1..7`、本项 |
@@ -471,6 +472,64 @@ mimic 只能是 revolute"，挂在 `unused_finger_width_link` 上（那个 link
 适配器的做法：反馈取"与主关节直接相连、|multiplier| 最大的那个从动关节"
 （GN01 是 `left_outer_knuckle_joint`），再按耦合关系反推开口。
 ### 坑 13：二阶段的孔，在仿真里"每拍都丢"
 现象（`log_sim/gauge_flow.log`）：
 ```
 一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段
 [pipeline] === S05_snap ===
 精拍 5帧中值图  L=(265.2,320.4) R=(529.7,288.8)
 WARN  两孔基座间距 107.9mm 不像 k1/k2（期望 32–85mm），丢弃本拍
 ...（同样一行重复 8 次）
 ERROR S05 精拍定位失败 code=4
 ```
 一阶段（S04）完全正常：真值目标锁定、跟踪到位、悬停到 0.632m。所以看起来像
 "孔检测的门槛配错了"。真因有两段，**第二段才是关键**：
 1. 工件检测模型在合成图上出不了框（实测最高分 0.017，见 `models/` 的说明）→
   `hole_detect_hough` 拿不到 ROI，于是按调用方的允许退到**整幅画面**当 ROI。
   这一步本身是设计好的退路（`detect_holes(allow_full_frame_roi=True)`），
   真机上"权重不匹配但孔看得见"时就靠它。
 2. 整幅画面里还站着**夹爪和通止规**（它们就在工件上方、比工件更近）。
   HoughCircles 在它们身上找到一对"看起来对称的圆"，间距 108mm —— 真值
   58mm。于是每一拍都被间距门槛丢掉，丢满 8 拍超时。
 **为什么不能靠"把门槛放宽"解决**：107.9mm 离 58mm 不算远，放宽一点就会让这对
 **假孔**通过，然后机械臂去对准一个错的点 —— 症状推迟到 S07 才爆（拧不进 /
 掉件重放），比现在难查得多。宁可现在"每拍都丢"。
 修法：仿真里不要用检测器，用**真值孔位** —— 与一阶段的 `sim_truth_perception`
 同一套思路（都是"场景里本来就知道，何必去推"）：
 ```
 孔世界点 = scene.yaml 的物体位姿 + 模型 <visual name="*_ring"> 的局部位置
 孔像素   = K · T_cam_world · 孔世界点     （T 由 TF 查，K 来自 camera_info）
 ```
 * 实现在 `arm_sim_gazebo/truth_holes.py`：**纯几何**，没有 ROS 依赖，
  所以在没有 Gazebo 的机器上也能测（`test_truth_holes.py`，13 条）。
 * 由 `stage2_locate_node` 在 `perception.source == truth` 时接管
  `_detect_holes_img` / `_best_bbox`（`gauge_assembly_node` 继承这两个方法，
  所以 S05 的整条链一起生效）。
 * 真机（`source: yolo`）那条路**一行都没改**。
 * 为什么用 `perception.source` 而不是新加一个"仿真孔位"开关：这个问题本来就是
  "谁产生视觉测量"，一阶段的目标点与二阶段的孔位只是它的两个消费者 ——
  拆成两个开关，迟早出现"一阶段走真值、二阶段走模型"的半仿状态。
 顺带被这条坑钉住的两个行为：
 * 孔在**相机背后**时真值路径返回"不可用"而不是硬给一个点（给伺服幻影目标
  会让机械臂朝错误方向走，看起来像"标定坏了"）；
 * 模型里嵌件亮环不是**正好 2 个**时直接报"有几个环"，不猜。
 验收：
 ```
 python3 -m pytest src/arm_sim_gazebo/test/test_truth_holes.py -q
 ```
 ---
 ## 6. 现状与进度
@@ -80,7 +80,9 @@ _SIDE_WORDS = {
    "右臂": "right",
 }
-_ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo")
+_ARM_SECTION_KEYS = (
    "camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo", "perception",
 )
 def arm_short_id(sn: str) -> str:
@@ -36,6 +36,15 @@ DRY_RUN=false
 HOLES=""
 BACKEND="auto"
 # 用户**这一条命令**里有没有显式指定臂？要看原始 argv：
 # `parse_arm_args` 只设 ROBOT_SN，而调用者的 shell 可能已经 export 了 ROBOT_SN
 #（比如先 source 过 env.sh）—— 那种情况下"仿真要不要默认成 sim 台"就分不清了。
 _HAD_ARM=false
 for _a in "$@"; do
  if [[ "${_a}" == "--arm" || "${_a}" == "--sn" ]]; then _HAD_ARM=true; fi
 done
 unset _a
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/lib/parse_arm_args.sh"
 parse_arm_args "$@"
@@ -57,6 +66,15 @@ case "${BACKEND}" in
  *) echo "--backend 只能是 auto|contract|legacy，收到 ${BACKEND}" >&2; exit 1 ;;
 esac
 if [[ "${USE_SIM}" == true && "${_HAD_ARM}" == false ]]; then
  # 仿真默认选「仿真台」——就是 site.yaml 里 robots[] 的那条 `sn: sim`
  #（TF/关节名是 sim_* 前缀，相机是 sim_d405）。**必须在 source sim_env.sh 之前**：
  # 那里会把 site.yaml 导成 SITE_*，之后再改就晚了。
  # 显式 --arm 仍然优先：想用仿真核对某台真机的点位时会这么用。
  export ROBOT_SN=sim
  export ROBOT_ARM=sim
 fi
 if [[ "${USE_SIM}" == true ]]; then
  echo "=== 仿真环境（install_sim + ROS_DOMAIN_ID=77） ==="
  # shellcheck disable=SC1091
@@ -77,7 +95,7 @@ if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
 fi
 echo "=== 通止规装配  arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN}) ==="
-echo "    孔数=${HOLES:-site}  夹爪后端=${BACKEND}  干跑=${DRY_RUN}"
+echo "    孔数=${HOLES:-site}  夹爪后端=${BACKEND}  干跑=${DRY_RUN}  仿真时钟=${USE_SIM}"
 # 清掉上一轮可能残留的装配节点（同一时间只允许一个进程在指挥这条臂）
 # 不 pkill 整个 arm_application：会连相机/触觉服务一起带走（那些是本脚本的前置）
@@ -91,6 +109,34 @@ rm -f "${RESULT_FILE}"
 ARGS=(gripper_backend:="${BACKEND}" result_file:="${RESULT_FILE}")
 if [[ -n "${HOLES}" ]]; then ARGS+=(holes_total:="${HOLES}"); fi
 if [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi
 # 仿真必须 use_sim_time:=true：本流程的"帧新不新"和 TF 查询都按节点时钟比对。
 # 忘了带的现场是"相机 13Hz 明明在发，S00 却报腕部相机无数据（过期 1.79e9s）" ——
 # 报出来的原因是假的，人只会去查相机。
 if [[ "${USE_SIM}" == true ]]; then ARGS+=(use_sim_time:=true); fi
 # 仿真里手眼是"URDF 的真值导出成文件"（不标定）。没有这份文件，S04 的
 # visual_servo 会直接拒绝启动（"手眼标定结果缺失"），整条流程卡在 S04↔S03 循环。
 # 所以这里先把它刷成当前真值（已是最新则什么都不做）。
 # 手眼静态 TF 由 URDF 提供 → 本流程**不**发（否则同一对父子两个发布者）。
 if [[ "${USE_SIM}" == true ]]; then
  ARGS+=(publish_handeye_tf:=false)
  echo "=== 刷新仿真手眼真值（sim_eye_in_hand.yaml / sim_d435_eye_to_hand.yaml） ==="
  if ! python3 "${ROOT}/scripts/gen_sim_handeye.py" --arm "${ROBOT_SN:-sim}"; then
    echo "ERROR: 仿真手眼真值导出失败（上面有原因）。仿真栈起来了吗？" >&2
    echo "       bash scripts/run_sim_gz.sh  之后重试。" >&2
    exit 1
  fi
  # 仿真没有"人把规塞进夹爪"这一步：不装规，整条流程会在 S07 才暴露
  #（下了 25mm 什么也没碰到，看着像力阈值问题）。所以这里上前先上料，
  # 装不上就直接停 —— 不允许带着空夹爪往下跑。
  # 真机不需要：现场由人装夹，S02 的夹持确认是预留跳过项（grip_check）。
  echo "=== 仿真上料：把通止规装进夹爪（真机由人工装夹） ==="
  if ! python3 "${ROOT}/scripts/sim_load_gauge.py"; then
    echo "ERROR: 仿真上料失败（上面有原因）。可以单独重跑：" >&2
    echo "       python3 scripts/sim_load_gauge.py            # 装规" >&2
    echo "       python3 scripts/sim_load_gauge.py --dry-run  # 只看算出来的位姿" >&2
    exit 1
  fi
 fi
 echo "=== launch gauge_assembly（自检不过会自动报缺什么） ==="
 # 不用 exec：exec 之后 Ctrl-C 直接打给 ros2 launch，这里的清理就没了；
@@ -0,0 +1,44 @@
 #!/usr/bin/env bash
 # 跑全仓库的单元测试（纯逻辑：不需要硬件、不需要 Gazebo、不发运动指令）。
 #
 # 为什么要有这个脚本，而不是让人手敲 pytest
 # ----------------------------------------
 # 测试都按**包名**导入（arm_application.* / arm_sim_gazebo.* / arm_kinematics.*），
 # 所以 PYTHONPATH 必须同时包含 src/ 下所有包目录。手敲很容易漏一个：
 # 漏了 src/arm_kinematics，`from arm_application.pipeline_node import ...` 会在
 # 收集阶段就 ModuleNotFoundError —— 现象是"测试跑了 0 个 / 1 个 error"，
 # 很容易被读成"测试环境坏了"，而不是"少了一个路径"。
 #
 # 用法：
 #     bash scripts/run_unit_tests.sh                 # 全部
 #     bash scripts/run_unit_tests.sh -k gauge        # 只跑名字带 gauge 的
 #     bash scripts/run_unit_tests.sh src/arm_sim_gazebo/test/test_sim_scene.py -x
 #
 # 环境：真机 conda 环境或仿真环境都能跑；缺可选依赖的用例自己会 skip
 #（例如 onnx/ultralytics 只在真机环境有 —— 见各测试里的 pytest.importorskip）。
 set -euo pipefail
 ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 PY="${PYTHON:-python3}"
 # 把所有 src/* 目录挂到 PYTHONPATH 最前面（包名不冲突，顺序无关）。
 paths=()
 for d in "${ROOT}"/src/*/; do
  paths+=("${d%/}")
 done
 export PYTHONPATH="$(IFS=:; echo "${paths[*]}")${PYTHONPATH:+:${PYTHONPATH}}"
 test_dirs=()
 for d in "${ROOT}"/src/*/test; do
  [ -d "$d" ] && test_dirs+=("$d")
 done
 if [ "${#test_dirs[@]}" -eq 0 ]; then
  echo "没有找到任何测试目录（src/*/test）—— 仓库不完整？" >&2
  exit 1
 fi
 cd "${ROOT}"
 if [ "$#" -gt 0 ]; then
  exec "${PY}" -m pytest "${test_dirs[@]}" -q "$@"
 fi
 exec "${PY}" -m pytest "${test_dirs[@]}" -q
@@ -50,12 +50,13 @@
 from __future__ import annotations
 import dataclasses
 import json
 import math
 import os
 import time
 from pathlib import Path
-from typing import Any, Dict, List, Mapping, Optional, Sequence, Tuple
+from typing import Any, Dict, Iterable, List, Mapping, Optional, Sequence, Tuple
 import numpy as np
 import rclpy
@@ -74,6 +75,9 @@ from arm_msgs.msg import GripperStatus
 from arm_msgs.srv import GripperExecute
 from arm_application.gauge_boot import (
    dependency_advice,
    missing_modules,
    perception_source_of,
    BootCheck,
    abs_name,
    blocking_failures,
@@ -82,15 +86,25 @@ from arm_application.gauge_boot import (
    optional_warnings,
    summary_counts,
 )
-from arm_application.gauge_config import GaugeConfig, GaugeConfigError, from_site
+from arm_application.gauge_config import (
    GaugeConfig,
    GaugeConfigError,
    from_site,
    snap_feature_of,
 )
 from arm_application.gauge_fsm import (
    DESCEND_STALL_STEPS,
    Event,
    FsmError,
    GaugeContext,
    GaugeFsm,
    State,
    descend_iter_budget,
    descend_progress,
    descend_tol_m,
    is_joint_limit_error,
    pixel_stable,
    points_stable,
    ratchet_cycles,
    screw_stop,
    tangential_jump,
@@ -114,7 +128,7 @@ 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
+from arm_application.site_io import deep_merge, load_site_dict, ros2_root
 # 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2）
 from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point
@@ -317,6 +331,8 @@ class GaugeAssembly(AssemblyPipeline):
        # 夹爪后端要按"服务实际类型"选，所以放到 S00 里做（此时才知道服务在不在）
        self._boot_results: List[Tuple[BootCheck, bool, str]] = []
        self._warn_if_sim_time_missing()
        # ---- 状态机 ----
        self.fsm = GaugeFsm(GaugeContext(holes_total=self.cfg.holes_total))
        self.grip: Optional[GripperClient] = None
@@ -337,6 +353,26 @@ class GaugeAssembly(AssemblyPipeline):
        self._coarse_depth: Optional[Image] = None
        self._coarse_info: Optional[CameraInfo] = None
        self._coarse_ns = self._d435_ns(site)
        # 手外相机名的**唯一数据源**是 site.yaml 的 cameras 注册表（上面那行）。
        # 配置段里的 gauge_assembly.coarse_camera 是"注册表里没登记手外相机"时的
        # 兜底；两者不一致时以注册表为准，但要**出声** —— 否则改了一处没改另一处，
        # 表现是"话题在发、S03 却拿到另一台相机/另一个 frame"，静默错。
        if self._coarse_ns and self.cfg.coarse_camera != self._coarse_ns:
            self.get_logger().warn(
                f"手外相机名不一致：cameras 注册表说 {self._coarse_ns}，"
                f"gauge_assembly.coarse_camera 写的是 {self.cfg.coarse_camera}。"
                f"以注册表为准（{self._coarse_ns}）。"
                "site.yaml 里那处可以删掉了（留着只会误导下一个人）"
            )
            # GaugeConfig 是 frozen 的：要改只能造一个新的（replace）。
            # 直接赋值会抛 FrozenInstanceError，而且是在 __init__ 里 ——
            # 表现为"节点起不来"，看不出跟相机名有关。
            self.cfg = dataclasses.replace(self.cfg, coarse_camera=self._coarse_ns)
        #: 各相机的**收到时刻**（单调钟）。判"有没有活的数据"用它，不用
        #: header.stamp —— 两者可能不在同一个时钟域（见 `_img_age` 的注释）。
        self._t_color: Optional[float] = None
        self._t_coarse_color: Optional[float] = None
        self._skew_warned = False
        if self._coarse_ns:
            img_t, info_t = resolve_realsense_topics(self._coarse_ns, image_topic="")
            # QoS 必须用 sensor data（BEST_EFFORT）：RealSense 驱动发的是
@@ -401,14 +437,51 @@ class GaugeAssembly(AssemblyPipeline):
        return {}
    def _d435_ns(self, site: Dict[str, Any]) -> str:
-        """手外相机的命名空间。``role: overhead_eye_to_hand`` 的那个。"""
+        """手外相机的命名空间：``role: overhead*`` 且**属于当前这台臂**的那一个。
-        cams = site.get("cameras") or {}
+
-        for _key, cfg in cams.items():
+        为什么必须按 ``arm`` 过滤：仿真那台也有手外相机（``sim_d435``），
-            if not isinstance(cfg, dict):
+        只按 role 扫会同时命中两台，而"命中哪一台"取决于字典顺序 ——
-                continue
+        同一份配置在仿真/真机之间**错而不报**地互串。
-            if str(cfg.get("role") or "").startswith("overhead"):
+
-                return str(cfg.get("name") or "").strip("/")
+        规则（确定性的三级）：
-        return ""
+          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 "").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}）—— 取第一个，"
                "但请给 site.yaml 的 cameras.* 补上 arm 字段区分，否则选哪台看运气"
            )
        return str(hits[0].get("name") or "").strip("/")
    def _eye_to_hand_file(self) -> Optional[Path]:
        """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。
        路径按**仓库根**解析而不是当前工作目录：相对 CWD 的路径会在"换个目录
        启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
        """
        root = ros2_root()
        for rel in (
            f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml",
            "handeye/results/eye_to_hand.yaml",
        ):
            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)
@@ -417,8 +490,16 @@ class GaugeAssembly(AssemblyPipeline):
        if bool(msg.data):
            self._reload_ok = True
    def _on_color(self, msg: Image) -> None:
        # 腕部相机：父类（stage2）负责存帧，这里只补"收到时刻"并顺带查时钟域。
        super()._on_color(msg)
        self._t_color = time.monotonic()
        self._note_stamp_skew(msg, "腕部相机")
    def _on_coarse_color(self, msg: Image) -> None:
        self._coarse_color = msg
        self._t_coarse_color = time.monotonic()
        self._note_stamp_skew(msg, "手外相机")
    def _on_coarse_depth(self, msg: Image) -> None:
        self._coarse_depth = msg
@@ -430,15 +511,65 @@ class GaugeAssembly(AssemblyPipeline):
        """孔名 k1/k2/...。直接用状态机的，别在节点里再实现一遍（规则 M2）。"""
        return self.fsm.ctx.hole_name()
-    def _img_age(self, msg: Optional[Image]) -> float:
+    def _img_age(self, t_recv: Optional[float]) -> float:
-        """图像从"它的时间戳"到现在过了多久 [s]。拿不到时间戳返回 1e9。"""
+        """图像从**收到的那一刻**到现在过了多久 [s]；从没收到返回 1e9。
-        if msg is None:
+
        为什么不用 `header.stamp`（原来就是那么写的，出过事）：
        节点时钟和消息时钟可能是**两个时钟域** —— 仿真里忘了 `use_sim_time`
        时，`/clock` 从 0 开始、节点却用系统时间，于是"按时间戳算年龄"会算出
        `1790217444.7s` 这种数，把"图像 13Hz 好好地流着"报成
        "腕部相机无数据"，而人只会去怀疑相机。收到时刻用单调钟记，
        与两个时钟域都无关，判"有没有活数据"永远是对的。
        """
        if t_recv is None:
            return 1e9
        return max(0.0, time.monotonic() - t_recv)
    def _stamp_skew(self, msg: Image) -> Optional[float]:
        """消息时间戳与节点时钟的差 [s]；取不到时间戳返回 None。"""
        try:
            stamp = float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) * 1e-9
        except Exception:  # noqa: BLE001
-            return 1e9
+            return None
-        return max(0.0, self.get_clock().now().nanoseconds * 1e-9 - stamp)
+        return self.get_clock().now().nanoseconds * 1e-9 - stamp
    def _note_stamp_skew(self, msg: Image, where: str) -> None:
        """时钟域不一致时说一次，别每帧都刷。
        > 1 小时就认为是两个时钟域（仿真差的是整段时间基准，
        > 不可能只差几十秒；真机上的时延是毫秒级）。
        """
        if self._skew_warned:
            return
        skew = self._stamp_skew(msg)
        if skew is None or abs(skew) < 3600.0:
            return
        self._skew_warned = True
        self.get_logger().warn(
            f"{where}的时间戳与节点时钟差 {skew:.0f}s：**不是数据问题，是时钟域不同**。"
            "仿真里给本节点加 use_sim_time:=true（run_gauge_assembly.sh --sim 已带）。"
            "不修的话 TF 查询也会全线失败 —— 因为 TF 是按时间戳比对的。"
        )
    def _warn_if_sim_time_missing(self) -> None:
        """看起来在仿真里，却没开 use_sim_time → 出声（否则症状极难定位）。
        `/clock` 有发布者 = 有人在做仿真（真机上没有 `/clock`）。这时如果
        `use_sim_time` 还是 false，所有 header.stamp 比对、TF lookup 都会错，
        而报出来的却是"相机没数据""TF 查不到"这类**假原因**。
        """
        if bool(self.get_parameter("use_sim_time").value):
            return
        try:
            n = self.count_publishers("/clock")
        except Exception:  # noqa: BLE001
            return
        if n > 0:
            self.get_logger().warn(
                "检测到 /clock 有发布者（在仿真里），但本节点 use_sim_time=false："
                "时间戳比对与 TF 查询都会失败。请加 use_sim_time:=true"
                "（scripts/run_gauge_assembly.sh --sim 会自动带）。"
            )
    def _state(self, s: str) -> None:  # 覆盖：三阶段的状态话题 + 本流程的
        super()._state(s)
@@ -517,6 +648,24 @@ class GaugeAssembly(AssemblyPipeline):
            d435_camera=self._coarse_ns or None,
            tool_name=self.tool_name,
            weights_path=self._weights,
            perception_source=self._perception_source(),
        )
    def _perception_source(self) -> str:
        """感知来源（site.yaml 顶层 perception.source，按当前臂覆盖后）；未知值报错。
        两件事在这里必须做对：
        1. **按当前臂覆盖**：本节点读的是文件里的原始 site.yaml，机器人级
           覆盖（``robots[].perception.source``）要自己合进来 —— 仿真那条
           ``source: truth`` 就写在 robots[`sim`] 下。忘了合的表现是
           "仿真里明明配了 truth，S00 还是去查 ultralytics"。
        2. **只有一处读它**：launch 文件（决定起哪个感知节点）和 S00（决定查
           不查推理依赖）必须看同一个值，否则输出会自相矛盾。
        """
        robot = self._robot_dict(self._site)
        return perception_source_of(
            deep_merge(self._site.get("perception") or {}, robot.get("perception") or {})
        )
    def _wrist_cam_ns(self) -> str:
@@ -525,10 +674,35 @@ class GaugeAssembly(AssemblyPipeline):
        cam = deep_merge(self._site.get("camera") or {}, robot.get("camera") or {})
        return str(cam.get("name") or "d405").strip().strip("/")
    def _service_snapshot(
        self, names: Iterable[str] = ()
    ) -> Dict[str, List[str]]:
        """等服务发现稳定下来，再取服务表。
        为什么不能"查一次就算"：`get_service_names_and_types()` 返回的是
        **当前已发现** 的那部分图。本节点是刚加入 DDS 的新参与者，起来那一瞬间
        别人的服务很可能还没被发现 —— 于是"服务明明在"被判成"名字不在图上"，
        同一份代码时好时坏。实测现场：仿真栈全绿时 S00 仍报 `/home`、
        `/set_named_pose`、`/gripper/status` 缺失，而 2 秒后手工 `ros2 service list`
        三个都在。这类"看起来像配置错"的抖动最难查，所以这里等到名字齐了再返回。
        等待期间必须 **spin**：发现事件要靠执行器回调处理，`time.sleep()` 只是干等。
        """
        want = {n for n in names if n}
        deadline = time.monotonic() + max(0.5, min(5.0, self.cfg.boot_timeout_s))
        snap: Dict[str, List[str]] = {}
        while True:
            snap = dict(self.get_service_names_and_types())
            if not (want - set(snap)):
                return snap
            if time.monotonic() >= deadline:
                return snap
            self._spin(0.2)
    def _probe(self, item: BootCheck) -> Tuple[bool, str]:
        """执行一项自检。返回 ``(就绪, 细节)``。"""
        if item.probe == "service":
-            found = dict(self.get_service_names_and_types()).get(item.target)
+            found = self._service_snapshot([item.target]).get(item.target)
            if found is None:
                return False, "名字不在图上"
            if item.expect_type and item.expect_type not in found:
@@ -549,6 +723,11 @@ class GaugeAssembly(AssemblyPipeline):
            sidecar = p.with_suffix(".labels.yaml")
            if not sidecar.is_file():
                return False, f"缺标签文件 {sidecar.name}（weights.sh doctor 可查）"
            # 文件在 ≠ 能推理：缺 torch/ultralytics 时这里必须失败，
            # 否则会一路跑到 S04 才以"跟踪没锁定"的形式暴露（见 gauge_boot 的注释）。
            miss = missing_modules()
            if miss:
                return False, dependency_advice(miss)
            return True, f"{p.name} + {sidecar.name}"
        # topic / tactile / camera：**要真收到消息**才算就绪。
@@ -558,7 +737,11 @@ class GaugeAssembly(AssemblyPipeline):
            detail = self._peek_topic(item)
            if detail is None:
                return True, ""
-            time.sleep(0.2)
+            # 必须 **spin** 而不是 sleep：订阅回调只在 executor 转起来时才被调用，
            # `time.sleep()` 期间一条消息都进不来。这个坑的现场是"相机/触觉/夹爪
            # 明明都在发数据，S00 却逐项报缺失"（真机和仿真都会中招），
            # 而人只会去怀疑硬件。
            self._spin(0.2)
        return False, detail or "等不到消息"
    def _probe_action(self, item: BootCheck) -> Tuple[bool, str]:
@@ -598,9 +781,18 @@ class GaugeAssembly(AssemblyPipeline):
        if item.probe == "camera":
            coarse = self._coarse_ns and f"/{self._coarse_ns}/" in item.target
            msg = self._coarse_color if coarse else getattr(self, "_color", None)
-            age = self._img_age(msg)
+            t_recv = self._t_coarse_color if coarse else self._t_color
-            if msg is None:
+            if msg is None or t_recv is None:
                return "还没收到图像（发布者不在？）"
            # 时钟域不对时**必须说清**：否则同一现象有两种完全不同的原因，
            # 而报出来的话术会把人引到错的方向（先查这个，再查数据）。
            skew = self._stamp_skew(msg)
            if skew is not None and abs(skew) > 3600.0:
                return (
                    f"图像时间戳与节点时钟差 {skew:.0f}s：数据在流，是**时钟域**不同 —— "
                    "仿真里要给节点 use_sim_time:=true（不加的话 TF 也会查不到）"
                )
            age = self._img_age(t_recv)
            if age > 3.0:
                return f"最后一帧已过期 {age:.1f}s"
            return None
@@ -628,13 +820,32 @@ class GaugeAssembly(AssemblyPipeline):
    def _step_boot(self) -> Event:
        """S00：全部必检就绪才开指令窗。"""
        self._state("S00_boot")
-        checks = self._boot_checks()
+        try:
            checks = self._boot_checks()
        except ValueError as exc:
            # 配置错（例如 perception.source 拼错）在这里拦下：继续跑下去只会在
            # S04 以"一阶段不锁定"的形式暴露，那时没人会想到是自检配置写错了。
            self.get_logger().error(f"S00 自检清单建不起来（配置有问题）：{exc}")
            return Event.BOOT_FAIL
        src = self._perception_source()
        if src != "yolo":
            self.get_logger().info(
                f"S00 自检：感知来源 perception.source = {src} —— 不查推理依赖"
                + ("（仿真真值节点随 S04 的 visual_servo 一起起）" if src == "truth"
                   else "（测量由外部节点按 visual_servo/filtered_point 提供）")
                + "；它只验控制与接口，不验检测算法"
            )
        # 夹爪后端：按**服务类型**选（真机 Trigger / 契约 GripperExecute）。
        # 选不出来时把这条也算成一项必检失败，而不是抛异常中断整段日志。
        backend_detail = ""
        try:
-            choice = pick_backend(dict(self.get_service_names_and_types()), self._gripper_prefer)
+            # 夹爪后端按"服务实际类型"选：同样要等发现齐了（见 _service_snapshot）。
            # 两种后端都有 /gripper/open、/gripper/close，等这两个就够。
            choice = pick_backend(
                self._service_snapshot(["/gripper/open", "/gripper/close"]),
                self._gripper_prefer,
            )
            for line in describe_backend_choice(choice):
                self.get_logger().info(line)
            self.grip = GripperClient(self, choice.backend, choice.status_topic)
@@ -969,18 +1180,8 @@ class GaugeAssembly(AssemblyPipeline):
        self._coarse_ok = True
        return Event.COARSE_OK
    def _eye_to_hand_file(self) -> Optional[Path]:
        """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。"""
        for cand in (
            Path(f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml"),
            Path("handeye/results/eye_to_hand.yaml"),
        ):
            if cand.is_file():
                return cand
        return None
    def _camera_alive(self, ns: str) -> bool:
-        return self._coarse_color is not None and self._img_age(self._coarse_color) < 3.0
+        return self._img_age(self._t_coarse_color) < 3.0
    def _wait_coarse_frame(self, ns: str) -> bool:
        """等手外相机的**新鲜**图像，最多 ``coarse_timeout_s`` 秒。
@@ -1106,28 +1307,63 @@ class GaugeAssembly(AssemblyPipeline):
        )
        return Event.SNAP_OK
    def _snap_feature(self) -> str:
        """S05 稳定判据用哪个特征：``bbox``（检测框像素）还是 ``motion``（法兰 mm）。
        ``auto`` 按感知来源选：真机（yolo）用检测框 —— 保持原有行为不变；
        仿真真值/外部感知没有可用的工件检测器（合成图过不了真机训练出来的模型，
        实测最高分 0.017），用检测框只会永远"不稳定"地空转，所以改用法兰位移。
        """
        return snap_feature_of(self.cfg.snap_stable_feature, self._perception_source())
    def _snap_sample(self, feature: str) -> Optional[Tuple[float, ...]]:
        """采一帧"画面特征"（单位随 feature 变：bbox=像素，motion=毫米）。"""
        if feature == "bbox":
            img = getattr(self, "_color", None)
            if img is None:
                return None
            bbox = self._best_bbox(self._bgr_from_msg(img))
            if bbox is None:
                return None
            return (0.5 * (bbox[0] + bbox[2]), 0.5 * (bbox[1] + bbox[3]))
        # motion：法兰（= 相机）在世界系里的位置。相机装在法兰上，法兰不动，
        # 画面就不会动 —— 比"检测框不动"更直接，也不依赖任何模型。
        _q, Tee = self._wait_state(0.5)
        if Tee is None:
            return None
        return (float(Tee[0, 3]) * 1000.0, float(Tee[1, 3]) * 1000.0, float(Tee[2, 3]) * 1000.0)
    def _wait_pixel_stable(self) -> bool:
-        """等画面稳定：连续采 ``snap_stable_s`` 秒的检测框中心，抖动在容差内才算稳。
+        """等画面稳定：连续采 ``snap_stable_s`` 秒的特征，抖动在容差内才算稳。
        用"到均值的最远距离"而不是相邻帧之差 —— 跟踪抖动是零均值的，
        相邻差会让"明明在晃"的时刻被判成稳定，然后拍出一张糊图（表现为
        "孔位偶尔偏 1mm"，随机且难复现）。判据实现见 gauge_fsm.pixel_stable。
        """
        feature = self._snap_feature()
        tol = self.cfg.snap_tol_px if feature == "bbox" else self.cfg.snap_tol_mm
        got_any = False
        deadline = time.monotonic() + 20.0
        while time.monotonic() < deadline:
-            samples: List[Tuple[float, float]] = []
+            samples: List[Tuple[float, ...]] = []
            t0 = time.monotonic()
            while time.monotonic() - t0 < self.cfg.snap_stable_s:
-                img = getattr(self, "_color", None)
+                s = self._snap_sample(feature)
-                if img is not None:
+                if s is not None:
-                    bbox = self._best_bbox(self._bgr_from_msg(img))
+                    samples.append(s)
-                    if bbox is not None:
+                    got_any = True
                        samples.append(
                            (0.5 * (bbox[0] + bbox[2]), 0.5 * (bbox[1] + bbox[3]))
                        )
                self._spin(0.05)
-            if pixel_stable(samples, self.cfg.snap_tol_px, self.cfg.snap_min_frames):
+            if points_stable(samples, tol, self.cfg.snap_min_frames):
                return True
        if not got_any and feature == "bbox":
            # 一条都采不到 = 检测器根本没出框。这跟"画面在晃"是**两件事**，
            # 分开报才找得到人（真机上这是"权重与图像不匹配"的典型症状）。
            self.get_logger().error(
                "S05 等稳定时工件检测器一直没出框（0 帧样本）——"
                "不是画面在晃，是没检测到工件。查：权重是否与这台相机匹配"
                "（models/yolo/README.md）、或把 gauge_assembly.snap_stable_feature "
                "设成 motion 改用法兰位移判据"
            )
        return False
    # =====================================================================
@@ -1219,19 +1455,66 @@ 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
        # 这一步的到位容差**必须小于步长**（见 gauge_fsm.descend_tol_m 的说明：
        # 容差≥步长时 _goto 判"已在容差内"直接返回，一步都不走 → 静默自旋）。
        tol_step_m = descend_tol_m(step_m)
        budget = descend_iter_budget(max_depth_m, step_m)
        self.get_logger().info(
            f"S07 步进下压：步长 {step_m * 1000:.2f}mm、到位容差 {tol_step_m * 1000:.3f}mm、"
            f"最多 {max_depth_m * 1000:.1f}mm / {budget} 步"
        )
        steps = 0
        stalls = 0
        z_prev = z0
        while float(Tee[2, 3]) - z0 > -max_depth_m:
            if steps >= budget:
                self.get_logger().error(
                    f"S07 下压步数用尽（{budget} 步 ≈ {max_depth_m * 1000:.1f}mm）仍未触发接触判据。\n"
                    "  这不是正常路径：正常应该是「接触→CONTACT_OK」或「到底→超深失败」。\n"
                    "  走到这里通常意味着臂**没在动**（控制器死区/被夹住/孔位偏得擦不到面）。"
                )
                self._retract(q, Tee, dz_m=0.01)
                return Event.TIMEOUT
            steps += 1
            target = np.array([float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]) - step_m])
            # 不用 _step_to：它把速度写死成 search_speed，而这**一步**必须慢
            # （接触判据要在"碰到"的瞬间停住，快了就是一个冲击）。速度取
            # gauge_assembly.contact_speed_m_s，其余参数与 _step_to 对齐。
            ok, q, Tee = self._goto(
-                q, Tee, target, verticalize=True, tol_m=0.0005,
+                q, Tee, target, verticalize=True, tol_m=tol_step_m,
                tol_tilt=self.vert_tol_rad * 2.5, tag="S07_step",
                v=self.cfg.contact_speed_m_s, max_hops=2,
                settle_s=max(0.06, self.settle3 * 0.3), min_dur=0.16,
            )
            if not ok:
                self.get_logger().warn("S07 单步未完全到位，继续按实测高度判断")
            # 实测位移 = 唯一的推进证据。指令发出去不等于臂动了（R5.1 静默失效）。
            z_now = float(Tee[2, 3])
            if descend_progress(z_prev, z_now, step_m):
                stalls = 0
            else:
                stalls += 1
                self.get_logger().warn(
                    f"S07 第 {steps} 步实测只走了 {(z_prev - z_now) * 1000:+.3f}mm"
                    f"（要求 ≈{step_m * 1000:.2f}mm），连续 {stalls}/{DESCEND_STALL_STEPS} 步没动"
                )
                if stalls >= DESCEND_STALL_STEPS:
                    self.get_logger().error(
                        f"S07 连续 {stalls} 步**实测零位移**，臂根本没往下走。\n"
                        "  常见原因：① 这一步的容差 ≥ 步长（_goto 判「已在容差内」直接返回）；\n"
                        "  ② 控制器死区/增益太小，0.x mm 的微步被吃掉；\n"
                        "  ③ 工件/夹具把臂顶住了（此时应报接触，却没达到切向阈值）；\n"
                        "  ④ 孔位偏了，规擦着孔壁下去但没接触面。"
                    )
                    self._retract(q, Tee, dz_m=0.01)
                    return Event.TIMEOUT
            z_prev = z_now
            if steps % 5 == 0:
                now_dbg = self._shear_axes()
                self.get_logger().info(
                    f"S07 已下压 {abs(z_now - z0) * 1000:.2f}mm"
                    f"（{steps}/{budget} 步）切向 ({now_dbg[0]:.1f}, {now_dbg[1]:.1f})"
                )
            try:
                fn, _fn_max, _shear, _contact = self._tac_fresh(self.tac_timeout)
            except Exception as exc:  # noqa: BLE001
@@ -22,7 +22,7 @@
 from __future__ import annotations
 from dataclasses import dataclass
-from typing import Dict, Iterable, List, Optional, Sequence, Tuple
+from typing import Dict, Iterable, List, Mapping, Optional, Sequence, Tuple
 #: 探针类型 —— 决定节点用哪种方式确认"就绪"。
 #:
@@ -34,6 +34,68 @@ from typing import Dict, Iterable, List, Optional, Sequence, Tuple
 #: ``detector``    算法权重能加载（推理预热）
 PROBES = ("service", "action", "topic", "tactile", "camera", "detector")
 #: ``detector`` 自检除了"权重文件在"，还要求这些 Python 模块能 import。
 #: 为什么必须查：权重文件在 ≠ 能推理。实测现场 —— 仿真环境（ros_gz_sim）里
 #: 没有 torch/ultralytics，S00 的"检测算法预热"照样报 OK，然后 S04 空转 150s
 #: 才报"没锁定"，看起来像算法或标定问题（真正的错在很远的另一处）。
 #: ``ultralytics`` 会连带把 torch 拉进来，查它就够；真要细分再看 yolo_detector。
 DETECTOR_MODULES = ("ultralytics",)
 #: 感知来源（``site.yaml`` 顶层 ``perception.source``）—— 谁产生"视觉测量"。
 #:
 #: ``yolo``     本体 perception_node（要 torch/ultralytics）
 #: ``truth``    仿真真值：按场景里已知的物体位姿直接算目标点，不跑推理
 #: ``external`` 外部节点按同一话题（``visual_servo/filtered_point``）提供
 #:
 #: ⚠️ 别和 ``yolo.backend`` 混：那个是"**怎么**推理"（tensorrt/onnx）。
 PERCEPTION_SOURCES = ("yolo", "truth", "external")
 def perception_source_of(perception_cfg: Optional[Mapping] = None) -> str:
    """读 ``perception.source`` 并校验。**未知值直接报错，不默默当 yolo**。
    默默退回 yolo 的后果：仿真里 perception_node 起不来，S00 又去查
    "推理依赖"，报出来的错跟真正的原因（source 拼错）隔着两层。
    """
    src = str(((perception_cfg or {}) or {}).get("source") or "yolo").strip().lower()
    if src not in PERCEPTION_SOURCES:
        raise ValueError(
            f"site.yaml 的 perception.source = {src!r} 不认识。"
            f"只支持 {' / '.join(PERCEPTION_SOURCES)}"
            "（yolo=本体推理，truth=仿真真值，external=外部节点提供）。"
        )
    return src
 def missing_modules(modules: Sequence[str] = DETECTOR_MODULES) -> List[str]:
    """哪些模块当前解释器里 import 不到（纯逻辑，可离线测试）。"""
    import importlib.util
    out: List[str] = []
    for m in modules:
        try:
            if importlib.util.find_spec(m) is None:
                out.append(m)
        except (ImportError, ValueError):
            # find_spec 对"父包不存在/名字非法"会抛，这同样等于"用不了"
            out.append(m)
    return out
 def dependency_advice(missing: Sequence[str]) -> str:
    """缺依赖时给**两条**出路：装它、或换掉这个后端。
    只写"请安装 X"是不够的：仿真环境常常是离线的（这台机器实测 pip 直接
    SSL 失败），那样的话现场需要的是"换一条不依赖它的路"，而不是反复重试安装。
    """
    mods = ", ".join(missing)
    return (
        f"缺推理依赖 {mods} —— 权重文件在也没用，导入不了就跑不了推理。两条路：\n"
        f"  1) 装上：pip install {' '.join(missing)}（需要联网；本机仿真环境离线时做不到）\n"
        "  2) 换后端：仿真里可以不用 YOLO —— 由外部按同一话题/同一类型提供感知结果\n"
        "     （见 devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节）"
    )
@dataclass(frozen=True)
 class BootCheck:
@@ -97,6 +159,7 @@ def build_checks(
    d435_camera: Optional[str] = None,
    tool_name: str = "",
    weights_path: str = "",
    perception_source: str = "yolo",
 ) -> List[BootCheck]:
    """按现场情况生成检查清单。
@@ -110,6 +173,10 @@ def build_checks(
    :param tool_name: ``site.yaml`` 的 ``robot.tool_name``。空 = 没指定工具系，
        则 S01 的"工具系垂直向下锁定"无法确认 → 单独列为选检告警。
    :param weights_path: 检测权重路径。空 = 不检查推理预热。
    :param perception_source: ``site.yaml`` 的 ``perception.source``。**只有
        ``yolo`` 才查推理预热**：truth/external 根本不跑推理，查了必然是
        假阴性（"缺 ultralytics"），而那是预期之内的。这里刻意**不生成**
        这一项，而不是生成一个永远 OK 的项 —— 后者会让人以为"感知也验过了"。
    清单的顺序 = 打印顺序 = 排障顺序：先臂、再夹爪、再相机、再触觉、最后算法。
    """
@@ -255,7 +322,7 @@ def build_checks(
                expect_type="arm_msgs/srv/SwitchTool",
            )
        )
-    if weights_path:
+    if weights_path and perception_source == "yolo":
        checks.append(
            BootCheck(
                key="detector",
@@ -22,6 +22,28 @@ import difflib
 from dataclasses import dataclass, fields
 from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple
 from arm_application.gauge_fsm import descend_tol_m
 #: S05 稳定判据可用的特征来源。``auto`` 按 ``perception.source`` 自动选。
 #:
 #: 为什么需要有"motion"这一路：判据原本是**工件检测框中心**的抖动（像素），
 #: 那要求检测器真的出框。仿真里合成图过不了真机训练出来的模型（实测最高分
 #: 0.017），于是 S05 会永远"不稳定"地空转 —— 而真正该等的是"相机不再动"。
 #: 用法兰位移（mm）当特征既直接又不依赖模型，且判据本身（到均值的最远距离）不变。
 SNAP_STABLE_FEATURES = ("auto", "bbox", "motion")
 def snap_feature_of(want: str, perception_source: str) -> str:
    """把 ``snap_stable_feature``（可能是 ``auto``）+ 感知来源 → 实际用哪个特征。
    抽成纯函数是为了能脱离 ROS 节点测：这个选择错了，S05 会在真机上"永远不稳定"
    或者在仿真里空转到超时，而两种表现都像是"机械臂在晃"，很难往配置上想。
    """
    want = str(want or "auto")
    if want != "auto":
        return want
    return "bbox" if perception_source == "yolo" else "motion"
 #: 合法的单位后缀。键名必须以 ``_<单位>`` 结尾（``au`` = Orisys 任意单位增量）。
 _UNIT_SUFFIX: Dict[str, str] = {
    "s": "_s",
@@ -43,6 +65,7 @@ _UNITLESS = frozenset(
        "coarse_camera",
        "coarse_conf",
        "snap_min_frames",
        "snap_stable_feature",
        "screw_max_cycles",
        "unscrew_extra_cycles",
        "holes_total",
@@ -66,7 +89,9 @@ _TYPES: Dict[str, type] = {
    "coarse_timeout_s": float,
    "snap_stable_s": float,
    "snap_tol_px": float,
    "snap_tol_mm": float,
    "snap_min_frames": int,
    "snap_stable_feature": str,
    "align_tol_mm": float,
    "contact_speed_m_s": float,
    "contact_step_mm": float,
@@ -104,7 +129,9 @@ _UNITS: Dict[str, str] = {
    "coarse_timeout_s": "s",
    "snap_stable_s": "s",
    "snap_tol_px": "px",
    "snap_tol_mm": "mm",
    "snap_min_frames": "",
    "snap_stable_feature": "",
    "align_tol_mm": "mm",
    "contact_speed_m_s": "m/s",
    "contact_step_mm": "mm",
@@ -165,7 +192,9 @@ DEFAULTS: Dict[str, Any] = {
    # S05
    "snap_stable_s": 1.0,
    "snap_tol_px": 2.0,
    "snap_tol_mm": 0.4,
    "snap_min_frames": 6,
    "snap_stable_feature": "auto",
    # S06
    "align_tol_mm": 1.0,
    # S07
@@ -213,7 +242,9 @@ class GaugeConfig:
    coarse_timeout_s: float
    snap_stable_s: float
    snap_tol_px: float
    snap_tol_mm: float
    snap_min_frames: int
    snap_stable_feature: str
    align_tol_mm: float
    contact_speed_m_s: float
    contact_step_mm: float
@@ -263,14 +294,19 @@ class GaugeConfig:
            f"限位余量 {self.extreme_margin_deg:.1f}°，关节速度 {self.extreme_speed_deg_s:.1f}°/s）",
            f"  S03 粗定位={'开' if self.coarse_enabled else '关'} 相机={self.coarse_camera} "
            f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}",
-            f"  S05 稳定 {self.snap_stable_s:.1f}s / 抖动≤{self.snap_tol_px:.1f}px / 至少 {self.snap_min_frames} 帧"
+            f"  S05 稳定 {self.snap_stable_s:.1f}s / 至少 {self.snap_min_frames} 帧 / "
            f"特征={self.snap_stable_feature}"
            f"（检测框 ≤{self.snap_tol_px:.1f}px、法兰位移 ≤{self.snap_tol_mm:.2f}mm）"
            "（稳定判据用「到均值的最远距离」，不是相邻帧之差）",
            # 悬停高度**不在这里**：它取自 servo.hole_hover_z_m（stage2 那一层），
            # 这里只印本段的容差 —— 一段配置里出现"另一个段的键"会让人改错地方。
            f"  S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）",
-            f"  S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一），"
+            f"  S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一，现场标定），"
-            f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm"
+            f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm",
-            "（触觉读数是任意单位增量，必须现场标定）",
+            f"  S07 步进 {self.contact_step_mm:.2f}mm @ {self.contact_speed_m_s * 1000:.0f}mm/s，"
            f"到位容差 {descend_tol_m(self.contact_step_mm / 1000.0) * 1000:.3f}mm"
            "（=步长的 40%，**推导出来的、不是可调项**：容差 ≥ 步长会让每一步都被判成"
            "「已在容差内」，臂一步都不走）",
            f"  S08 棘轮 {self.screw_step_deg:.0f}°×{self.screw_max_cycles} 轮 = 累计 "
            f"{self.total_screw_deg:.0f}°；停止判据「切向 >+{self.screw_shear_delta:.0f} 且 "
            f"法向 >+{self.screw_normal_delta:.0f}」",
@@ -402,6 +438,16 @@ def _sanity(cfg: GaugeConfig) -> None:
        )
    if cfg.snap_tol_px <= 0.0:
        raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}")
    if cfg.snap_tol_mm <= 0.0:
        raise GaugeConfigError(f"gauge_assembly.snap_tol_mm 必须为正，收到 {cfg.snap_tol_mm}")
    if cfg.snap_stable_feature not in SNAP_STABLE_FEATURES:
        raise GaugeConfigError(
            f"gauge_assembly.snap_stable_feature={cfg.snap_stable_feature!r} 不认识，"
            f"只支持 {sorted(SNAP_STABLE_FEATURES)}。\n"
            f"      auto   = 按 perception.source 自动选（真机 yolo→bbox、仿真/外部→motion）\n"
            f"      bbox   = 用工件检测框中心（像素），需要检测器真的出框\n"
            f"      motion = 用法兰位移（mm），不依赖检测器"
        )
 # =============================================================================
@@ -24,7 +24,7 @@ from __future__ import annotations
 import math
 from dataclasses import dataclass, field
 from enum import Enum
-from typing import Dict, List, Optional, Tuple
+from typing import Dict, List, Optional, Sequence, Tuple
 # =============================================================================
@@ -306,6 +306,26 @@ class GaugeFsm:
 # =============================================================================
 # 判据（纯函数，被节点与测试共用）
 # =============================================================================
 def points_stable(samples: List[Sequence[float]], tol: float, min_frames: int) -> bool:
    """稳定判据（任意维）：样本够多、且所有点到均值距离都 ≤ ``tol``。
    与 :func:`pixel_stable` 是同一套判据，只是不限定二维。为什么需要：
    S05 的"画面稳了再拍"在仿真里用不了检测框（合成图过不了真机训练出来的
    工件模型），于是改用**法兰位移**（mm）当特征 —— 判据本身一模一样，
    只是量纲从像素变成毫米，所以抽出来共用，而不是各写一份。
    """
    if len(samples) < max(2, int(min_frames)):
        return False
    dim = len(samples[0])
    means = [sum(float(s[i]) for s in samples) / len(samples) for i in range(dim)]
    if any(len(s) != dim for s in samples):
        return False
    for s in samples:
        if math.sqrt(sum((float(s[i]) - means[i]) ** 2 for i in range(dim))) > tol:
            return False
    return True
 def pixel_stable(samples: List[Tuple[float, float]], tol_px: float, min_frames: int) -> bool:
    """S05：像素稳定判据。
@@ -319,13 +339,7 @@ def pixel_stable(samples: List[Tuple[float, float]], tol_px: float, min_frames:
    定位图 —— 表现为"孔位偶尔偏 1mm"，随机且难复现。用"到均值的最远距离"
    才对得上"画面不动"这个物理事实。
    """
-    if len(samples) < max(2, int(min_frames)):
+    return points_stable(samples, tol_px, min_frames)
        return False
    us = [s[0] for s in samples]
    vs = [s[1] for s in samples]
    cu = sum(us) / len(us)
    cv = sum(vs) / len(vs)
    return all(math.hypot(u - cu, v - cv) <= tol_px for u, v in samples)
 def tangential_jump(
@@ -344,6 +358,56 @@ def tangential_jump(
    return any(abs(n - r) >= delta for n, r in zip(shear_now, shear_ref))
 #: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。
 #: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、
 #: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内"
 #: → 臂原地不动、循环条件永远成立 → **静默自旋**（日志停在基线那行、CPU 跑满、
 #: 既不报错也不前进）。2026-09-24 在仿真里踩到过一次，真机同参数同结果。
 DESCEND_TOL_FRAC: float = 0.4
 #: 容差下限 [m]：再小就追不动了（伺服死区/噪声会让臂永远"差一点点"）。
 DESCEND_TOL_FLOOR_M: float = 2.0e-5
 #: 判定"这一步真的往下走了"的位移下限，占步长的比例。
 DESCEND_PROGRESS_FRAC: float = 0.2
 #: 允许连续多少步毫无位移后中止（防"没接触也一直转"的静默自旋）。
 DESCEND_STALL_STEPS: int = 5
 def descend_tol_m(step_m: float) -> float:
    """S07 单步下压该给 ``_goto`` 的到位容差 [m]。
    取步长的 40%，但不超过步长的一半、不低于 ``DESCEND_TOL_FLOOR_M``。
    这样"走满这一步"才是循环推进的**唯一**出口 —— 容差比步长小，`_goto`
    就不可能因为"已经在容差内"而空转（那正是上面注释里那个静默自旋）。
    """
    if step_m <= 0.0:
        raise ValueError(f"下压步长必须为正，收到 {step_m}")
    return max(step_m * DESCEND_TOL_FRAC, min(DESCEND_TOL_FLOOR_M, step_m * 0.5))
 def descend_iter_budget(max_depth_m: float, step_m: float, slack: int = 8) -> int:
    """S07 下压循环的**硬**迭代上限：够走完整个下探深度，再多给几步余量。
    为什么要有硬上限：循环条件写的是"实测高度还没到最深"，一旦臂因为死区、
    被夹住、孔位偏了而**实际没动**，这个条件就恒真。上限不是用来"早点失败"
    的（那是 ``contact_max_depth_mm`` 的活），而是**兜住静默自旋**：
    宁可报"S07 没走完也没接触"，也不能让节点无声无息地空转到天亮。
    """
    if step_m <= 0.0:
        raise ValueError(f"下压步长必须为正，收到 {step_m}")
    if max_depth_m <= 0.0:
        return 0
    return int(math.ceil(max_depth_m / step_m)) + max(1, int(slack))
 def descend_progress(z_prev: float, z_now: float, step_m: float) -> bool:
    """这一步是否真的往下走了（法兰 z 向下为负）。
    判据是**实测**位移，不是"指令发出去了"：``_goto`` 因为容差而空转、
    或步长被控制器死区吞掉时，实测位移≈0 而指令日志一切正常。
    """
    return (float(z_prev) - float(z_now)) >= step_m * DESCEND_PROGRESS_FRAC
 def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
    """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
@@ -2,6 +2,8 @@
 from __future__ import annotations
 import logging
 from pathlib import Path
 from typing import Any
@@ -23,6 +25,9 @@ def load_config(path: str | Path | None = None) -> dict[str, Any]:
    return data
 logger = logging.getLogger(__name__)
 def _fail(
    msg: str,
    draw_img: np.ndarray,
@@ -383,12 +388,20 @@ def detect_holes(
    skip_center_gate: bool = False,
    depth_m: np.ndarray | None = None,
    intr: dict | None = None,
    allow_full_frame_roi: bool = False,
 ) -> dict[str, Any]:
    """
    二阶段检测螺纹孔中心。
-    detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[DetBox]
+    detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[Detection]
-              （YOLODetector / FasterRCNNDetector 均可）
+              （YOLODetector / FasterRCNNDetector / ONNX 均可）
    :param allow_full_frame_roi: 检测器**没给出工件框**时，是否退回"整幅画面当 ROI"。
        默认 False（保持原行为：没有框就不做，报「未检测到工件」）。
        置 True 的场景是**调用方明确要试一把**：知道检测器可能漏检，但孔本身
        在画面里已经看得见（例如工件检测模型与当前相机/画面不匹配、或工件贴着
        画面中心）。此时 ROI 的约束由孔自身的判据接手 —— 环半径、两孔间距、
        居中门控都还在，所以不会凭空多出"随便一个圆"。
    返回:
      status, msg, hole_points [[cx,cy],...], hole_radii [r1,r2], bbox, draw_img(BGR)
@@ -413,6 +426,14 @@ def detect_holes(
        # 1) 检测器定位工件 bbox（最多 1 个）— 兼容 YOLO / Faster R-CNN
        dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
        bbox = _parse_best_bbox(dets, conf)
        if bbox is None and allow_full_frame_roi:
            # 退回整幅画面：ROI 从"检测器给的框"换成"整幅"，孔自身的判据
            #（环半径区间、两孔间距、居中门控）继续把关。
            bbox = (0, 0, int(w - 1), int(h - 1))
            logger.info(
                "工件检测器没给框，按调用方要求用整幅画面当 ROI 继续找孔"
                "（允许的原因见 allow_full_frame_roi 的说明）"
            )
        if bbox is None:
            return _fail("未检测到工件", draw_img)
@@ -19,11 +19,56 @@ from typing import Optional
 import numpy as np
 import rclpy
 from geometry_msgs.msg import PointStamped
 from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 from std_msgs.msg import Bool, String
 from arm_application.stage3_search_node import Stage3Search
 #: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
 #:
 #: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
 #: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
 #: 小幅下降（只需 5cm 以内的行程）。
 #:
 #: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
 #: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
 #: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
 TRACK_CLEARANCE_M = 0.05
 def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
    """一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。
    这两种情况的排查方向完全相反，而现象长得一模一样（都是"没锁定"）：
    * **一个点都没收到** → 感知侧的事：yolo 后端多半缺 ultralytics/torch、
      truth 后端真值节点没起来、external 后端没人发。跟标定、跟工件在不在
      视野里**都没关系**。
    * **收到了点但没收敛** → 控制/几何侧的事：工件不在视野、手眼不对、
      目标超出工作空间、增益太小。
    以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
    因为缺依赖早退了"被读成"算法不收敛"，白查半天。
    """
    if points_seen <= 0:
        return (
            f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
            "（visual_servo/filtered_point 上 0 条）—— 这是感知没起来，"
            "不是标定/视野问题。按 perception.source 对号入座：\n"
            "        yolo     → 多半缺推理依赖（ultralytics/torch），"
            f"看感知进程退出前几行：{log_path}\n"
            "        truth    → 仿真真值节点没起来，或场景/物体名读不出来"
            "（它退出前会打印原因）\n"
            "        external → 外部节点没按这个话题/这个类型发"
        )
    return (
        f"一阶段 {timeout_s:.0f}s 没锁定（收到 {points_seen} 个目标点但没收敛）："
        f"工件不在视野 / 手眼不对 / 目标超出工作空间。看 {log_path}"
    )
 class AssemblyPipeline(Stage3Search):
    def __init__(self, node_name: str = "assembly_pipeline") -> None:
        # node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用
@@ -40,6 +85,28 @@ class AssemblyPipeline(Stage3Search):
        self._s1: Optional[subprocess.Popen] = None
        self._locked = False
        self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
        # 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
        # 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
        #
        # ⚠️ QoS 必须与感知发布端**逐项一致**（BEST_EFFORT + KEEP_LAST(5)，
        # 见 perception_node / sim_truth_perception）。用默认的 RELIABLE 订阅
        # BEST_EFFORT 的话题：不报错、话题列表里也在，但**一条都收不到** ——
        # 于是超时归因会一口咬定"感知没起来"，把人往完全错误的方向带
        #（真实原因可能只是这里 QoS 不匹配）。这类"诊断自己坏了"最难查，
        # 所以宁可多写这几行。
        self.declare_parameter("diag_point_topic", "visual_servo/filtered_point")
        self._diag_point_topic = str(self.get_parameter("diag_point_topic").value)
        self._point_msgs = 0
        self.create_subscription(
            PointStamped,
            self._diag_point_topic,
            self._on_point_seen,
            QoSProfile(
                history=HistoryPolicy.KEEP_LAST,
                depth=5,
                reliability=ReliabilityPolicy.BEST_EFFORT,
            ),
        )
        self._pipe_pub = self.create_publisher(String, "/pipeline/state", 10)
        self._pipe_result = self.create_publisher(String, "/pipeline/result", 10)
@@ -62,6 +129,10 @@ class AssemblyPipeline(Stage3Search):
        if bool(msg.data):
            self._locked = True
    def _on_point_seen(self, _msg: PointStamped) -> None:
        """只记"收到过目标点"，用于超时后的归因（见 stage1_timeout_reason）。"""
        self._point_msgs += 1
    def _kill_old_stage1(self) -> None:
        for pat in (
            "visual_servo.launch",
@@ -92,8 +163,8 @@ class AssemblyPipeline(Stage3Search):
        self._kill_old_stage1()
    def _track_z(self) -> float:
-        """跟踪高度 = 孔悬停高 + 5cm，通止规离开工件，二阶段只需小幅下降。"""
+        """跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
-        return float(self.hover_z) + 0.05
+        return float(self.hover_z) + TRACK_CLEARANCE_M
    def _ensure_clearance(self) -> None:
        """上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
@@ -125,12 +196,17 @@ class AssemblyPipeline(Stage3Search):
        log_path = "/tmp/pipeline_stage1.log"
        logf = open(log_path, "w", encoding="utf-8")
        lock_z = self._track_z()
        # use_sim_time 必须**原样传下去**：本节点在仿真里是 true，子 launch 若是
        # false，perception/servo 就跑在系统时间上，TF 全是仿真时刻的戳 →
        # "看得见图、算得出点，就是查不到变换"，一阶段永远不收敛。
        use_sim = bool(self.get_parameter("use_sim_time").value)
        cmd = [
            "ros2", "launch", "arm_application", "visual_servo.launch.py",
            "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(
            f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
@@ -139,6 +215,7 @@ class AssemblyPipeline(Stage3Search):
            cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True,
        )
        timeout = float(self.get_parameter("stage1_timeout_sec").value)
        self._point_msgs = 0
        t0 = time.monotonic()
        while time.monotonic() - t0 < timeout:
            self._spin(0.1)
@@ -150,7 +227,7 @@ class AssemblyPipeline(Stage3Search):
        else:
            self._stop_stage1()
            self.get_logger().error(
-                f"一阶段 {timeout:.0f}s 没锁定（视野里没工件？看 {log_path}）"
+                stage1_timeout_reason(self._point_msgs, timeout, log_path)
            )
            return 1
        self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
@@ -46,6 +46,7 @@ from arm_application.geom_ik import (
    quat_xyzw_to_R,
 )
 from arm_application.handeye_realsense import resolve_realsense_topics
 from arm_application.gauge_boot import perception_source_of
 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
@@ -153,6 +154,9 @@ class Stage2Locate(Node):
        self.declare_parameter("hover_z_m", -1.0)      # 大于 0 → 覆盖 site 里的值
        self.declare_parameter("preview", True)
        self.declare_parameter("action_name", "rizon_arm_controller/follow_joint_trajectory")
        # 真值孔位（perception.source=truth）要把 scene.yaml 的物体位姿投影成像，
        # 场景位姿在 Gazebo 的 world 系里，所以要有世界系的名字。
        self.declare_parameter("world_frame", "world")
        # ---------------- site.yaml 解析（和脚本同一套） ----------------
        site = load_site_dict()
@@ -194,6 +198,14 @@ class Stage2Locate(Node):
            servo.get("hover_offset_ee_xy_m") or [0.0, 0.0], dtype=float
        ).reshape(2)
        # ---- 感知来源：本节点的孔位是"检测出来的"还是"场景真值算出来的" ----
        # 与 gauge_assembly_node 用**同一个解析函数**（gauge_boot.perception_source_of）：
        # 两处各自读一遍 site，迟早会出现"流程以为在用真值、检测节点还在跑模型"。
        perception = deep_merge(site.get("perception") or {}, robot.get("perception") or {})
        self.perception_source = perception_source_of(perception)
        self._truth_object = str((perception.get("truth") or {}).get("object") or "")
        self._truth_warn_t = 0.0
        self.target_hole = (
            str(self.get_parameter("target_hole").value or "").strip().lower()
            or str(stage2.get("target_hole") or "k1").strip().lower()
@@ -854,8 +866,77 @@ class Stage2Locate(Node):
        if fb is not None:
            yield fb
    def _truth_enabled(self) -> bool:
        """本节点该不该走"真值孔位"。
        只看 ``perception.source``（site.yaml 的 robots[].perception.source 也会
        合进来）：``truth`` = 仿真真值；``yolo``/``external`` = 老老实实跑检测器。
        真机上永远是后者 —— 这条分支在真机不可达。
        """
        return str(getattr(self, "perception_source", "yolo")) == "truth"
    def _truth_hole_result(self, bgr: np.ndarray, depth_m=None, intr=None) -> Optional[dict]:
        """真值孔位（像素 + ROI 框）。拿不到就返回 status=False 的结果并说明原因。
        为什么失败也**不**退回检测器：既然明确配了 ``perception.source=truth``，
        说明这台机器上"检测器给不出可信结果"（仿真里合成图过不了真机模型）。
        退回检测器的后果是**用整幅画面当 ROI**，于是 HoughCircles 在夹爪/通规/
        台面上找出一对"假孔"（实测间距 108mm，真值 58mm），再被门槛丢掉 ——
        现象是"每拍都丢、最后超时"，比直接说"TF 没到"难查得多。
        """
        try:
            from arm_sim_gazebo import truth_holes
        except ImportError:
            # 真机上没有仿真包，perception.source 也不会是 truth —— 到这里只可能是
            # 有人把 source 配成 truth 却没装仿真包，直接报清楚。
            return None
        T = self._lookup(self.cam_frame, str(self.get_parameter("world_frame").value or "world"))
        if intr is None and self._info is not None:
            k = self._info.k
            intr = {"fx": float(k[0]), "fy": float(k[4]), "cx": float(k[2]), "cy": float(k[5])}
        i3 = None
        if intr is not None:
            i3 = (intr.get("fx"), intr.get("fy"), intr.get("cx"), intr.get("cy"))
        res = truth_holes.locate_holes(
            T, i3, (int(bgr.shape[0]), int(bgr.shape[1])),
            object_name=self._truth_object, root=None,
        )
        if not res.ok:
            now = time.monotonic()
            if now - self._truth_warn_t > 5.0:
                self._truth_warn_t = now
                self.get_logger().warn(f"真值孔位不可用：{res.why}")
            return {
                "status": False,
                "msg": f"真值孔位不可用：{res.why}",
                "hole_points": [],
                "hole_radii": [],
                "bbox": None,
                "draw_img": truth_holes.draw(bgr, res),
            }
        return {
            "status": True,
            "msg": f"真值孔位（{res.note}）",
            "hole_points": [[float(p[0]), float(p[1])] for p in res.hole_points],
            "hole_radii": [float(r) for r in res.hole_radii],
            "bbox": [int(v) for v in res.bbox],
            "label": "workpiece(truth)",
            "draw_img": truth_holes.draw(bgr, res),
        }
    def _detect_holes_img(self, bgr: np.ndarray, *, skip_center_gate: bool,
                          depth_m: np.ndarray = None, intr: dict = None) -> dict:
        """跑一次孔检测。第二次调用（``skip_center_gate=True``）允许整幅画面当 ROI。
        为什么第二次要放宽到整幅：走到那一步时调用方的处境是"**没找到工件框**，
        但可能已经看得见孔了"。不放宽的话会停在"未检测到工件"上 —— 而这恰恰是
        "模型没出框、孔明明在画面里"的情形（换相机/换画面域、或模型漏检时会出现）。
        放宽不等于乱认：环半径区间、两孔间距、居中门控这些判据都还在。
        """
        if self._truth_enabled():
            res = self._truth_hole_result(bgr, depth_m=depth_m, intr=intr)
            if res is not None:
                return res
        last = {}
        for det in self._iter_dets():
            res = detect_holes(
@@ -868,7 +949,15 @@ class Stage2Locate(Node):
            if (not res.get("status")) and res.get("gate_block") and not skip_center_gate:
                res = detect_holes(
                    bgr, det, config=self._hole_cfg, skip_center_gate=True,
-                    depth_m=depth_m, intr=intr,
+                    depth_m=depth_m, intr=intr, allow_full_frame_roi=True,
                )
                last = res
                if res.get("status"):
                    return res
            elif (not res.get("status")) and skip_center_gate:
                res = detect_holes(
                    bgr, det, config=self._hole_cfg, skip_center_gate=True,
                    depth_m=depth_m, intr=intr, allow_full_frame_roi=True,
                )
                last = res
                if res.get("status"):
@@ -876,6 +965,14 @@ class Stage2Locate(Node):
        return last
    def _best_bbox(self, bgr: np.ndarray, conf: Optional[float] = None) -> Optional[Tuple[int, int, int, int]]:
        if self._truth_enabled():
            # 真值模式：工件框由场景几何投出来。**必须走这条路**，不能落到检测器上：
            # 检测器在仿真里出不了框，S05 的"粗居中"就会拿整幅画面当工件，
            # 于是把"画面中心 ≠ 工件中心"算成 58.8mm 偏差，白白横移。
            res = self._truth_hole_result(bgr)
            if res is not None and res.get("status") and res.get("bbox"):
                return tuple(int(v) for v in res["bbox"])  # type: ignore[return-value]
            return None
        thr = float(conf if conf is not None else min(0.12, float(self._hole_cfg.get("conf", 0.25))))
        min_area = 4000.0
        best = None
@@ -33,7 +33,7 @@ import json
 import math
 import time
 from pathlib import Path
-from typing import Any, Dict, List, Optional, Tuple
+from typing import Any, Dict, List, Optional, Sequence, Tuple
 import numpy as np
 import rclpy
@@ -58,6 +58,52 @@ from trajectory_msgs.msg import JointTrajectory
 # ==================== 纯逻辑（可离线单测） ====================
 #: 触觉帧"够新"的窗口 [s]：比它更久没更新就认为这一侧没数据。
 TACTILE_MAX_AGE_S = 0.4
 def live_tactile(
    now: float, pairs: Sequence[Any], max_age_s: float = TACTILE_MAX_AGE_S
 ) -> List[Any]:
    """挑出这一轮**可用**的触觉帧。``pairs`` 是每侧一个 ``(收到时刻, msg)``。
    判据只有两条：**够新** + ``connected=true``。
    ⚠️ 不要拿力的**幅值**当"传感器活着"的代理（曾经写的是 ``|fnormal| > 1.0``）：
    静止时读数本来就是 0 —— 空手、还没接触、或者读数本身就是形变任意单位
    （Orisys 的量纲）—— 于是 S07 一进柔顺下压就报"触觉断流（左右指都无新鲜
    数据）"，而话题其实以 19Hz 在发、``connected`` 也是 true。
    上下游的契约在 ``sim_tactile_adapter`` 里写得很清楚：
    "没数据"发零 **且** ``connected=false``；"零力"是 ``connected=true``。
    """
    out: List[Any] = []
    for pair in pairs or ():
        if pair is None:
            continue
        t, msg = pair
        if now - float(t) < max_age_s and bool(getattr(msg, "connected", False)):
            out.append(msg)
    return out
 def tactile_timeout_reason(pairs: Sequence[Any], timeout_s: float) -> str:
    """触觉等超时了 → 给一句**照着就能修**的话。
    两种"没数据"现象一样（都是等不到可用帧），修法却完全不同：
      · 有帧到、但 ``connected=false`` → 驱动在跑，设备/SDK 那边的问题；
      · 一帧都没到 / 帧全过期       → 话题或驱动根本没在发。
    以前共用一句"断流（左右指都无新鲜数据）"，于是"设备没连上"被读成
    "话题没起来"，查错方向。
    """
    seen = [p for p in (pairs or ()) if p is not None]
    if seen and not any(bool(getattr(m, "connected", False)) for _t, m in seen):
        return (
            f"触觉帧有到但 connected=false（驱动在跑、设备没连上；已等 {timeout_s:.1f}s）"
            f"——看 /tmp/stage3_tactile.log，或先跑 "
            f"bash ~/ros2/scripts/run_tactile.sh --cli 验证"
        )
    return f"触觉话题断流 >{timeout_s:.1f}s（左右指都无新鲜数据）"
 def spiral_points(
    cx: float, cy: float, pitch_mm: float, step_mm: float, r_max_mm: float
 ) -> List[Tuple[float, float]]:
@@ -230,32 +276,33 @@ class Stage3Search(Stage2Locate):
    def _tac_fresh(self, timeout_s: float = 1.0) -> Tuple[float, float, float, bool]:
        """等新鲜触觉帧 → (fn_avg, fn_max, shear_max, contact_any)。
-        单侧可用即可（另一指掉线/未插时用健在侧继续，阈值相对同侧基线仍自洽）。"""
+        单侧可用即可（另一指掉线/未插时用健在侧继续，阈值相对同侧基线仍自洽）。
        "可用"= **帧到了（0.4s 内）且 connected=true**，与幅值无关。
        曾经这里写的是 ``|fnormal| > 1.0``（拿力的大小当"传感器活着"的代理），
        那条判据在**静止读数≈0** 时必然误判：空手、还没接触、或者读数本身就是
        形变任意单位（Orisys 的量纲）—— 于是 S07 一进柔顺下压就报
        "触觉断流 >3.0s（左右指都无新鲜数据）"，而话题其实以 19Hz 在发、
        ``connected`` 也是 true。仿真适配器早就把这两个概念分开了
        （没数据时发零 + ``connected=false``，见 sim_tactile_adapter），
        下游必须按同一条契约判"有没有数据"。
        """
        t0 = time.monotonic()
        while time.monotonic() - t0 < timeout_s:
            self._spin(0.02)
-            now = time.monotonic()
+            live = live_tactile(time.monotonic(), (self._tac_l, self._tac_r))
            live = []
            for tac in (self._tac_l, self._tac_r):
                if tac is None:
                    continue
                t, m = tac
                if now - t < 0.4:
                    live.append(m)
            if live:
-                live_ok = [m for m in live if abs(float(m.fnormal)) > 1.0]
+                fns = [float(m.fnormal) for m in live]
                if not live_ok:
                    continue
                fns = [float(m.fnormal) for m in live_ok]
                shear = max(
-                    math.hypot(float(m.fshearx), float(m.fsheary)) for m in live_ok
+                    math.hypot(float(m.fshearx), float(m.fsheary)) for m in live
                )
                return (
                    sum(fns) / len(fns), max(fns), shear,
-                    any(bool(m.contact) for m in live_ok),
+                    any(bool(m.contact) for m in live),
                )
        # 超时了 —— 归因写在纯函数里（两条判据、两种修法，能被测试钉住）
        raise SearchAbort(
-            f"触觉话题断流 >{timeout_s:.1f}s（左右指都无新鲜数据）", code=7
+            tactile_timeout_reason((self._tac_l, self._tac_r), timeout_s), code=7
        )
    def _ft_mag(self) -> Optional[float]:
@@ -92,12 +92,20 @@ def _launch_setup(context, *args, **kwargs):
    dry_run = _flag("dry_run")
    holes = LaunchConfiguration("holes_total").perform(context).strip()
    # 仿真里必须 true：本节点所有 header.stamp 比对和 TF 查询都按这个时钟走。
    # 忘了开的话症状是"相机没数据/TF 查不到"，看不出真正原因（脚本 --sim 会带）。
    use_sim_time = _flag("use_sim_time")
    backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto"
    result_file = LaunchConfiguration("result_file").perform(context).strip()
    # 是否由本 launch 发手眼静态 TF。真机必须发（相机系与法兰之间除了标定值
    # 没有别的来源）；仿真**不能**发 —— 仿真里相机在 URDF 里就挂在法兰上，
    # 再发一条同父子关系的静态 TF 会出现两个父节点（TF_MULTIPLE_AUTHORITIES），
    # 查出来的位姿在两条之间跳。脚本 --sim 会传 false。
    publish_handeye_tf = _flag("publish_handeye_tf")
    nodes = []
    frames = []
-    if os.path.isfile(handeye):
+    if publish_handeye_tf and os.path.isfile(handeye):
        try:
            tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
        except Exception as exc:  # noqa: BLE001
@@ -118,6 +126,11 @@ def _launch_setup(context, *args, **kwargs):
                    output="screen",
                )
            )
    elif not publish_handeye_tf:
        frames.append(
            f"[gauge] 按参数不发手眼静态 TF（publish_handeye_tf=false）："
            f"相机系 {cam_frame} 由 URDF/驱动给出（仿真就是这条路径）"
        )
    else:
        frames.append(
            f"[gauge] 没有手眼文件（{handeye or 'paths.handeye_file 为空'}）"
@@ -138,6 +151,7 @@ def _launch_setup(context, *args, **kwargs):
                    "holes_total": int(holes) if holes else -1,
                "gripper_backend": backend,
                "result_file": result_file,
                "use_sim_time": use_sim_time,
            }
        ],
    )
@@ -147,7 +161,8 @@ def _launch_setup(context, *args, **kwargs):
        LogInfo(msg=f"[gauge] site={path}"),
        LogInfo(msg=f"[gauge] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
        LogInfo(
-            msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} backend={backend}"
+            msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} "
            f"backend={backend} use_sim_time={use_sim_time}"
        ),
        LogInfo(msg=f"[gauge] 结论文件 {result_file}"),
        *[LogInfo(msg=m) for m in frames],
@@ -170,6 +185,10 @@ def generate_launch_description() -> LaunchDescription:
            DeclareLaunchArgument("gripper_backend", default_value="auto"),
            # 结论落盘路径（脚本读它判成败：ros2 launch 的退出码恒为 0）
            DeclareLaunchArgument("result_file", default_value="/tmp/gauge_assembly_result.json"),
            # 仿真必须 true（真机 false）。理由见 _launch_setup 里的注释。
            DeclareLaunchArgument("use_sim_time", default_value="false"),
            # 真机 true（相机系只能由标定值给出）；仿真 false（URDF 已给出）。
            DeclareLaunchArgument("publish_handeye_tf", default_value="true"),
            OpaqueFunction(function=_launch_setup),
        ]
    )
@@ -12,7 +12,15 @@ from pathlib import Path
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.actions import (
    DeclareLaunchArgument,
    EmitEvent,
    LogInfo,
    OpaqueFunction,
    RegisterEventHandler,
 )
 from launch.event_handlers import OnProcessExit
 from launch.events import Shutdown
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -153,20 +161,63 @@ def _launch_setup(context, *args, **kwargs):
        except ValueError:
            pass
-    perception = Node(
+    # ------------------------------------------------------------------
-        package="arm_application",
+    # 感知来源：谁来产生"视觉测量"（site.yaml 的 perception.source）
-        executable="perception_node",
+    #
-        name="perception_node",
+    #   yolo      本体 perception_node（要 torch/ultralytics）
-        output="screen",
+    #   truth     仿真真值节点（按场景已知位姿直接算目标点，不跑推理）
-        parameters=[detect_yaml, perc_params],
+    #   external  外部节点按同一话题提供，本 launch 不起感知进程
    #
    # 为什么要有这个开关：仿真环境（ros_gz_sim conda）没有 torch/ultralytics
    # 且离线装不上，perception_node 一启动就死；而仿真里工件位姿本来就是
    # 已知的。切到 truth 后，流程能在仿真里验"控制与接口"那一半。
    # 值写错**直接报错**，不要默默当 yolo —— 那会退回到"一阶段永远不锁定"。
    # ------------------------------------------------------------------
    # 允许值与校验只有一处实现（gauge_boot.perception_source_of），S00 自检用的
    # 也是它 —— 两边各写一份的话，会出现"起了真值节点、S00 却去查 ultralytics"。
    from arm_application.gauge_boot import perception_source_of
    perc_src = perception_source_of(site.get("perception"))
    # 仿真时间必须**一路传下去**：这个 launch 由装配流程（pipeline_node）在
    # S04 里以子进程拉起，父节点在仿真里带 use_sim_time=true，这里若漏传，
    # perception/servo 会跑在系统时间上 —— TF 全是仿真时刻的戳，于是
    # "看得见图、算得出点，就是查不到变换"，表现为一阶段永远不收敛。
    use_sim = LaunchConfiguration("use_sim_time").perform(context).lower() in (
        "1", "true", "yes",
    )
    perception = None
    if perc_src == "yolo":
        perception = Node(
            package="arm_application",
            executable="perception_node",
            name="perception_node",
            output="screen",
            parameters=[detect_yaml, perc_params, {"use_sim_time": use_sim}],
        )
    elif perc_src == "truth":
        truth_cfg = (site.get("perception") or {}).get("truth") or {}
        perception = Node(
            package="arm_sim_gazebo",
            executable="sim_truth_perception",
            name="sim_truth_perception",
            output="screen",
            parameters=[
                {
                    "camera_frame": cam_frame,
                    "target_object": str(truth_cfg.get("object") or ""),
                    "offset_m": [float(v) for v in (truth_cfg.get("offset_m") or [0.0, 0.0, 0.0])],
                    "use_sim_time": use_sim,
                }
            ],
        )
    servo = Node(
        package="arm_application",
        executable="servo_node",
        name="servo_node",
        output="screen",
-        parameters=[servo_yaml, servo_params],
+        parameters=[servo_yaml, servo_params, {"use_sim_time": use_sim}],
    )
    weights = perc_params.get("weights", "")
@@ -176,12 +227,31 @@ def _launch_setup(context, *args, **kwargs):
            msg=f"[arm] {robot.get('label') or robot.get('sn')}  "
            f"handeye={handeye}"
        ),
-        LogInfo(msg=f"[site] YOLO weights: {weights}"),
+        LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"),
        LogInfo(
            msg="节点：perception_node（YOLO）| servo_node（100Hz）—— "
            "各自独立进程，话题 /visual_servo/filtered_point"
        ),
    ]
    if perc_src == "yolo":
        nodes.append(LogInfo(msg=f"[site] YOLO weights: {weights}"))
        nodes.append(
            LogInfo(
                msg="节点：perception_node（YOLO）| servo_node（100Hz）—— "
                "各自独立进程，话题 /visual_servo/filtered_point"
            )
        )
    elif perc_src == "truth":
        nodes.append(
            LogInfo(
                msg="节点：sim_truth_perception（仿真真值，不跑推理）| servo_node（100Hz）\n"
                "      真值只验**控制与接口**：像素噪声/遮挡/算法错判它都没有。"
                "能不能在真机上看到孔，要看真机那一次（见 devices/sim/gazebo/README.md）"
            )
        )
    else:
        nodes.append(
            LogInfo(
                msg="节点：servo_node（100Hz）。感知由**外部**节点按 "
                "visual_servo/filtered_point 提供（本 launch 不起感知进程）"
            )
        )
    pub_tf = LaunchConfiguration("publish_handeye_tf").perform(context).lower() in (
        "1", "true", "yes",
    )
@@ -190,7 +260,31 @@ def _launch_setup(context, *args, **kwargs):
            LogInfo(msg=f"静态手眼 {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]")
        )
        nodes.append(static_tf)
-    nodes.extend([perception, servo])
+    if perception is not None:
        nodes.append(perception)
    nodes.append(servo)
    # 感知进程一死就整段退出（truth 后端同样适用 —— 它可能因为配置/场景读
    # 不出来而退出，那时 servo 还活着，调用方只能等满 stage1_timeout_sec）。
    # 为什么必须显式写：yolo 后端是最需要 GPU/torch 的那个（缺 ultralytics/torch
    # 时**启动即死**，而 servo_node 还活着继续跑）。没有这个 handler，launch 不会
    # 结束，调用方（装配流程的 S04）只能等满 stage1_timeout_sec 才报"没锁定"——
    # 现场看到的是"算法不收敛"，真正原因是感知进程早就不在了。
    if perception is not None:
        nodes.append(
            RegisterEventHandler(
                OnProcessExit(
                    target_action=perception,
                    on_exit=[
                        LogInfo(
                            msg=f"[visual_servo] ✘ 感知进程（{perc_src}）退出了 —— 停止本段（"
                            "看它退出前最后几行：yolo 多为缺推理依赖，"
                            "truth 多为场景/参数读不出来）"
                        ),
                        EmitEvent(event=Shutdown(reason="perception exited")),
                    ],
                )
            )
        )
    return nodes
@@ -220,6 +314,15 @@ def generate_launch_description() -> LaunchDescription:
                "publish_handeye_tf",
                default_value="true",
            ),
            # 仿真时间开关。**仿真里必须传 true**：/clock 由 Gazebo 发，
            # 节点若跑在系统时间上，TF/图像时间戳全对不上（症状是"看得见图、
            # 就是查不到变换，一阶段永远不锁定"）。
            # 装配流程在仿真里跑时由 pipeline_node 自动传 true（见
            # pipeline_node._run_stage1），手工跑时自己加上。
            DeclareLaunchArgument(
                "use_sim_time",
                default_value="false",
            ),
            DeclareLaunchArgument(
                "ee_z_m",
                default_value="",
@@ -14,12 +14,14 @@ from __future__ import annotations
 import pytest
 from arm_application.gauge_boot import (
    PERCEPTION_SOURCES,
    PROBES,
    BootCheck,
    blocking_failures,
    build_checks,
    format_report,
    optional_warnings,
    perception_source_of,
    summary_counts,
 )
@@ -232,3 +234,64 @@ def test_bootcheck_defaults_to_required():
    with pytest.raises(TypeError):
        BootCheck(key="k", label="l", probe="service", target="/x")  # type: ignore[call-arg]
 def test_missing_modules_detects_a_module_that_cannot_exist():
    """依赖检查要能真的报缺（否则 detector 自检又会变成假阳性）。"""
    from arm_application.gauge_boot import missing_modules
    assert missing_modules(["json"]) == []
    assert missing_modules(["__it_cannot_exist__"]) == ["__it_cannot_exist__"]
    # 父包不存在时 find_spec 会抛，这同样算"缺"
    assert missing_modules(["__no_such_pkg__.sub"]) == ["__no_such_pkg__.sub"]
 def test_dependency_advice_offers_a_way_out_without_network():
    """提示必须给两条路：装它、或换掉这个后端。
    只写"请 pip install"在离线机器上等于没写（本机仿真环境实测 pip 直接 SSL 失败）。
    """
    from arm_application.gauge_boot import dependency_advice
    msg = dependency_advice(["ultralytics"])
    assert "ultralytics" in msg
    assert "pip install" in msg
    assert "真值" in msg or "外部" in msg
 # =============================================================================
 # 感知来源（perception.source）
 # =============================================================================
 def test_detector_check_only_exists_for_the_yolo_backend():
    """只有 yolo 才查推理预热；truth/external 不跑推理，查了必然是假阴性。
    反面做法（生成一个"永远 OK"的项）更糟：它会让人以为"感知也验过了"。
    """
    yolo = _by_key(_build(weights_path="/tmp/w.onnx", perception_source="yolo"))
    assert "detector" in yolo
    for src in ("truth", "external"):
        checks = _by_key(_build(weights_path="/tmp/w.onnx", perception_source=src))
        assert "detector" not in checks, f"{src} 后端不该查推理依赖"
 def test_no_weights_path_still_means_no_detector_check():
    """没给权重路径（比如没配检测）就不生成这一项 —— 老行为不能丢。"""
    assert "detector" not in _by_key(_build(weights_path=""))
 def test_perception_source_defaults_to_yolo_when_unset():
    assert perception_source_of(None) == "yolo"
    assert perception_source_of({}) == "yolo"
    assert perception_source_of({"source": ""}) == "yolo"
    assert perception_source_of({"source": " TRUTH "}) == "truth"
 def test_perception_source_rejects_typo_with_the_allowed_values():
    """拼错必须报错：默默当 yolo 会让人去查推理依赖，而真因是键值写错。"""
    with pytest.raises(ValueError) as e:
        perception_source_of({"source": "yolo2"})
    msg = str(e.value)
    assert "yolo2" in msg
    for s in PERCEPTION_SOURCES:
        assert s in msg
    assert "site.yaml" in msg
@@ -10,6 +10,8 @@
 from __future__ import annotations
 from pathlib import Path
 import pytest
 from arm_application.gauge_config import (
@@ -21,9 +23,11 @@ from arm_application.gauge_config import (
    _UNITS,
    dataclass_keys,
    from_site,
    snap_feature_of,
    missing_entries,
    unit_violations,
 )
 from arm_application.gauge_fsm import descend_iter_budget, descend_tol_m
 # =============================================================================
@@ -186,3 +190,78 @@ def test_describe_prints_effective_values_and_marks_calibration_needed():
    assert "现场标定" in text
    # 稳定判据的口径要写出来，避免下次有人改成"相邻帧之差"
    assert "到均值的最远距离" in text
 # =============================================================================
 # S05 稳定判据的特征来源
 # =============================================================================
 def test_snap_feature_auto_follows_perception_source():
    """auto：真机（yolo）用检测框，仿真真值/外部感知用法兰位移。
    为什么必须分开：检测框那一路上游是工件模型，仿真合成图过不了真机训练出来的
    模型（实测最高分 0.017）——选错的话 S05 会永远"不稳定"地空转到超时，
    而日志看起来像"机械臂一直在晃"。
    """
    assert snap_feature_of("auto", "yolo") == "bbox"
    assert snap_feature_of("auto", "truth") == "motion"
    assert snap_feature_of("auto", "external") == "motion"
 def test_snap_feature_explicit_override_wins():
    assert snap_feature_of("bbox", "truth") == "bbox"
    assert snap_feature_of("motion", "yolo") == "motion"
 def test_snap_feature_rejects_unknown_value():
    with pytest.raises(GaugeConfigError) as e:
        from_site({"gauge_assembly": {"snap_stable_feature": "detect"}})
    assert "snap_stable_feature" in str(e.value)
 def test_snap_tolerance_must_be_positive():
    with pytest.raises(GaugeConfigError):
        from_site({"gauge_assembly": {"snap_tol_mm": 0.0}})
 def test_snap_keys_are_declared_with_units():
    """新键必须登记单位/类型，否则"单位自检"对它们就是空话。"""
    assert _UNITS["snap_tol_mm"] == "mm"
    assert _UNITS["snap_stable_feature"] == ""
    assert "snap_stable_feature" in _UNITLESS
    assert _TYPES["snap_tol_mm"] is float
    assert _TYPES["snap_stable_feature"] is str
 # =============================================================================
 # 出厂配置的 S07：步长与到位容差不能相等
 # =============================================================================
 def test_shipped_site_yaml_s07_step_cannot_deadlock():
    """**回归锚点**（2026-09-24 在仿真里踩到，真机同参数同结果）。
    ``contact_step_mm: 0.5`` 配上一个写死的 0.5mm 到位容差时，S07 的"往下走
    一步"会被 ``_goto`` 判成"已在容差内"→ 直接返回、一动不动；循环条件
    （实测高度还没到最深）恒真 → 静默自旋：日志停在基线那行、CPU 跑满、
    不报错也不前进。
    容差现在由 ``gauge_fsm.descend_tol_m`` 从步长推出来。这条测试盯住
    **出厂 site.yaml 的真实取值**（不是默认值字典），因为现场改的正是它：
    只要容差 ≥ 步长，这里必须报红。
    """
    import yaml
    root = Path(__file__).resolve().parents[3]
    doc = yaml.safe_load((root / "config" / "site.yaml").read_text(encoding="utf-8"))
    sec = doc["gauge_assembly"]
    cfg = from_site(doc)
    step_m = float(sec["contact_step_mm"]) / 1000.0
    assert step_m > 0.0
    assert descend_tol_m(step_m) < step_m, (
        f"出厂配置 S07 步长 {sec['contact_step_mm']}mm 的到位容差 ≥ 步长："
        "臂会一步都不走（静默自旋）"
    )
    # 步数上限要够走完整个下探深度，否则"没接触"会被误报成"步数用尽"
    depth_m = float(sec["contact_max_depth_mm"]) / 1000.0
    assert descend_iter_budget(depth_m, step_m) >= depth_m / step_m
    # 顺带钉住"出厂配置的派生值印得出来"（describe 里会打印容差）
    assert "S07" in "\n".join(cfg.describe())
@@ -23,8 +23,12 @@ from arm_application.gauge_fsm import (
    GaugeContext,
    GaugeFsm,
    State,
    descend_iter_budget,
    descend_progress,
    descend_tol_m,
    is_joint_limit_error,
    pixel_stable,
    points_stable,
    ratchet_cycles,
    resolve_next,
    screw_stop,
@@ -386,3 +390,86 @@ def test_math_hypot_precision_is_not_used_loosely():
    assert math.isclose(2.0, 2.0)
    assert pixel_stable(samples, tol_px=2.0, min_frames=3)
    assert not pixel_stable(samples, tol_px=1.9, min_frames=3)
 def test_points_stable_works_in_three_dimensions():
    """S05 在仿真里改用**法兰位移**（三维、毫米）判稳定，判据必须支持三维。
    同一套"到均值最远距离"的判据：换成毫米只是量纲变了，逻辑不该重写一份。
    """
    still = [(1.0, 2.0, 3.05) for _ in range(6)]
    assert points_stable(still, tol=0.4, min_frames=6)
    # 零均值抖动：相邻帧很近，整体在 ±5mm 之间摆 —— 必须判为不稳
    sawtooth = [(0.0, 0.0, 0.0), (0.0, 0.0, 0.0), (5.0, 0.0, 0.0),
                (5.0, 0.0, 0.0), (-5.0, 0.0, 0.0), (-5.0, 0.0, 0.0)]
    assert not points_stable(sawtooth, tol=0.4, min_frames=6)
 def test_points_stable_rejects_ragged_samples():
    """样本维数不一致时必须判不稳（否则会按第一个样本的维数静默比较）。
    真实来源是"两种特征混进同一个窗口"：比如特征从 bbox 切成 motion 时，
    残留的二维样本会被当成三维来算 —— 与其猜，不如直接判不稳重采。
    """
    assert not points_stable([(1.0, 2.0), (1.0, 2.0, 3.0)] * 3, tol=1.0, min_frames=6)
 def test_points_stable_needs_enough_frames():
    assert not points_stable([(0.0, 0.0, 0.0)] * 5, tol=10.0, min_frames=6)
    assert points_stable([(0.0, 0.0, 0.0)] * 6, tol=10.0, min_frames=6)
 # =============================================================================
 # S07 步进下压：容差 < 步长（否则 _goto 空转，一步都不走）
 # =============================================================================
@pytest.mark.parametrize("step_mm", [0.05, 0.1, 0.5, 1.0, 2.0, 4.0])
 def test_descend_tol_is_strictly_smaller_than_the_step(step_mm):
    """**回归测试**（2026-09-24 仿真踩到过）：
    原实现把 ``_goto`` 的到位容差写死 0.5mm，而 ``contact_step_mm`` 也是 0.5mm。
    ``_goto`` 的第一件事就是"已落在容差内 → 直接返回、不动"，于是 S07
    每次"往下走一步"都变成原地不动：循环条件恒真、日志停在基线那行、
    CPU 跑满、不报错也不前进 —— 典型的静默自旋（规则 R5.1）。
    纪律：单步下压的容差必须**严格小于**步长，这是循环能推进的前提，
    不是调参风格的偏好。
    """
    step_m = step_mm / 1000.0
    assert descend_tol_m(step_m) < step_m
 def test_descend_tol_has_a_floor_for_tiny_steps():
    """步长极小时容差不能退化成 0：伺服追不到 0，会永远"差一点点"。"""
    tol = descend_tol_m(0.00001)  # 10µm 的步长（比任何现实步长都小）
    assert 0.0 < tol <= 0.00001 * 0.5
 def test_descend_tol_rejects_nonpositive_step():
    with pytest.raises(ValueError):
        descend_tol_m(0.0)
    with pytest.raises(ValueError):
        descend_tol_m(-0.0005)
 def test_descend_iter_budget_covers_the_whole_depth():
    """步数上限必须够走完整个下探深度（否则会在"还没到底"时被上限截断）。"""
    step_m, depth_m = 0.0005, 0.025
    assert descend_iter_budget(depth_m, step_m) >= math.ceil(depth_m / step_m)
    # 但也不能是无限：这正是"臂没动时不要把节点转成电暖器"的那道闸
    assert descend_iter_budget(depth_m, step_m) < 200
 def test_descend_iter_budget_zero_depth_means_no_iterations():
    assert descend_iter_budget(0.0, 0.0005) == 0
    with pytest.raises(ValueError):
        descend_iter_budget(0.025, 0.0)
 def test_descend_progress_only_counts_real_downward_motion():
    """推进的证据是**实测位移**：指令发出去不等于臂动了。"""
    step_m = 0.0005
    assert descend_progress(0.6316, 0.6311, step_m)        # 走满一步
    assert descend_progress(0.6316, 0.63146, step_m)       # 只走了 0.14mm 也算动了
    assert not descend_progress(0.6316, 0.63159, step_m)   # 只走了 0.01mm：被死区吃掉
    assert not descend_progress(0.6316, 0.6316, step_m)    # 完全没动（空转）
    assert not descend_progress(0.6316, 0.6317, step_m)    # 往上跑 ≠ 推进
@@ -84,3 +84,56 @@ def test_default_model_path_is_canonical_name():
 def test_default_model_path_is_absolute():
    """必须是绝对路径：相对路径会随 CWD 漂移，节点从不同目录启动就找不到。"""
    assert _default_model_path().is_absolute()
 # ---------------------------------------------------------------------------
 # detect_holes：没有工件框时，调用方可以要求"整幅画面当 ROI"
 # ---------------------------------------------------------------------------
 class _NoDetector:
    """一个什么都检测不到的"检测器"（模型漏检 / 与画面域不匹配时的替身）。"""
    def detect(self, *_a, **_k):
        return []
 def test_detect_holes_needs_a_bbox_unless_asked_otherwise():
    """默认行为不变：没有工件框就直接报「未检测到工件」。
    这条是给真机路径兜底的：默认不能被"整幅画面当 ROI"悄悄改掉 —— ROI 一放宽，
    画面里别的圆（工装、夹爪上的特征）都有可能被当成孔。
    """
    import cv2
    import numpy as np
    from arm_application.hole_detect_hough import detect_holes, load_config
    blank = np.full((200, 200, 3), 120, np.uint8)
    res = detect_holes(
        blank, _NoDetector(), config=load_config(), skip_center_gate=True
    )
    assert res["status"] is False
    assert res["msg"] == "未检测到工件"
 def test_detect_holes_full_frame_roi_gets_past_the_bbox_gate():
    """``allow_full_frame_roi=True`` 时，没框也要继续往下走（走到找孔那一步）。
    判据用"失败原因变了"：从「未检测到工件」（卡在 ROI）变成「HoughCircles
    未检测到圆孔」（已经真的找过孔了）。用空白图是有意的 —— 它保证结论只
    来自"是否跳过了 ROI 要求"，不掺杂检测器本身好不好用。
    """
    import cv2
    import numpy as np
    from arm_application.hole_detect_hough import detect_holes, load_config
    blank = np.full((200, 200, 3), 120, np.uint8)
    res = detect_holes(
        blank,
        _NoDetector(),
        config=load_config(),
        skip_center_gate=True,
        allow_full_frame_roi=True,
    )
    assert res["status"] is False
    assert res["msg"] != "未检测到工件"
@@ -0,0 +1,42 @@
 """一阶段超时的**归因**：是"感知没给点"还是"给了点没收敛"。
 两者现象完全一样（都是"没锁定"），排查方向却相反：
 * 一个点都没收到 → 感知侧（缺依赖 / 真值节点没起 / 没人发）；
 * 收到了点没收敛 → 控制与几何侧（工件不在视野、手眼、工作空间）。
 以前共用一句"没锁定（视野里没工件？）"，于是"perception_node 缺
 ultralytics 早退了"被读成"算法不收敛"，白查半天。这里把这句话钉住。
 """
 from __future__ import annotations
 import pytest
 from arm_application.pipeline_node import stage1_timeout_reason
 def test_zero_points_blames_perception_not_calibration():
    msg = stage1_timeout_reason(0, 150.0, "/tmp/pipeline_stage1.log")
    assert "一个视觉目标点都没收到" in msg
    # 必须逐后端给出对号入座的查法，否则人还是不知道下一步做什么
    assert "yolo" in msg and "truth" in msg and "external" in msg
    assert "ultralytics" in msg          # yolo 后端最常见的原因
    assert "perception.source" in msg    # 指明这个开关怎么用
    # 不能把人往"标定/视野"上带
    assert "不是标定" in msg
 def test_points_seen_but_no_convergence_blames_geometry():
    msg = stage1_timeout_reason(123, 150.0, "/tmp/pipeline_stage1.log")
    assert "123" in msg
    assert "没收敛" in msg
    assert "手眼" in msg
    # 这时**不该**再提感知依赖
    assert "ultralytics" not in msg
 def test_timeout_seconds_and_log_path_are_in_the_message():
    msg = stage1_timeout_reason(0, 42.0, "/tmp/x.log")
    assert "42s" in msg
    assert "/tmp/x.log" in msg
@@ -0,0 +1,96 @@
 """site.yaml 的**机器人级覆盖**必须登记在案。
 site.yaml 有两层：顶层是站点配置，``robots[]`` 里每台臂可以覆盖若干**段**
 （camera/paths/servo/…）。合并只发生在 ``apply_active_arm`` 的
 ``_ARM_SECTION_KEYS`` 上 —— 没登记进那张表的段，**覆盖会静默失效**：
 文件里写着 ``source: truth``，合出来还是 yolo。
 这个坑踩过两次（yolo、perception），两次的现象都是"配置明明改了，行为没变"，
 而且不报错。所以这里钉一条结构性的规矩：
    robots[] 里任何**嵌套映射**（段）都必须出现在 _ARM_SECTION_KEYS 里。
 标量键（sn/arm/label/ip/tool_name/tactile…）由代码逐个读，不参与合并，不受此限。
 """
 from __future__ import annotations
 import sys
 from pathlib import Path
 from typing import Iterable, List, Mapping
 import pytest
 import yaml
 # 测试在 src/<包>/test/ 下 → 仓库根是 parents[3]（[0]=test,[1]=包,[2]=src）
 ROOT = Path(__file__).resolve().parents[3]
 SCRIPTS_LIB = ROOT / "scripts" / "lib"
 def robot_entries(doc: Mapping) -> List[Mapping]:
    """site.yaml 里的机器人列表（``robot.robots``）。
    ⚠️ 路径是 **robot.robots**，不是顶层的 robots。写成 ``doc.get("robots")``
    会拿到 None → 循环一次都不跑 → 测试"永远通过"（这个假通过本文件就踩过一次）。
    所以下面那个函数一律经过这里取，并且断言列表非空。
    """
    entries = ((doc.get("robot") or {}).get("robots")) or []
    assert entries, "site.yaml 里 robot.robots 是空的 —— 键路径写错了？"
    return list(entries)
 def unregistered_sections(robots: Iterable[Mapping], keys: Iterable[str]) -> List[str]:
    """哪些机器人级的"段"没登记进合并表（返回 ``SN.段名``）。"""
    known = set(keys)
    bad: List[str] = []
    for robot in robots:
        for key, val in dict(robot).items():
            if isinstance(val, dict) and key not in known:
                bad.append(f"{robot.get('sn')}.{key}")
    return bad
@pytest.fixture(scope="module")
 def arm_section_keys() -> tuple[str, ...]:
    sys.path.insert(0, str(SCRIPTS_LIB))
    import export_site_env  # type: ignore
    return tuple(export_site_env._ARM_SECTION_KEYS)
 def test_every_robot_level_section_is_registered_for_merging(arm_section_keys):
    doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8"))
    bad = unregistered_sections(robot_entries(doc), arm_section_keys)
    assert not bad, (
        "这些机器人级段没登记进 _ARM_SECTION_KEYS，覆盖会**静默失效**："
        f"{bad}\n"
        "  修法：scripts/lib/export_site_env.py 的 _ARM_SECTION_KEYS 里加上它们。"
    )
 def test_robot_entries_refuses_a_wrong_key_path():
    """取错键路径必须**报错**，不能静默返回空列表（那会让自检永远通过）。"""
    with pytest.raises(AssertionError):
        robot_entries({"robots": [{"sn": "x"}]})  # 少了外层 robot:
 def test_the_predicate_can_actually_fail(arm_section_keys):
    """判据本身要能报错：否则上面那条可能只是"永远通过"。"""
    bad = unregistered_sections(
        [{"sn": "x", "camera": {"name": "c"}, "brand_new_section": {"a": 1}}],
        arm_section_keys,
    )
    assert bad == ["x.brand_new_section"]
 def test_sim_robot_carries_the_truth_backend_override(arm_section_keys):
    """仿真那条 ``perception.source: truth`` 是真的会被合并进去的。
    这条同时钉住"两份配置别漂移"：site.yaml 里删掉了它，
    仿真就会退回 yolo 去跑推理（然后因为缺依赖起不来）。
    """
    doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8"))
    sim = [r for r in robot_entries(doc) if r.get("sn") == "sim"]
    assert sim, "site.yaml 里没有 sim 机器人 —— 仿真流程会挑不到配置"
    src = (sim[0].get("perception") or {}).get("source")
    assert src == "truth", f"sim 的 perception.source 应为 truth，现在是 {src!r}"
@@ -0,0 +1,79 @@
 """触觉"有没有数据"的判据：为什么不能拿力的大小当传感器活着的代理。
 背景（真实踩到）：S07 一进柔顺下压就报
    ERROR S07 触觉断流，无法判断接触：触觉话题断流 >3.0s（左右指都无新鲜数据）
 而同一时刻 ``ros2 topic hz /tactile/left/status`` 是 19Hz、``connected: true``、
 ``fnormal ≈ 1.3e-05``。旧判据是 ``|fnormal| > 1.0`` —— 把"力小"当成了"没数据"，
 于是**空手/未接触/形变任意单位**这几种正常情形全都判成断流。
 契约（``sim_tactile_adapter`` 里写明、真机 ``tactile_node`` 同款）：
    没数据  → 发零 **且** connected=false
    零  力  → connected=true + fnormal≈0
 所以判据只能是"够新 + connected"，与幅值无关。
 """
 from __future__ import annotations
 import time
 from dataclasses import dataclass
 from arm_application.stage3_search_node import (
    TACTILE_MAX_AGE_S,
    live_tactile,
    tactile_timeout_reason,
 )
@dataclass
 class _Msg:
    fnormal: float = 0.0
    connected: bool = True
 def test_zero_force_but_connected_is_live_data():
    """静止读数≈0 是**有效数据**，不是断流。"""
    now = time.monotonic()
    live = live_tactile(now, [(now, _Msg(fnormal=1.3e-05, connected=True)), None])
    assert len(live) == 1
    assert live[0].fnormal == 1.3e-05
 def test_disconnected_frames_are_not_live_even_if_they_keep_arriving():
    """帧一直在到、但 connected=false（设备没连上）→ 不算可用。"""
    now = time.monotonic()
    assert live_tactile(now, [(now, _Msg(connected=False)), (now, _Msg(connected=False))]) == []
 def test_stale_frames_are_not_live():
    now = time.monotonic()
    old = now - TACTILE_MAX_AGE_S - 0.01
    assert live_tactile(now, [(old, _Msg(connected=True))]) == []
 def test_both_sides_live_keeps_both():
    now = time.monotonic()
    live = live_tactile(now, [(now, _Msg(fnormal=3.0)), (now, _Msg(fnormal=5.0))])
    assert [m.fnormal for m in live] == [3.0, 5.0]
 def test_timeout_reason_separates_device_problem_from_no_publisher():
    """两种"没数据"必须报**不同**的话：一个查驱动/设备，一个查谁在发话题。"""
    now = time.monotonic()
    dev = tactile_timeout_reason([(now, _Msg(connected=False)), None], 3.0)
    assert "connected=false" in dev
    assert "run_tactile.sh" in dev          # 给出下一步做什么
    assert "断流" not in dev                # 不能把人往"话题没起来"上带
    none = tactile_timeout_reason([None, None], 3.0)
    assert "断流" in none and "3.0" in none
 def test_timeout_reason_still_reports_stall_when_only_stale_frames_exist():
    """帧过期（驱动卡住）仍然算"断流"，不该被说成设备没连上。"""
    old = time.monotonic() - 10.0
    msg = tactile_timeout_reason([(old, _Msg(connected=True)), None], 3.0)
    assert "断流" in msg