P6 通止规装配：主流程节点（S00–S11 跑通）+ 入口脚本 + 真机/仿真验证

新增
----
- arm_application/gauge_assembly_node.py：主流程。每个阶段一个 _step_*，
  返回值就是状态机的事件；纯逻辑（状态机/配置/限位/夹爪后端/自检）全部
  在 gauge_*.py 里，节点只做"发指令 + 读反馈 + 报错"。
  * S00 自检报告带"缺了会怎样"，必检拦住、选检告警；
  * S01 回 home → 切工具系 → 逆时针预绕，**事后用 TF 实测绕工具 Z 转了多少度**
    （转轴偏差 >5° 直接中止：以"度"为单位的动作全建立在这条假设上）；
  * S03 手外粗定位：没有 eye-to-hand 标定文件时明确报"缺哪个文件、怎么生成"，
    回 S04 兜底（不做假装成功的反投影）；
  * S05 稳定判据用"到均值的最远距离"，不用相邻帧之差；
  * S07 悬停处测基线 → 步进下压 → 两路切向任一突变即停，法向超安全上限抬起中止；
  * S08 棘轮四步（慢转→松爪→快回转→夹紧），停止判据「切向 >阈值 且 法向突增」，
    掉件走 E1 重放；S09 反向拧出（比拧入多一轮）；
  * E1 重放等 /gauge/reload_ok 人工确认（超时按中止），E2 任何中止都先退安全高度。
- launch/gauge_assembly.launch.py：只做一件事 —— 发布手眼静态 TF（S05 的前提），
  其余判据全交给 S00（判断标准只有一处）。
- scripts/run_gauge_assembly.sh：--arm/--sim/--dry-run/--holes/--backend。
- gauge_assembly 入口（setup.py）+ 站点参数 reload_timeout_s。
- test/test_gauge_step_events.py：**读节点源码 AST**，把每个 _step_* 可能返回的
  事件逐个问状态机"合法吗" —— 节点与转移表是两份数据，漏登记要在 pytest 里炸，
  而不是在真机上跑到那一步才炸。

顺手修掉的两个真问题
--------------------
1. 四份 launch 里的四元数换算**已经互相抄歪了**（退化分支的公式各不相同）。
   统一收口到 handeye_lib.read_transform_yaml，并加回环测试（真实标定文件 +
   180°/270° 等分支切换角度）；实测 4 个 launch 输出与库完全一致。
2. pipeline_node.AssemblyPipeline 支持 node_name 复用（本流程继承它的一阶段
   跟踪/二阶段定位链，但在图上必须是另一个节点名）。

验证（都在本机真跑过）
----------------------
  pytest（仿真环境）              → 349 passed, 14 skipped（缺 onnx 的用例跳过）
  干跑全流程                      → S00→S01→…→S11→DONE，exit 0
  仿真里不干跑（缺 /home 等）      → S00 拦住，exit 1，未发任何运动指令
另：仿真环境没有 onnx（只有真机 conda 有），相关用例改为 skip —— 让测试结果
"红就是真红"。

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

scripts/run_gauge_assembly.sh

@@ -0,0 +1,90 @@
+#!/usr/bin/env bash
+# 通止规装配（看板 S00–S11）：自检 → 回 home+极限角 → 跟踪 → 定位 → 悬停 →
+# 柔顺接触 → 棘轮拧入 → 反向拧出 → 换孔 → 复位。
+#
+# 前置（本脚本**不**替你起这些，只让 S00 自检告诉你缺什么）：
+#   T1  bash ~/ros2/scripts/run_real.sh --arm 89          # 机械臂控制器 + /home
+#   T2  bash ~/ros2/scripts/run_camera_d405.sh --arm 89   # 腕部 D405 预览
+#   T2  bash ~/ros2/scripts/run_tactile.sh                # Orisys 触觉三向力
+#   T3  bash ~/ros2/scripts/run_gripper.sh                # 夹爪服务（张开/夹紧/状态）
+#   ——  手眼标定产物 handeye/results/<SN>_eye_in_hand.yaml（S05 定位的前提）
+#
+# 为什么不在脚本里检查/拉起这些：判断标准只能有一处。脚本里查一遍、节点里
+# 再查一遍，就会出现"脚本说没问题、节点说没数据"（发布者不在的假阳性），
+# 而 S00 的报告是打印给人看的，带"缺了会怎样"。
+#
+# 用法：
+#   bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89                # 完整装配（2 个孔）
+#   bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --holes 1      # 只装 k1
+#   bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --dry-run      # 只验编排/配置，不动臂
+#   bash ~/ros2/scripts/run_gauge_assembly.sh --sim --dry-run         # 仿真环境跑干跑
+#   bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --backend legacy   # 强制真机夹爪后端
+#
+# 参数接口：config/site.yaml → gauge_assembly（力阈值/角度/轮数/超时全部可调）
+# 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看读数量级
+# 掉件重放：bash ~/ros2/scripts/run_gauge_assembly.sh 之外另开一个终端
+#           ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}'
+set -eo pipefail
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
+
+USE_SIM=false
+DRY_RUN=false
+HOLES=""
+BACKEND="auto"
+
+# shellcheck disable=SC1091
+source "${ROOT}/scripts/lib/parse_arm_args.sh"
+parse_arm_args "$@"
+set -- "${PARSE_ARM_REST[@]}"
+
+while [[ $# -gt 0 ]]; do
+  case "$1" in
+    --sim) USE_SIM=true; shift ;;
+    --dry-run) DRY_RUN=true; shift ;;
+    --holes) HOLES="${2:?}"; shift 2 ;;
+    --backend) BACKEND="${2:?}"; shift 2 ;;
+    -h|--help) sed -n '2,26p' "$0"; exit 0 ;;
+    *) echo "Unknown: $1" >&2; exit 1 ;;
+  esac
+done
+
+case "${BACKEND}" in
+  auto|contract|legacy) ;;
+  *) echo "--backend 只能是 auto|contract|legacy，收到 ${BACKEND}" >&2; exit 1 ;;
+esac
+
+if [[ "${USE_SIM}" == true ]]; then
+  echo "=== 仿真环境（install_sim + ROS_DOMAIN_ID=77） ==="
+  # shellcheck disable=SC1091
+  source "${ROOT}/scripts/sim_env.sh"
+else
+  # shellcheck disable=SC1091
+  source "${ROOT}/scripts/env.sh"
+fi
+
+if [[ -z "${SITE_ROBOT_SN:-}" ]]; then
+  echo "ERROR: 没选出机械臂。用 --arm 89 指定，或填 config/site.yaml 的 robot.sn" >&2
+  exit 1
+fi
+
+if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
+  export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
+  export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
+fi
+
+echo "=== 通止规装配  arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN}) ==="
+echo "    孔数=${HOLES:-site}  夹爪后端=${BACKEND}  干跑=${DRY_RUN}"
+
+# 清掉上一轮可能残留的装配节点（同一时间只允许一个进程在指挥这条臂）
+# 不 pkill 整个 arm_application：会连相机/触觉服务一起带走（那些是本脚本的前置）
+pkill -f 'lib/arm_application/gauge_assembly' 2>/dev/null || true
+sleep 0.5
+
+ARGS=(gripper_backend:="${BACKEND}")
+if [[ -n "${HOLES}" ]]; then ARGS+=(holes_total:="${HOLES}"); fi
+if [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi
+
+echo "=== launch gauge_assembly（自检不过会自动报缺什么） ==="
+# 不用 exec：exec 之后 Ctrl-C 直接打给 ros2 launch，这里的清理就没了；
+# gauge_assembly 自己也会在收到 KeyboardInterrupt 时退到安全高度。
+ros2 launch arm_application gauge_assembly.launch.py "${ARGS[@]}"

src/arm_application/arm_application/gauge_boot.py

@@ -67,8 +67,12 @@ class BootCheck:
 _ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"})
 
 
-def _abs(name: str) -> str:
+def abs_name(name: str) -> str:
-    """把 ROS 名字规范成绝对名（``home`` → ``/home``）。"""
+    """把 ROS 名字规范成绝对名（``home`` → ``/home``）。
+
+    对外公开：调用方（节点）也要用同一套规范化去比较名字，否则会出现
+    "清单里是 /foo、参数里是 foo，比较不相等"这类只在运行时暴露的错。
+    """
     n = (name or "").strip()
     if not n:
         return n
@@ -116,7 +120,12 @@ def build_checks(
             probe="action",
             target=joint_action,
             why="没有它，S01/S06/S07/S10 的所有运动都发不出去",
-            expect_type="arm_msgs/action/FollowJointTrajectory",
+            # 类型必须是 **control_msgs**：本流程（和 stage2/stage3/pipeline 一样）
+            # 直接把轨迹发给 ros2_control 的 JTC 服务端，不经过 arm_control 的
+            # 轨迹适配器（那个用的是 arm_msgs/action/FollowJointTrajectory）。
+            # 写成 arm_msgs 的会得到一个"永远探不到服务端"的自检 —— 假阴性，
+            # 而且看起来像控制器没起来。
+            expect_type="control_msgs/action/FollowJointTrajectory",
         ),
         BootCheck(
             key="arm_home",
@@ -134,20 +143,16 @@ def build_checks(
             why="/home 内部转发给它；它不在时回零会超时",
             expect_type="arm_msgs/srv/SetNamedPose",
         ),
-        BootCheck(
-            key="arm_switch_tool",
-            label="工具系切换服务",
-            probe="service",
-            target="/switch_tool",
-            why="tool1 必须全程垂直向下且位姿锁定，切不过去后面全是歪的",
-            expect_type="arm_msgs/srv/SwitchTool",
-        ),
         BootCheck(
             key="arm_compliant",
             label="柔顺下压 action",
             probe="action",
             target="/compliant_descent",
-            why="S07 的柔顺接触靠它；缺了只能用位置控制硬压",
+            # 选检：S07 现在走节点内的"步进 + 力突变停"（与这个 action 内部
+            # 同一套做法），不依赖它；有它时驱动侧原语更顺，属于加分项。
+            # 标成必检会让仿真（arm_driver_flexiv 不参与）永远过不了自检。
+            why="有它时 S07 用驱动侧柔顺原语；没有则用节点内步进（不影响正确性）",
+            required=False,
             expect_type="arm_msgs/action/CompliantDescent",
         ),
         BootCheck(
@@ -223,6 +228,33 @@ def build_checks(
                 expect_type="arm_msgs/srv/SwitchTool",
             )
         )
+        # 配了工具系就必须能切：S01 切不过去会直接 TIMEOUT 中止，
+        # 所以在自检里按**必检**拦下（早报比跑到 S01 再报少一次上电动作）。
+        checks.append(
+            BootCheck(
+                key="arm_switch_tool",
+                label="工具系切换服务",
+                probe="service",
+                target="/switch_tool",
+                why=f"site.yaml 指定了工具 {tool_name!r}，切不过去后面全是歪的",
+                expect_type="arm_msgs/srv/SwitchTool",
+            )
+        )
+    else:
+        # 没配工具系：S01 只会告警不会切，所以这条只算选检 ——
+        # 真机没配工具系是**配置问题**（看板要求 tool1），但仿真里根本没有
+        # 工具系切换服务，标必检会让仿真永远过不去。
+        checks.append(
+            BootCheck(
+                key="arm_switch_tool",
+                label="工具系切换服务",
+                probe="service",
+                target="/switch_tool",
+                why="本机没配 robot.tool_name：真机应配 tool1 并确认这个服务在；仿真不需要",
+                required=False,
+                expect_type="arm_msgs/srv/SwitchTool",
+            )
+        )
     if weights_path:
         checks.append(
             BootCheck(
@@ -239,7 +271,7 @@ def build_checks(
     return [
         c if c.probe not in _ABSOLUTE_PROBES
         else BootCheck(
-            key=c.key, label=c.label, probe=c.probe, target=_abs(c.target),
+            key=c.key, label=c.label, probe=c.probe, target=abs_name(c.target),
             why=c.why, required=c.required, expect_type=c.expect_type,
         )
         for c in checks

src/arm_application/arm_application/gauge_config.py

@@ -90,6 +90,7 @@ _TYPES: Dict[str, type] = {
     "unscrew_extra_cycles": int,
     "holes_total": int,
     "dry_run": bool,
+    "reload_timeout_s": float,
 }
 
 #: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。
@@ -131,6 +132,7 @@ _UNITS: Dict[str, str] = {
     "unscrew_extra_cycles": "",
     "holes_total": "",
     "dry_run": "",
+    "reload_timeout_s": "s",
 }
 
 #: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类
@@ -201,6 +203,9 @@ DEFAULTS: Dict[str, Any] = {
     # 全局
     "holes_total": 2,
     "dry_run": False,
+    # E1（掉件）重放：等操作员放好并确认 /gauge/reload_ok 的最长时间。
+    # 超时 = 中止（臂在 home，人不在旁边时"继续等"比"停"更危险）。
+    "reload_timeout_s": 180.0,
 }
 
 
@@ -245,6 +250,7 @@ class GaugeConfig:
     unscrew_extra_cycles: int
     holes_total: int
     dry_run: bool
+    reload_timeout_s: float
 
     # -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）-------------------
     @property

src/arm_application/arm_application/handeye_lib.py

@@ -7,8 +7,10 @@
 
 from __future__ import annotations
 
+import math
 from collections import deque
 from dataclasses import dataclass
+from pathlib import Path
 from typing import Deque, List, Optional, Sequence, Tuple
 
 import numpy as np
@@ -32,6 +34,69 @@ class CameraIntrinsics:
     dist: np.ndarray
 
 
+def R_to_quat_xyzw(R: np.ndarray) -> Tuple[float, float, float, float]:
+    """3×3 旋转 → ``(x, y, z, w)`` 四元数（Shepperd 法，按最大分量选分支）。
+
+    为什么放在库里而不是各个 launch 里：这段换算**已经在两份 launch 里各抄了
+    一遍**，而且抄错过（对角分支的公式被写歪，症状是手眼 TF 差了 30°，
+    定位看起来"总能到附近"但永远差一点）。换算只有一份才可能被测试：
+    ``test_handeye_lib_roundtrip.py`` 拿真实标定文件验证
+    ``quat_xyzw_to_R(R_to_quat_xyzw(R)) == R``。
+    """
+    R = np.asarray(R, dtype=float).reshape(3, 3)
+    tr = float(R[0, 0] + R[1, 1] + R[2, 2])
+    if tr > 0.0:
+        s = math.sqrt(tr + 1.0) * 2.0
+        w = 0.25 * s
+        x = (R[2, 1] - R[1, 2]) / s
+        y = (R[0, 2] - R[2, 0]) / s
+        z = (R[1, 0] - R[0, 1]) / s
+    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+        s = math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
+        w = (R[2, 1] - R[1, 2]) / s
+        x = 0.25 * s
+        y = (R[0, 1] + R[1, 0]) / s
+        z = (R[0, 2] + R[2, 0]) / s
+    elif R[1, 1] > R[2, 2]:
+        s = math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
+        w = (R[0, 2] - R[2, 0]) / s
+        x = (R[0, 1] + R[1, 0]) / s
+        y = 0.25 * s
+        z = (R[1, 2] + R[2, 1]) / s
+    else:
+        s = math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
+        w = (R[1, 0] - R[0, 1]) / s
+        x = (R[0, 2] + R[2, 0]) / s
+        y = (R[1, 2] + R[2, 1]) / s
+        z = 0.25 * s
+    n = math.sqrt(x * x + y * y + z * z + w * w)
+    if n <= 0.0:
+        raise ValueError("旋转矩阵无法转成四元数（矩阵为零？）")
+    return x / n, y / n, z / n, w / n
+
+
+def read_transform_yaml(path: str | Path) -> Tuple[np.ndarray, Tuple[float, float, float, float]]:
+    """读手眼标定产物 → ``(xyz_m, quat_xyzw)``。
+
+    文件格式见 :func:`format_transform_yaml`：``T_4x4`` 是 4 行 4 列的列表。
+    读取时**必须校验形状**：手眼文件是现场人手拷来拷去的，形状不对时
+    直接报错，比"取到一堆 nan 然后发了个全零 TF"好得多（后者会让定位
+    永远差一个常量，而且看起来像相机标定不准）。
+    """
+    import yaml  # 局部导入：本模块其它函数不需要 yaml（标定计算本身不依赖）
+
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f) or {}
+    T = data.get("T_4x4")
+    if T is None:
+        raise ValueError(f"{path}: 缺少 T_4x4 字段")
+    arr = np.asarray(T, dtype=float)
+    if arr.shape != (4, 4):
+        raise ValueError(f"{path}: T_4x4 应为 4x4，实际 {arr.shape}")
+    xyz = np.array([arr[0, 3], arr[1, 3], arr[2, 3]], dtype=float)
+    return xyz, R_to_quat_xyzw(arr[:3, :3])
+
+
 def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
     x, y, z, w = [float(v) for v in q]
     n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12

src/arm_application/arm_application/pipeline_node.py

@@ -25,8 +25,12 @@ from arm_application.stage3_search_node import Stage3Search
 
 
 class AssemblyPipeline(Stage3Search):
-    def __init__(self) -> None:
+    def __init__(self, node_name: str = "assembly_pipeline") -> None:
-        super().__init__(node_name="assembly_pipeline")
+        # node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用
+        # 一阶段跟踪的拉起/拆分与二阶段定位链，但它是**另一条流程**，
+        # 在图上必须是另一个节点名（否则 ros2 node list 里两个同名的节点，
+        # 出问题时连"是哪条流程在发指令"都分不清）。
+        super().__init__(node_name=node_name)
         self.declare_parameter("skip_stage1", False)
         self.declare_parameter("skip_stage2", False)
         self.declare_parameter("skip_stage3", False)  # 默认搜孔+轻压；拧入靠 --depth

src/arm_application/launch/gauge_assembly.launch.py

@@ -0,0 +1,157 @@
+#!/usr/bin/env python3
+"""启动通止规装配主流程（看板 S00–S11）。
+
+与 stage2/stage3 的 launch 差别：**这里不检查任何服务**。
+"所有服务是否就绪"是 S00 的职责（``gauge_boot`` + 节点里的探测），
+在 launch/shell 里再查一遍就会出现两处判据 —— 一处改了另一处没改，
+现场表现为"脚本说没问题、节点说没数据"。
+
+唯一在这里做的是**发布手眼静态 TF**：腕部相机的 eye-in-hand 外参是主流程
+（S05 精拍定位）算坐标的前提，而它只能由标定产物提供（site.yaml →
+paths.handeye_file）。文件不存在时不发 TF，节点会在 S05 报"查不到 TF"。
+
+用法（一般由 scripts/run_gauge_assembly.sh 调用）::
+
+    ros2 launch arm_application gauge_assembly.launch.py
+    ros2 launch arm_application gauge_assembly.launch.py dry_run:=true holes_total:=1
+"""
+
+from __future__ import annotations
+
+import os
+import sys
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def _ws_root() -> Path:
+    """工作区根目录：优先环境变量，其次向上找 config/site.yaml。"""
+    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v)
+    here = Path(__file__).resolve()
+    for i in range(2, 8):
+        p = here.parents[i]
+        if (p / "config" / "site.yaml").is_file():
+            return p
+    return Path.home() / "ros2"
+
+
+def _handeye_xyz_xyzw(path: str):
+    """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
+
+    **不要在这里再写一遍四元数换算**：这段换算在别的 launch 里已各抄了一遍，
+    抄错过的症状是"手眼 TF 差了 30°"（定位总能到附近，永远差一点）。
+    实现只有一份在 ``arm_application.handeye_lib``，且被回环测试钉住。
+    """
+    from arm_application.handeye_lib import read_transform_yaml
+
+    xyz, quat = read_transform_yaml(path)
+    return (
+        float(xyz[0]), float(xyz[1]), float(xyz[2]),
+        float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
+    )
+
+
+def _launch_setup(context, *args, **kwargs):
+    root = _ws_root()
+    sys.path.insert(0, str(root / "scripts" / "lib"))
+    from export_site_env import apply_active_arm, load_site, site_path  # type: ignore
+
+    cfg = os.environ.get("SITE_CONFIG", "").strip()
+    path = Path(cfg).expanduser() if cfg else site_path()
+    if not path.is_file():
+        path = root / "config" / "site.yaml"
+    data = load_site(path)
+    merged, robot = apply_active_arm(data)
+
+    ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
+    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()
+    cam_cfg = merged.get("camera") or {}
+    cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
+    cam_frame = f"{cam}_color_optical_frame"
+
+    def _flag(name: str) -> bool:
+        return LaunchConfiguration(name).perform(context).strip().lower() in (
+            "1", "true", "yes", "on",
+        )
+
+    dry_run = _flag("dry_run")
+    holes = LaunchConfiguration("holes_total").perform(context).strip()
+    backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto"
+
+    nodes = []
+    frames = []
+    if os.path.isfile(handeye):
+        try:
+            tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
+        except Exception as exc:  # noqa: BLE001
+            frames.append(f"[gauge] 手眼文件读不了（{handeye}）：{exc}")
+        else:
+            frames.append(f"[gauge] 手眼 TF {ee_frame} ← {cam_frame}（{Path(handeye).name}）")
+            nodes.append(
+                Node(
+                    package="tf2_ros",
+                    executable="static_transform_publisher",
+                    name="gauge_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", ee_frame,
+                        "--child-frame-id", cam_frame,
+                    ],
+                    output="screen",
+                )
+            )
+    else:
+        frames.append(
+            f"[gauge] 没有手眼文件（{handeye or 'paths.handeye_file 为空'}）"
+            "—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh"
+        )
+
+    nodes.append(
+        Node(
+            package="arm_application",
+            executable="gauge_assembly",
+            name="gauge_assembly",
+            output="screen",
+            parameters=[
+                {
+                    "robot_sn": str(
+                        robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""
+                    ),
+                    "dry_run": dry_run,
+                    "holes_total": int(holes) if holes else -1,
+                    "gripper_backend": backend,
+                }
+            ],
+        )
+    )
+
+    return [
+        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}"
+        ),
+        *[LogInfo(msg=m) for m in frames],
+        *nodes,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("dry_run", default_value="false"),
+            DeclareLaunchArgument("holes_total", default_value=""),
+            DeclareLaunchArgument("gripper_backend", default_value="auto"),
+            OpaqueFunction(function=_launch_setup),
+        ]
+    )

src/arm_application/launch/pipeline.launch.py

@@ -7,12 +7,10 @@
 
 from __future__ import annotations
 
-import math
 import os
 import sys
 from pathlib import Path
 
-import yaml
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
 from launch.actions import EmitEvent, RegisterEventHandler
@@ -34,43 +32,21 @@ def _ws_root() -> Path:
     return Path.home() / "ros2"
 
 
-def _quat_from_R(R):
-    tr = float(R[0][0] + R[1][1] + R[2][2])
-    if tr > 0:
-        s = math.sqrt(tr + 1.0) * 2.0
-        w = 0.25 * s
-        x = (R[2][1] - R[1][2]) / s
-        y = (R[0][2] - R[2][0]) / s
-        z = (R[1][0] - R[0][1]) / s
-    elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
-        s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
-        w = (R[2][1] - R[1][2]) / s
-        x = 0.25 * s
-        y = (R[0][1] + R[1][0]) / s
-        z = (R[0][2] - R[2][1]) / s
-    elif R[1][1] > R[2][2]:
-        s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
-        w = (R[0][2] - R[2][1]) / s
-        x = (R[0][1] + R[1][0]) / s
-        y = 0.25 * s
-        z = (R[1][2] - R[2][1]) / s
-    else:
-        s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
-        w = (R[1][2] - R[0][1]) / s
-        x = (R[0][2] - R[0][1]) / s
-        y = (R[1][2] - R[2][0]) / s
-        z = 0.25 * s
-    return x, y, z, w
-
-
 def _handeye_xyz_xyzw(path: str):
-    with open(path, "r", encoding="utf-8") as f:
+    """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
-        data = yaml.safe_load(f)
+
-    T = data["T_4x4"]
+    实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
-    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
-    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
-    qx, qy, qz, qw = _quat_from_R(R)
+    手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
-    return tx, ty, tz, qx, qy, qz, qw
+    """
+    from arm_application.handeye_lib import read_transform_yaml
+
+    xyz, quat = read_transform_yaml(path)
+    return (
+        float(xyz[0]), float(xyz[1]), float(xyz[2]),
+        float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
+    )
 
 
 def _as_bool(s: str) -> bool:

src/arm_application/launch/stage2_locate.launch.py

@@ -7,12 +7,10 @@
 
 from __future__ import annotations
 
-import math
 import os
 import sys
 from pathlib import Path
 
-import yaml
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
@@ -32,43 +30,21 @@ def _ws_root() -> Path:
     return Path.home() / "ros2"
 
 
-def _quat_from_R(R):
-    tr = float(R[0][0] + R[1][1] + R[2][2])
-    if tr > 0:
-        s = math.sqrt(tr + 1.0) * 2.0
-        w = 0.25 * s
-        x = (R[2][1] - R[1][2]) / s
-        y = (R[0][2] - R[2][0]) / s
-        z = (R[1][0] - R[0][1]) / s
-    elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
-        s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
-        w = (R[2][1] - R[1][2]) / s
-        x = (R[0][1] + R[1][0]) / s
-        y = (R[1][0] + R[0][1]) / s
-        z = (R[0][2] - R[2][1]) / s
-    elif R[1][1] > R[2][2]:
-        s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
-        w = (R[0][2] - R[2][1]) / s
-        x = (R[0][1] + R[1][0]) / s
-        y = (R[1][2] + R[2][1]) / s
-        z = (R[1][2] - R[2][1]) / s
-    else:
-        s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
-        w = (R[1][2] - R[0][1]) / s
-        x = (R[0][2] - R[0][1]) / s
-        y = (R[1][2] - R[2][0]) / s
-        z = (R[1][2] - R[2][1]) / s
-    return x, y, z, w
-
-
 def _handeye_xyz_xyzw(path: str):
-    with open(path, "r", encoding="utf-8") as f:
+    """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
-        data = yaml.safe_load(f)
+
-    T = data["T_4x4"]
+    实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
-    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
-    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
-    qx, qy, qz, qw = _quat_from_R(R)
+    手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
-    return tx, ty, tz, qx, qy, qz, qw
+    """
+    from arm_application.handeye_lib import read_transform_yaml
+
+    xyz, quat = read_transform_yaml(path)
+    return (
+        float(xyz[0]), float(xyz[1]), float(xyz[2]),
+        float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
+    )
 
 
 def _launch_setup(context, *args, **kwargs):

src/arm_application/launch/stage3_search.launch.py

@@ -8,12 +8,10 @@
 
 from __future__ import annotations
 
-import math
 import os
 import sys
 from pathlib import Path
 
-import yaml
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
@@ -32,43 +30,21 @@ def _ws_root() -> Path:
     return Path.home() / "ros2"
 
 
-def _quat_from_R(R):
-    tr = float(R[0][0] + R[1][1] + R[2][2])
-    if tr > 0:
-        s = math.sqrt(tr + 1.0) * 2.0
-        w = 0.25 * s
-        x = (R[2][1] - R[1][2]) / s
-        y = (R[0][2] - R[2][0]) / s
-        z = (R[1][0] - R[0][1]) / s
-    elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
-        s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
-        w = (R[2][1] - R[1][2]) / s
-        x = (R[0][1] + R[1][0]) / s
-        y = (R[1][0] + R[0][1]) / s
-        z = (R[1][2] - R[2][1]) / s
-    elif R[1][1] > R[2][2]:
-        s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
-        w = (R[0][2] - R[2][1]) / s
-        x = (R[1][0] + R[0][1]) / s
-        y = (R[1][2] + R[2][1]) / s
-        z = (R[1][2] - R[2][1]) / s
-    else:
-        s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
-        w = (R[1][2] - R[0][1]) / s
-        x = (R[0][2] - R[0][1]) / s
-        y = (R[1][2] + R[2][1]) / s
-        z = (R[1][2] - R[2][1]) / s
-    return x, y, z, w
-
-
 def _handeye_xyz_xyzw(path: str):
-    with open(path, "r", encoding="utf-8") as f:
+    """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
-        data = yaml.safe_load(f)
+
-    T = data["T_4x4"]
+    实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
-    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
-    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
-    qx, qy, qz, qw = _quat_from_R(R)
+    手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
-    return tx, ty, tz, qx, qy, qz, qw
+    """
+    from arm_application.handeye_lib import read_transform_yaml
+
+    xyz, quat = read_transform_yaml(path)
+    return (
+        float(xyz[0]), float(xyz[1]), float(xyz[2]),
+        float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
+    )
 
 
 def _launch_setup(context, *args, **kwargs):

src/arm_application/launch/visual_servo.launch.py

@@ -6,12 +6,10 @@
 
 from __future__ import annotations
 
-import math
 import os
 import sys
 from pathlib import Path
 
-import yaml
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
@@ -54,43 +52,21 @@ def _load_site():
     return merged, path, perception_overrides(data), servo_overrides(data), robot
 
 
-def _quat_from_R(R):
-    tr = float(R[0][0] + R[1][1] + R[2][2])
-    if tr > 0:
-        s = math.sqrt(tr + 1.0) * 2.0
-        w = 0.25 * s
-        x = (R[2][1] - R[1][2]) / s
-        y = (R[0][2] - R[2][0]) / s
-        z = (R[1][0] - R[0][1]) / s
-    elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
-        s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
-        w = (R[2][1] - R[1][2]) / s
-        x = 0.25 * s
-        y = (R[0][1] + R[1][0]) / s
-        z = (R[0][2] + R[2][0]) / s
-    elif R[1][1] > R[2][2]:
-        s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
-        w = (R[0][2] - R[2][0]) / s
-        x = (R[0][1] + R[1][0]) / s
-        y = 0.25 * s
-        z = (R[1][2] + R[2][1]) / s
-    else:
-        s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
-        w = (R[1][0] - R[0][1]) / s
-        x = (R[0][2] + R[2][0]) / s
-        y = (R[1][2] + R[2][1]) / s
-        z = 0.25 * s
-    return x, y, z, w
-
-
 def _handeye_xyz_xyzw(path: str):
-    with open(path, "r", encoding="utf-8") as f:
+    """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
-        data = yaml.safe_load(f)
+
-    T = data["T_4x4"]
+    实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
-    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
-    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
-    qx, qy, qz, qw = _quat_from_R(R)
+    手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
-    return tx, ty, tz, qx, qy, qz, qw
+    """
+    from arm_application.handeye_lib import read_transform_yaml
+
+    xyz, quat = read_transform_yaml(path)
+    return (
+        float(xyz[0]), float(xyz[1]), float(xyz[2]),
+        float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
+    )
 
 
 def _launch_setup(context, *args, **kwargs):

src/arm_application/setup.py

@@ -78,6 +78,8 @@ setup(
             'stage3_search = arm_application.stage3_search_node:main',
             # 三段式装配流水线（跟踪 → 定位 → 找孔，单进程）
             'assembly_pipeline = arm_application.pipeline_node:main',
+            # 通止规装配（看板 S00–S11，含自检/换孔/掉件重放）
+            'gauge_assembly = arm_application.gauge_assembly_node:main',
             'tactile = arm_application.tactile_node:main',
             'tactile_cli = arm_application.tactile_cli:main',
         ],

src/arm_application/test/test_gauge_boot.py

@@ -46,12 +46,13 @@ def _by_key(checks):
 # 清单内容
 # =============================================================================
 def test_core_checks_are_present_and_required():
-    checks = _by_key(_build())
+    """核心判据依赖的东西必须是**必检**（配了工具系的那种机器）。"""
+    checks = _by_key(_build(tool_name="tool1"))
     for key in (
         "arm_jtc",
         "arm_home",
+        "arm_named_pose",
         "arm_switch_tool",
-        "arm_compliant",
         "gripper_open",
         "gripper_close",
         "camera_color",
@@ -63,6 +64,36 @@ def test_core_checks_are_present_and_required():
         assert checks[key].required, f"{key} 是核心判据依赖，必须是必检"
 
 
+def test_jtc_check_expects_the_type_this_flow_actually_calls():
+    """JTC 自检的类型必须是 control_msgs（本流程直接发给 ros2_control）。
+
+    写成 arm_msgs/action/FollowJointTrajectory（arm_control 适配器那一套）会
+    让自检**永远探不到服务端**：报"控制器没起来"，而控制器明明好着。
+    """
+    c = _by_key(_build())["arm_jtc"]
+    assert c.expect_type == "control_msgs/action/FollowJointTrajectory"
+
+
+def test_switch_tool_required_only_when_a_tool_is_configured():
+    """配了工具系 → 切不过去 S01 会中止，所以按必检拦；没配 → 选检。
+
+    没配工具系的机器上（比如仿真）标必检，会让"仿真里根本不存在
+    /switch_tool 服务"变成每次启动的一条红字，人很快就不看自检了。
+    """
+    assert _by_key(_build(tool_name="tool1"))["arm_switch_tool"].required is True
+    assert _by_key(_build(tool_name=""))["arm_switch_tool"].required is False
+
+
+def test_compliant_descent_is_optional():
+    """S07 不依赖柔顺 action（节点内步进 + 力突变停是同一套做法）。
+
+    标必检的后果：仿真里永远过不了自检，而 S07 其实跑得挺好。
+    """
+    c = _by_key(_build())["arm_compliant"]
+    assert c.required is False
+    assert "S07" in c.why
+
+
 def test_tactile_must_not_be_downgraded_to_optional():
     """**负向**：触觉是 S07/S08/E1 三个判据的唯一数据源。
 
@@ -175,9 +206,17 @@ def test_report_lists_why_so_the_operator_knows_urgency():
 
 
 def test_counts_summary():
+    """数量刻意写死：加/减一条自检项时必须来这里改一行并想清楚它属于哪一类。
+
+    当前（未配工具系、有手外相机、无权重）：
+      必检 9 = arm_jtc, arm_home, arm_named_pose, gripper_open, gripper_close,
+               camera_color, camera_info, tactile_status, gripper_status
+      选检 3 = coarse_camera, arm_switch_tool, arm_compliant
+    """
     c = summary_counts(_results(coarse_camera=False))
-    assert c["required_total"] == 11 and c["required_ok"] == 11
+    assert c["required_total"] == 9 and c["required_ok"] == 9
-    assert c["optional_total"] == 1 and c["optional_ok"] == 0
+    # 选检里只有手外相机没起来 → 3 项里 2 项通过（工具系/柔顺下压按通过算）
+    assert c["optional_total"] == 3 and c["optional_ok"] == 2
 
 
 def test_all_ready_says_so():

src/arm_application/test/test_gauge_step_events.py

@@ -0,0 +1,159 @@
+"""主流程节点与状态机的**接缝**测试（不需要 ROS、不需要真机）。
+
+为什么要有这一层
+----------------
+``gauge_assembly_node`` 的每个 ``_step_*`` 方法会返回一个 :class:`Event`，
+由 ``gauge_fsm.TRANSITIONS`` 决定它去哪。这两份东西是**分开写的**：
+
+* 状态机里没有的转移 → 运行到那一步抛 :class:`FsmError`（好：当场炸）；
+* 但如果是**最危险的那一步**才炸（力超限、掉件、关节极限），代价就大了。
+
+所以这里直接读节点源码的 AST，把"每个阶段可能返回哪些事件"抽出来，
+逐个问状态机"这一步合法吗"。这样新增/修改流程时，忘了登记转移会在
+``pytest`` 里就报出来，而不是等到真机上。
+"""
+
+from __future__ import annotations
+
+import ast
+import pathlib
+from typing import Dict, Set
+
+import pytest
+
+from arm_application.gauge_fsm import Event, GaugeContext, GaugeFsm, State
+
+NODE_SRC = (
+    pathlib.Path(__file__).resolve().parents[1]
+    / "arm_application" / "gauge_assembly_node.py"
+)
+
+
+def _node_ast() -> ast.Module:
+    return ast.parse(NODE_SRC.read_text(encoding="utf-8"))
+
+
+def _step_map(tree: ast.Module) -> Dict[str, str]:
+    """``State.X -> "_step_y"``（读节点的 ``_STEPS`` 表）。"""
+    for node in ast.walk(tree):
+        # 表是带类型标注的（``_STEPS: Dict[State, str] = {...}``）→ AnnAssign
+        target = None
+        if isinstance(node, ast.AnnAssign) and isinstance(node.target, ast.Name):
+            target = node.target.id
+        elif isinstance(node, ast.Assign) and len(node.targets) == 1:
+            tgt = node.targets[0]
+            target = tgt.id if isinstance(tgt, ast.Name) else None
+        if target != "_STEPS":
+            continue
+        value = node.value
+        assert isinstance(value, ast.Dict), "_STEPS 必须是字面量字典（测试要读它）"
+        out: Dict[str, str] = {}
+        for k, v in zip(value.keys, value.values):
+            assert isinstance(k, ast.Attribute) and isinstance(v, ast.Constant)
+            out[k.attr] = str(v.value)
+        return out
+    raise AssertionError("节点里找不到 _STEPS 表")
+
+
+def _events_in(node: ast.AST) -> Set[str]:
+    """表达式里出现的所有 ``Event.XXX`` 名字。"""
+    return {
+        n.attr
+        for n in ast.walk(node)
+        if isinstance(n, ast.Attribute)
+        and isinstance(n.value, ast.Name)
+        and n.value.id == "Event"
+    }
+
+
+def _returned_events(func: ast.FunctionDef) -> Set[str]:
+    """一个 ``_step_*`` 方法可能返回的事件。
+
+    收集两处，别的地方不算（比如异常处理里顺手记一条 ``Event.ABORT`` 做日志，
+    那不是"返回"）：
+    1. 所有 ``return <expr>`` 里的 ``Event.X``；
+    2. 所有 ``fsm_event=Event.X`` 关键字实参（喂给 ``_rotate_joint_and_verify``，
+       它会把成功事件原样返回）。
+    """
+    out: Set[str] = set()
+    for node in ast.walk(func):
+        if isinstance(node, ast.Return) and node.value is not None:
+            out |= _events_in(node.value)
+        elif isinstance(node, ast.Call):
+            for kw in node.keywords:
+                if kw.arg == "fsm_event":
+                    out |= _events_in(kw.value)
+    return out
+
+
+def _func_by_name(tree: ast.Module, name: str) -> ast.FunctionDef:
+    for node in ast.walk(tree):
+        if isinstance(node, ast.FunctionDef) and node.name == name:
+            return node
+    raise AssertionError(f"节点里找不到方法 {name}")
+
+
+def test_steps_table_covers_every_non_terminal_state() -> None:
+    """每个非终态都要有处理方法 —— 漏一个就是"流程走到这儿不知道干什么"。"""
+    tree = _node_ast()
+    steps = _step_map(tree)
+    for st in State:
+        if st in (State.DONE, State.ABORT):
+            continue
+        assert st.name in steps, f"{st} 没有登记处理方法"
+    assert len(steps) == len(set(steps.values())), "一个方法不该被两个状态复用"
+
+
+def test_every_returned_event_is_legal_in_that_state() -> None:
+    """本阶段可能返回的每个事件，在状态机里必须合法。
+
+    这是本文件的主要断言：它把"节点写的事件"和"状态机登记的转移"对齐，
+    免得在真机上跑到那一步才抛 FsmError。
+    """
+    tree = _node_ast()
+    problems = []
+    for state_name, method in _step_map(tree).items():
+        state = State[state_name]
+        func = _func_by_name(tree, method)
+        for ev_name in sorted(_returned_events(func)):
+            event = Event[ev_name]
+            # 每个事件都在"干净上下文 + 该状态"下问一次状态机
+            fsm = GaugeFsm(GaugeContext(holes_total=2), start=state)
+            if not fsm.can(event):
+                problems.append(f"{state.value} 的 {method} 会返回 {ev_name}，但状态机不接受")
+    assert not problems, "节点与状态机不一致：\n  " + "\n  ".join(problems)
+
+
+def test_s08_drop_goes_to_home_extreme() -> None:
+    """E1 的落点必须还是 S01：它要在那里张开爪、等人工放好。
+
+    这条单列出来，是因为"掉件后去哪"很容易被顺手改成 S11（"反正收工"），
+    那样掉在孔里的规就没人管了。
+    """
+    fsm = GaugeFsm(GaugeContext(holes_total=2), start=State.SCREW_IN)
+    assert fsm.handle(Event.GAUGE_DROPPED) is State.HOME_EXTREME
+    assert fsm.handle(Event.RELOADED) is State.NEXT_HOLE
+    # 掉件后孔序号推进（看板：该孔记为完成，换下一个孔）
+    assert fsm.ctx.hole_index == 1
+
+
+def test_contact_force_over_limit_aborts_not_retries() -> None:
+    """法向力超上限 → 中止，不是"抬起来再来一次"。
+
+    超限说明已经压进去了（位置下限或规没对准），原地重试会把工件压变形。
+    """
+    fsm = GaugeFsm(GaugeContext(holes_total=2), start=State.CONTACT)
+    assert fsm.handle(Event.FORCE_OVER_LIMIT) is State.ABORT
+
+
+@pytest.mark.parametrize("state_name", ["SNAP", "HOVER_K"])
+def test_snap_and_hover_have_a_back_edge(state_name: str) -> None:
+    """S05/S06 必须能退回去：不稳定 → 再等；对位超差 → 重拍。
+
+    没有回边的结果是：判据没过就一路往下走（去接触、去拧），
+    而这是"孔没对准"最危险的表达方式。
+    """
+    fsm = GaugeFsm(GaugeContext(holes_total=2), start=State[state_name])
+    back = Event.SNAP_UNSTABLE if state_name == "SNAP" else Event.ALIGN_OUT
+    assert fsm.can(back)
+    assert fsm.handle(back) is State.SNAP

src/arm_application/test/test_handeye_lib_roundtrip.py

@@ -0,0 +1,146 @@
+"""手眼文件读取与四元数换算的回环测试（用真实标定产物）。
+
+为什么值得单测这个
+------------------
+``R_to_quat_xyzw`` 这段换算在仓库里被抄过好几遍，并且**抄错过**：某个分支的
+公式写歪之后，四元数还是"像模像样的数字"，静态 TF 也照发。症状是手眼外参
+差了 30°：定位总能落到工件附近（因为平移是对的），但永远差一点，
+看起来像"相机标定不准"，很难查到是 launch 里的一行除法。
+
+所以判据必须是**数学回环**（``quat → R`` 要能还原 ``R``），而不是"看数字合不合理"。
+真实标定文件（``handeye/results/*.yaml``）也一起测：文件是人手拷来拷去的，
+形状/字段被改坏时，这里直接报错比发一个全零 TF 好。
+"""
+
+from __future__ import annotations
+
+import math
+import pathlib
+
+import numpy as np
+import pytest
+
+from arm_application.handeye_lib import (
+    R_to_quat_xyzw,
+    quat_xyzw_to_R,
+    read_transform_yaml,
+)
+
+
+def _repo_root() -> pathlib.Path:
+    here = pathlib.Path(__file__).resolve()
+    for p in here.parents:
+        if (p / "config" / "site.yaml").is_file():
+            return p
+    raise AssertionError("找不到仓库根（config/site.yaml）")
+
+
+def _rot(axis: str, deg: float) -> np.ndarray:
+    a = math.radians(deg)
+    c, s = math.cos(a), math.sin(a)
+    if axis == "x":
+        return np.array([[1, 0, 0], [0, c, -s], [0, s, c]], dtype=float)
+    if axis == "y":
+        return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=float)
+    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], dtype=float)
+
+
+@pytest.mark.parametrize("axis", ["x", "y", "z"])
+@pytest.mark.parametrize("deg", [0.0, 1.0, 45.0, 90.0, 179.0, 180.0, 181.0, 270.0])
+def test_quaternion_roundtrip_by_axis(axis: str, deg: float) -> None:
+    """绕单轴转任意角度：``quat → R`` 必须还原原矩阵。
+
+    覆盖 0/180/270 这些"某个分量恰好为零"的角度 —— 四元数换算的分支
+    正是在这些角度上切换的，抄错分支的公式只会在这些角度露出来。
+    """
+    R = _rot(axis, deg)
+    q = R_to_quat_xyzw(R)
+    assert abs(math.sqrt(sum(v * v for v in q)) - 1.0) < 1e-9, "四元数未归一化"
+    R2 = quat_xyzw_to_R(q)
+    assert np.allclose(R, R2, atol=1e-9), f"轴 {axis} 转角 {deg}：回环不一致"
+
+
+def test_quaternion_roundtrip_on_composite_rotations() -> None:
+    """复合旋转（三个分支都会被走到）+ 随机旋转。"""
+    mats = [
+        _rot("x", 30.0) @ _rot("y", -70.0) @ _rot("z", 120.0),
+        _rot("z", 179.9) @ _rot("x", 90.0),
+        _rot("y", 180.0) @ _rot("x", 180.0),
+    ]
+    rng = np.random.default_rng(20260924)
+    for _ in range(50):
+        A = rng.normal(size=(3, 3))
+        Q, _R = np.linalg.qr(A)
+        if np.linalg.det(Q) < 0:
+            Q[:, 0] *= -1.0
+        mats.append(Q)
+    for R in mats:
+        assert np.allclose(R, quat_xyzw_to_R(R_to_quat_xyzw(R)), atol=1e-8)
+
+
+def test_read_real_handeye_files_match_their_own_matrix() -> None:
+    """真实标定文件：读出来的 xyz/quat 必须和文件里的 T_4x4 自洽。
+
+    这条断言的价值在于"文件 + 读取代码 + 换算"三者一旦有一个变味就会报。
+    """
+    results = _repo_root() / "handeye" / "results"
+    files = sorted(results.glob("*_eye_in_hand.yaml"))
+    if not files:
+        pytest.skip("仓库里没有手眼标定产物（还没标定过）")
+    import yaml
+
+    for f in files:
+        xyz, quat = read_transform_yaml(f)
+        # 直接拿文件里的 T_4x4 当基准（不手写解析：解析器本身就是被测对象之一，
+        # 用手写解析比对等于让两份解析互相印错）
+        T_file = np.asarray(yaml.safe_load(f.read_text(encoding="utf-8"))["T_4x4"], dtype=float)
+        assert T_file.shape == (4, 4), f"{f.name} 的 T_4x4 形状不对"
+        R = quat_xyzw_to_R(quat)
+        assert np.allclose(xyz, T_file[:3, 3], atol=1e-6), f"{f.name} 平移不一致"
+        assert np.allclose(R, T_file[:3, :3], atol=1e-6), f"{f.name} 旋转不一致"
+
+
+def test_read_transform_yaml_rejects_broken_files(tmp_path: pathlib.Path) -> None:
+    """缺字段 / 形状不对 → 明确报错，而不是回一堆 nan 去发 TF。"""
+    (tmp_path / "no_field.yaml").write_text("translation_m: [0, 0, 0]\n", encoding="utf-8")
+    with pytest.raises(ValueError, match="T_4x4"):
+        read_transform_yaml(tmp_path / "no_field.yaml")
+
+    (tmp_path / "bad_shape.yaml").write_text(
+        "T_4x4:\n  - [1, 0, 0, 0]\n  - [0, 1, 0, 0]\n", encoding="utf-8"
+    )
+    with pytest.raises(ValueError, match="4x4"):
+        read_transform_yaml(tmp_path / "bad_shape.yaml")
+
+
+def test_identical_rotation_gives_same_quaternion_up_to_sign() -> None:
+    """同一个姿态的四元数允许差一个整体符号（q 与 -q 同一个旋转）。
+
+    写这条是因为很容易顺手加一句"四元数必须逐位相等"的断言，然后在某次
+    标定重跑后莫名其妙地红 —— 那不是回归，是符号约定。
+    """
+    R = _rot("z", 100.0)
+    q1 = np.array(R_to_quat_xyzw(R))
+    q2 = np.array(R_to_quat_xyzw(R))
+    assert np.allclose(q1, q2)
+    assert np.allclose(quat_xyzw_to_R(-q1), quat_xyzw_to_R(q1))
+
+
+def test_launch_files_do_not_reimplement_the_quaternion_math() -> None:
+    """launch 文件里不许再出现自己写的四元数换算。
+
+    这段换算曾在 4 个 launch 里各抄一份，而且**四份已经抄得不完全一样**
+    （退化分支的公式各不相同）。共用的那一份在 ``handeye_lib`` 里，有回环测试；
+    自己再写一份就等于加了一处没人测的数学。要用就 ``read_transform_yaml``。
+    """
+    launch_dir = _repo_root() / "src" / "arm_application" / "launch"
+    bad = [
+        f.name
+        for f in sorted(launch_dir.glob("*.launch.py"))
+        if "_quat_from_R" in f.read_text(encoding="utf-8")
+    ]
+    assert not bad, (
+        "这些 launch 自己实现了四元数换算："
+        + ", ".join(bad)
+        + "；请改用 arm_application.handeye_lib.read_transform_yaml"
+    )

src/arm_application/test/test_model_labels.py

@@ -39,6 +39,11 @@ from arm_application.model_labels import (
 
 def _make_onnx(path, names_json=None, nc=None, metadata=None):
     """造一个最小可读的 ONNX，用于测试 metadata 读取。"""
+    # onnx 只在真机环境（ros_flexiv）里装了，仿真环境（ros_gz_sim）没有。
+    # 缺它时**跳过**而不是失败：仿真环境里每次 pytest 都多十几条红字，
+    # 很快就没有人看测试结果了（"狼来了"），而真正需要 onnx 的用例
+    # 本来也没法在没有 onnx 的环境里验证。
+    pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）")
     import onnx
     from onnx import TensorProto, helper
 

src/arm_application/test/test_weights_layout.py

@@ -45,6 +45,11 @@ REPO = Path(__file__).resolve().parents[3]
 
 
 def _make_onnx(path: Path, names_json: str | None = None) -> Path:
+    # onnx 只在真机环境（ros_flexiv）里装了，仿真环境（ros_gz_sim）没有。
+    # 缺它时**跳过**而不是失败：仿真环境里每次 pytest 都多十几条红字，
+    # 很快就没有人看测试结果了（"狼来了"），而真正需要 onnx 的用例
+    # 本来也没法在没有 onnx 的环境里验证。
+    pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）")
     import onnx
     from onnx import TensorProto, helper