仿真感知：真值孔位 + 真值相机 + 补 /home，让流程不改代码就能跑仿真

三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：

1. 真值孔位（truth_holes.py）：把 scene.yaml 与模型 SDF 里已知的孔几何
   直接投影到相机像素坐标，替代 HoughCircles。perception.source 选：
     yolo / truth / external —— truth 就走这条路。
   为什么不用 Hough：仿真里孔是"方孔 + 贴片亮环"的近似（SDF 没有布尔运算），
   靠图像门槛去凑 radus 区间很脆；而孔位本来就是场景里已知量，直接投影
   既准又稳，还能当真实检测算法的对照基线。

2. 真值相机（sim_truth_perception.py）：同理，直接算物体位姿，供
   visual_servo/filtered_point 用，绕开 ONNX。

3. 补 /home 与 set_named_pose 服务（sim_home_service.py）：真机上这两个
   由 Flexiv 驱动提供，仿真里没有 —— 缺了 S01 直接超时。

4. sim_tactile_adapter 容错导入 ros_gz_interfaces：那一路只是"接触点"，
   力本身走 geometry_msgs/Wrench。typesupport 加载不了时退化成
   contact 恒 False + centroid 恒 0，三力判据（S07 用的）不受影响 ——
   宁可用退化模式跑，也不要整个适配器起不来。

5. repo_root 解析（已装包要能找回源码仓库根，见 test_repo_root.py 的用例）
   与 use_sim_time 向子 launch 的传递。

另：scripts/gen_sim_handeye.py 把 URDF 真值导出手眼文件（仿真不标定）。

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

devices/sim/gazebo/config/cameras.yaml

@@ -44,8 +44,13 @@
 cameras:
   # --- 腕上相机（eye-in-hand），挂在机械臂法兰上 ------------------------------
   - gz_ns: /d405          # Gazebo 传感器话题前缀（see arm.sim.sensors.xacro 的 <topic>）
-    ros_ns: d405          # ROS 侧命名空间。应用按 camera_name 参数用这个名字取话题，
+    # ROS 侧命名空间。应用按 site.yaml → robots[].camera.name 取话题，所以这里
-                          # 所以它必须等于启动应用时给的 camera_name（默认 d405）
+    # 必须**等于那一项**：仿真那台在 site.yaml 里登记为 robots[] 的 `sn: sim`，
+    # 它的 camera.name 就是这里写的 sim_d405。
+    # 曾经这里写的是 d405，而 site 写的是 d405_left —— 后果是通止规装配在仿真里
+    # S00 报"腕部相机无图像"（图像 13Hz 好好地流着），人只会去怀疑相机。
+    # 改这里的名字，site.yaml 里那个 sn: sim 条目的 camera.name 必须同步改。
+    ros_ns: sim_d405
     link_frame: sim_d405_link
     # 图像上盖的坐标系必须是**光学系**，不能是本体系。
     # gz 与 URDF 里的相机本体遵循"X 朝前 / Y 朝左 / Z 朝上"（相机机身约定），
@@ -78,9 +83,14 @@ cameras:
   #    world → d435_link。位姿从 world 文件里**解析**得到（不在这里重复配置），
   #    避免"改了一处忘了另一处"。
   - gz_ns: /d435
-    ros_ns: d435
+    # ROS 侧命名空间 = sim_d435（与真机 d435 分开）。为什么不能沿用 d435：
-    link_frame: d435_link
+    # eye-to-hand 标定文件是按**相机名**命名的（handeye/results/<相机名>_eye_to_hand.yaml），
-    optical_frame: d435_color_optical_frame
+    # 仿真自动生成的这份若叫 d435_...，会和真机的手外标定**抢同一个文件名** ——
+    # 后果是"跑一次仿真把真机的标定覆盖了"，而标定文件平时没人看。
+    # frame 名也必须跟着改（默认 <ros_ns>_color_optical_frame，应用按这个查 TF）。
+    ros_ns: sim_d435
+    link_frame: sim_d435_link
+    optical_frame: sim_d435_color_optical_frame
     depth: true
     horizontal_fov: 1.5184
     width: 640

devices/sim/gazebo/config/scene.yaml

@@ -50,7 +50,7 @@ table_bounds:
 #   详见 README 的"仿真 ≠ 真机"一节，那里写了决策点与影响面。
 operating:
   # 用哪台相机做孔检测（决定用哪组内参算像素尺度）
-  camera: d405
+  camera: sim_d405
   # 哪个物体是"工件" —— 相机到工件的距离以**它的上表面**为基准。
   # 这里写的是物体的 name（不是模型名）。
   part_object: workpiece

devices/sim/gazebo/config/tactile.yaml

@@ -94,11 +94,19 @@ sides:
     # 力传感器所在的坐标系（即 *_inner_finger_joint 的 child link）。
     # 接触点会转到这个系里，用来算 centroid_u/v，也是轴的参照系。
     frame: sim_left_finger_mount
+    # 指腹内侧的"夹持点"（= tool center point）。**上料脚本要用它**：
+    # 两指 TCP 的中点就是工具轴，规尖垂在这条轴线上。
+    # 与 frame 的区别：frame 是力传感器的参照系（指尖关节的 child link），
+    # TCP 在它的子链再往下 0.036m（见 flexiv_description 的 finger_tcp_joint）。
+    # 名字不能靠 frame 加后缀推 —— 实测这两条链分别是
+    # sim_left_finger_mount / sim_left_finger_tcp（父链与子链并列命名）。
+    tcp_frame: sim_left_finger_tcp
 
   - side: right
     wrench_topic: /tactile/right/wrench
     contact_topic: /tactile/right/raw
     frame: sim_right_finger_mount
+    tcp_frame: sim_right_finger_tcp
 
 # --- 轴向映射（已实测，不是猜的）----------------------------------------
 # 力传感器挂在指尖关节上，给出的是该关节 child link 坐标系里的三向力。

devices/sim/gazebo/urdf/arm.sim.sensors.xacro

@@ -45,7 +45,7 @@
       d405_y:=-0.06
       d405_z:=0.05
       d405_roll:=0.0
-      d405_pitch:=0.0
+      d405_pitch:=-1.5708
       d405_yaw:=0.0
       image_width:=848
       image_height:=480
@@ -93,13 +93,32 @@
     <!-- =======================================================================
          RealSense D405 —— 眼在手上
          =======================================================================
-         位姿默认值说明（法兰坐标系，z 朝法兰外侧）：
+         位姿默认值说明（法兰坐标系）：
-             z=+0.05  相机略微伸出法兰，避免看到夹爪本体
+             xyz: x=0, y=-0.06, z=+0.05
-             y=-0.06  偏置到一侧，与真机上的支架安装位置一致
+                  z=+0.05 相机略微伸出法兰，避免看到夹爪本体
+                  y=-0.06 偏置到一侧，与真机上的支架安装位置一致
+             rpy: roll=0, **pitch=-1.5708(-90°)**, yaw=0
          这些值**不是**真机的标定结果，只是仿真里的合理起始位。
          仿真里"手眼关系"本来就精确已知（我们直接定义了相机的安装位姿），
          所以不需要跑手眼标定 —— 但保留了可调位姿，方便模拟"标定不准"的情况，
          验证跟踪算法对误差的容忍度。
+
+         ⚠️⚠️ pitch 这一项是**必须**的，不是"随手调的角度"。原因在 Gazebo 的
+         相机惯例：**传感器/模型的视轴是 +X**（不是 +Z）。世界相机 d435 就是靠
+         在 world 文件里给它模型 pitch=50° 才瞄准台面的 —— 同一个惯例。
+         腕相机要在法兰垂直朝下时看工件，就得让 d405_link 的 +X 对上法兰的 +Z，
+         即绕 Y 转 -90°。
+
+         为什么"少转这 90°"极难发现（曾经默认 0，踩过）：
+             · 话题有数据、预览有图像、camera_info 内参也对；
+             · 深度反投影甚至"通过"了检查 —— 因为它看到的是**远处的地面**，
+               反投影出来同样是 z≈0 的水平面（check_sim_cameras 当时只看
+               "命中不用平面"，判据太松，现在已改成核对视轴朝向）；
+             · 症状只出现在应用侧：S04 跟踪里真值/检测永远说"目标不在相机前方"，
+               伺服一直"寻找工件"原地转圈 —— 看起来像算法或标定问题。
+         所以：改这里之前先想清楚视轴指向，改完跑
+             python3 scripts/check_sim_cameras.py
+         它会核对"腕部相机光学系的视轴是否与法兰 +Z 一致"。
          ======================================================================= -->
     <xacro:if value="${d405_enable}">
       <link name="${prefix}d405_link"/>

handeye/results/sim_d435_eye_to_hand.yaml

@@ -0,0 +1,11 @@
+# 仿真手眼真值 —— **自动生成，不要手改**
+# mode: eye_to_hand
+# 来源: Gazebo/URDF 的 TF 真值 `sim_base_link` ← `sim_d435_color_optical_frame`（仿真不需要标定）
+# 生成: 2026-09-24T13:10:21+0800  生成器: scripts/gen_sim_handeye.py
+# 仿真台：手外相机固定在工位/世界上（不随臂动）
+translation_m: [-0.45000000, 0.00000000, 1.35000000]
+T_4x4:
+  - [-0.00000236, -0.76604704, 0.64278451, -0.45000000]
+  - [-1.00000000, 0.00000000, -0.00000367, 0.00000000]
+  - [0.00000281, -0.64278451, -0.76604704, 1.35000000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/sim_eye_in_hand.yaml

@@ -0,0 +1,11 @@
+# 仿真手眼真值 —— **自动生成，不要手改**
+# mode: eye_in_hand
+# 来源: Gazebo/URDF 的 TF 真值 `sim_flange` ← `sim_d405_color_optical_frame`（仿真不需要标定）
+# 生成: 2026-09-24T13:10:21+0800  生成器: scripts/gen_sim_handeye.py
+# 仿真台：腕部相机装在法兰上，位姿来自 URDF
+translation_m: [0.00000000, -0.06000000, 0.05000000]
+T_4x4:
+  - [0.00000000, 1.00000000, -0.00000000, 0.00000000]
+  - [-1.00000000, 0.00000000, -0.00000367, -0.06000000]
+  - [-0.00000367, 0.00000000, 1.00000000, 0.05000000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

scripts/check_sim_cameras.py

@@ -54,11 +54,11 @@ except ImportError as exc:  # pragma: no cover - 环境问题，不是逻辑问
 
 
 # 默认期望：哪台相机"必须"看见哪个平面。
-#   d435 是手外相机，它的职责就是看见台面（P6 的粗定位要靠它），
+#   sim_d435 是手外相机，它的职责就是看见台面（P6 的粗定位要靠它），
 #        所以要求 ≥10% 的像素落在台面上。
-#   d405 是腕上相机，视野窄且会拍到机械臂自己，台面占比要求放宽到 5%。
+#   sim_d405 是腕上相机（仿真那台 = 左臂），视野窄且会拍到机械臂自己，台面占比放宽到 5%。
 #   地面(z=0) 不作为要求：它被拍到多少取决于姿态，不构成"对不对"的判据。
-DEFAULT_EXPECT = ("d435:0.40:10", "d405:0.40:5")
+DEFAULT_EXPECT = ("sim_d435:0.40:10", "sim_d405:0.40:5")
 
 
 def quat_to_R(q) -> np.ndarray:
@@ -115,6 +115,95 @@ def spin_until(node: Node, pred, timeout: float) -> bool:
     return False
 
 
+def _tf_rot(node: "Grab", parent: str, child: str) -> np.ndarray:
+    """TF parent ← child 的旋转部分（child 的三根轴在 parent 系里的表示）。"""
+    tf = node.buf.lookup_transform(parent, child, rclpy.time.Time())
+    r = tf.transform.rotation
+    return quat_to_R((r.x, r.y, r.z, r.w))
+
+
+def mount_problems(node: "Grab", ns: str, frame: str, problems: list[str]) -> None:
+    """核对**安装朝向**：腕相机视轴要沿法兰轴线，手外相机要朝下看台面。
+
+    为什么单独查这一条（光看"命中平面"不够）：
+    腕相机要是朝侧面看，它照样能反投影出 z≈0 的地面 —— 几何自洽、数值"正常"，
+    只是**看不见工件**。表现全在应用侧："目标不在相机前方"、伺服一直找工件。
+    所以这里查的不是"看见了什么"，而是"朝哪看"：前者会被姿态带偏，后者不会。
+    """
+    try:
+        from arm_sim_gazebo.sim_cameras import (
+            MOUNT_TOL_DEG,
+            WORLD_LOOK_DOWN_COS,
+            flange_frame_for,
+            load_cameras,
+            mount_axis_deg,
+            world_look_down_cos,
+        )
+    except ImportError as exc:
+        # 不静默跳过：安装朝向是本工具**最重要**的一条判据（它拦的是"相机装歪
+        # 但数值全正常"那类问题），悄悄不查等于把这条守门人撤了。
+        problems.append(
+            f"拿不到 sim_cameras 的安装朝向工具（{exc}）—— install_sim 是旧的，"
+            f"先跑 bash scripts/build_sim.sh"
+        )
+        return
+    spec = next((s for s in load_cameras() if s.ros_ns == ns), None)
+    if spec is None:
+        problems.append(f"cameras.yaml 里找不到 ros_ns={ns} 的相机声明，无法核对安装朝向")
+        return
+    now = rclpy.time.Time()
+    flange = flange_frame_for(spec)
+    if flange is not None and node.buf.can_transform(flange, frame, now):
+        R = _tf_rot(node, flange, frame)
+        ang = mount_axis_deg(R)
+        print(f"        相对法兰 {flange} 的视轴夹角: {ang:.1f}°  （0° = 正装在法兰轴线上）")
+        if ang > MOUNT_TOL_DEG:
+            problems.append(
+                f"腕部相机视轴与法兰 +Z 差 {ang:.1f}°（要求 ≤{MOUNT_TOL_DEG:.0f}°）—— "
+                f"相机装歪了，法兰垂直朝下时它看不到下面的工件。\n"
+                f"      90° 左右就是「少转一个直角」：话题/图像/内参全都正常，\n"
+                f"      而且反投影仍会命中地面（看的是远处），所以只查平面发现不了。\n"
+                f"      修法：devices/sim/gazebo/urdf/arm.sim.sensors.xacro 里"
+                f" d405_pitch（正装 = -1.5708）。"
+            )
+    else:
+        cos = world_look_down_cos(_tf_rot(node, "world", frame))
+        print(f"        视轴在世界系里的 z 分量: {cos:+.2f}  （-1 = 正朝下看）")
+        if cos > WORLD_LOOK_DOWN_COS:
+            problems.append(
+                f"手外相机没在朝下看台面（视轴 z 分量 {cos:+.2f}，要求 ≤{WORLD_LOOK_DOWN_COS:+.1f}）—— "
+                f"位姿 rpy 写错时几何自洽、不报错，只是拍不到工件。\n"
+                f"      修法：devices/sim/gazebo/worlds/assembly.world 里该 <model> 的 <pose> pitch。"
+            )
+
+
+def looking_at_table(node: "Grab", frame: str) -> "bool | None":
+    """相机此刻是不是**朝下看着台面**（光心在台面 xy 内 + 视轴朝下）。
+
+    为什么要问这一句：腕相机的职责是"悬在工件上方时看清螺纹孔"。机械臂停在初始位
+    或别处时它当然只看得见地面（初始位那副姿态里法兰甚至是横着的）—— 这时候拿
+    "命中台面 ≥5%"判它不合格是**假警报**，而假警报多了真问题就没人看了。
+    拿不到场景配置时返回 None（按"不知道"处理，照常判）。
+    """
+    try:
+        from arm_sim_gazebo.sim_cameras import WORLD_LOOK_DOWN_COS, world_look_down_cos
+        from arm_sim_gazebo import sim_scene
+
+        scene = sim_scene.load_scene()
+        b = scene.table_bounds
+        tf = node.buf.lookup_transform("world", frame, rclpy.time.Time())
+        x = tf.transform.translation.x
+        y = tf.transform.translation.y
+        margin = 0.05
+        inside = (b.x_min - margin <= x <= b.x_max + margin) and (
+            b.y_min - margin <= y <= b.y_max + margin
+        )
+        downward = world_look_down_cos(_tf_rot(node, "world", frame)) <= WORLD_LOOK_DOWN_COS
+        return inside and downward
+    except Exception:  # noqa: BLE001 —— 拿不到就不拦人
+        return None
+
+
 def expected_intrinsics(ns: str):
     """从 cameras.yaml 算出该相机的应有内参。拿不到就返回 None（不阻断检查）。"""
     try:
@@ -210,6 +299,9 @@ def check_one(ns: str, args) -> tuple[bool, list[str]]:
         ]
     )
     zw = (pts @ R.T + t)[:, 2]
+    # 安装朝向要在 destroy_node 之前查 —— 它还要用 TF（腕相机的视轴相对法兰）
+    mount_problems(node, ns, frame, problems)
+    above_table = looking_at_table(node, frame)
     node.destroy_node()
 
     print(f"\n[{ns}]  光学系={frame}   fx={fx:.1f} fy={fy:.1f} cx={cx:.1f} cy={cy:.1f}")
@@ -223,15 +315,26 @@ def check_one(ns: str, args) -> tuple[bool, list[str]]:
         print(f"        命中 {name}(z={pz:+.2f}) : {rate:5.1f}%")
 
     # 期望判据
+    table_z = 0.40
     for want_z, want_min in args.expect.get(ns, []):
         name = dict(args.planes).get(want_z, f"z={want_z}")
         got = rates.get(want_z, 0.0)
-        if got < want_min:
+        if got >= want_min:
-            problems.append(
+            continue
-                f"只看见 {name} 的 {got:.1f}%，要求 ≥{want_min:.0f}%。\n"
+        if abs(want_z - table_z) < 1e-9 and above_table is False:
-                f"      相机很可能是**背对**目标（位姿 rpy 写错）—— "
+            # 腕相机不在台面上方时，"没看见台面"是正常的（它看见的是地面）。
-                f"这种错几何自洽、不报错，只会让相机拍不到该拍的东西。"
+            print(
+                f"        ⚠ 跳过「{name}」判据：相机此刻没朝下看着台面"
+                f"（光心不在 scene.yaml 的 table_bounds 内，或视轴没朝下）——\n"
+                f"          此刻只看得见地面是正常的。\n"
+                f"          想让这条生效：把机械臂摆到工件上方（例如 S04 跟踪时）再跑本脚本。"
             )
+            continue
+        problems.append(
+            f"只看见 {name} 的 {got:.1f}%，要求 ≥{want_min:.0f}%。\n"
+            f"      相机很可能是**背对**目标（位姿 rpy 写错）—— "
+            f"这种错几何自洽、不报错，只会让相机拍不到该拍的东西。"
+        )
 
     total = sum(rates.values())
     if total < 1.0:
@@ -279,7 +382,7 @@ def main() -> int:
         description="仿真相机几何验收：反投影深度，检查是否命中预期平面。",
         formatter_class=argparse.RawDescriptionHelpFormatter,
     )
-    ap.add_argument("--cameras", default="d405,d435", help="要检查的相机命名空间，逗号分隔")
+    ap.add_argument("--cameras", default="sim_d405,sim_d435", help="要检查的相机命名空间，逗号分隔")
     ap.add_argument(
         "--planes",
         type=parse_planes,

scripts/check_sim_scene.py

@@ -125,8 +125,23 @@ def effective_gates(root: Optional[Path] = None) -> dict:
     }
 
 
+def sim_robot_servo(doc: dict) -> dict:
+    """仿真那台臂的 servo 段（顶层 servo 被 robots[] 里 sim 那台覆盖后的结果）。
+
+    ⚠️ 必须合并，不能只读顶层：顶层那套是**真机**的（台面与基座同高，所以
+    工作盒 z 是 0.18~0.65），而仿真里机械臂站在地面、台面 0.40 高，一切 Z 都
+    大 0.412。只读顶层会拿真机的界去核对仿真的悬停高 —— 看起来"过了"，
+    而真正生效的配置可能根本不对（或者反过来，明明配对了却报越界）。
+    """
+    servo = dict(doc.get("servo") or {})
+    for robot in (doc.get("robot") or {}).get("robots") or []:
+        if str(robot.get("arm") or "") == "sim" or str(robot.get("sn") or "") == "sim":
+            servo.update(robot.get("servo") or {})
+    return servo
+
+
 def workspace_z_limits(root: Optional[Path] = None):
-    """从 site.yaml 读机械臂法兰的工作空间 Z 上下界（用于核对悬停高度）。"""
+    """机械臂法兰的工作空间 Z 上下界（按**仿真那台臂**的生效值）。"""
     import yaml
 
     base_root = Path(root) if root is not None else REPO_ROOT
@@ -136,7 +151,7 @@ def workspace_z_limits(root: Optional[Path] = None):
     if not site.is_file():
         return None
     doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
-    servo = doc.get("servo") or {}
+    servo = sim_robot_servo(doc)
     try:
         return float(servo["workspace_xyz_min"][2]), float(servo["workspace_xyz_max"][2])
     except (KeyError, IndexError, TypeError, ValueError):
@@ -217,6 +232,25 @@ def check_hover_height(scene, root) -> list[str]:
         )
     elif fz < lo:
         out.append(f"算出的悬停法兰高度 z≈{fz:.3f}m 低于工作空间下界 {lo}m。")
+
+    # 一阶段跟踪高还要再抬 TRACK_CLEARANCE_M（见 pipeline_node 的常量）。
+    # 单独查这一条的原因：二阶段能过、S04 一跟踪就被"超出工作盒"截断，
+    # 现象是"跟踪位置总差一截/越跟踪越偏"，而配置看着完全合理。
+    try:
+        from arm_application.pipeline_node import TRACK_CLEARANCE_M
+    except Exception:  # noqa: BLE001 —— 拿不到常量就不查，不因此拦人
+        return out
+    track = fz + float(TRACK_CLEARANCE_M)
+    if track > hi:
+        out.append(
+            f"一阶段跟踪高 = 悬停 {fz:.3f} + {TRACK_CLEARANCE_M:.2f} = {track:.3f}m "
+            f"**超出工作空间上界 {hi}m**。\n"
+            f"      二阶段的位姿检查能过、S04 一跟踪却会被「超出工作盒」截断 —— "
+            f"现象是「越跟踪越偏」，很难往配置上想。\n"
+            f"      修法（仿真里都自由）：把 site.yaml 里仿真那台臂的 "
+            f"servo.workspace_xyz_max.z 抬到 ≥ {track:.3f}，"
+            f"或调小 scene.yaml 的 camera_to_part_m（法兰随之降低）。"
+        )
     return out
 
 

scripts/gen_sim_handeye.py

@@ -0,0 +1,292 @@
+#!/usr/bin/env python3
+"""把仿真里的**手眼真值**导出成和真机同格式的手眼文件。
+
+为什么要有它
+------------
+真机的手眼标定是"标出来"的（``run_handeye.sh``）。仿真里不用标：腕部相机在
+URDF 里就固定在法兰上，位姿**本来就知道**，而且就是真值。但应用读的是
+``handeye/results/*.yaml`` 这个接口（``servo_node`` 需要 ``T_flange_cam``
+矩阵，``visual_servo.launch.py`` 缺文件会直接拒绝启动）。
+
+所以这里的做法不是"让应用判断自己在不在仿真"，而是**把真值导出成同一格式的
+文件**，路径在 ``site.yaml`` 的仿真台条目里指定。应用一行都不用改。
+
+同时导出两个（真机上对应两次标定）::
+
+    sim_eye_in_hand.yaml     flange ← 腕部相机光学系（eye-in-hand，跟踪/精定位用）
+    sim_d435_eye_to_hand.yaml  base ← 手外相机光学系（eye-to-hand，S03 粗定位用）
+
+用法::
+
+    python3 scripts/gen_sim_handeye.py                # 生成/刷新（仿真栈要在跑）
+    python3 scripts/gen_sim_handeye.py --check        # 只校验：现有文件是否仍是当前真值
+    source scripts/sim_env.sh && python3 scripts/gen_sim_handeye.py
+
+为什么要 ``--check``：URDF 一改（相机装到别处、加了垫片），那份文件就过期了，
+而过期的表现是"定位总能到附近、永远差一点"—— 看起来像算法不准。
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import os
+import sys
+import tempfile
+import time
+from pathlib import Path
+from typing import Optional, Sequence
+
+import numpy as np
+
+_HERE = Path(__file__).resolve()
+sys.path.insert(0, str(_HERE.parent))          # scripts/
+sys.path.insert(0, str(_HERE.parent / "lib"))  # scripts/lib/
+
+import export_site_env as site_env  # noqa: E402
+from arm_application.handeye_lib import R_to_quat_xyzw  # noqa: E402
+
+try:
+    import rclpy
+    from rclpy.node import Node
+    from rclpy.time import Time
+    from tf2_ros import Buffer, TransformListener
+except ImportError as exc:  # pragma: no cover - 只在没 source 环境时触发
+    sys.exit(
+        f"找不到 {exc.name}：先 source 仿真环境再跑本脚本\n"
+        "    source scripts/sim_env.sh\n"
+        "若仍失败，说明 install_sim/ 还没构建：bash scripts/build_sim.sh"
+    )
+
+#: 认为"文件与真值一致"的容差。1e-9 m 太苛刻（YAML 打印会截断），
+#: 1e-6 m = 1 微米、1e-6 rad 级别足够：相机装配误差远大于它。
+TOL_M = 1e-6
+TOL_RAD = 1e-6
+
+
+class HandeyeError(RuntimeError):
+    """面向使用者的可预期失败。"""
+
+
+def _matrix_to_yaml(T: np.ndarray) -> str:
+    rows = []
+    for i in range(4):
+        vals = ", ".join(f"{float(v):.8f}" for v in T[i])
+        rows.append(f"  - [{vals}]")
+    return "\n".join(rows)
+
+
+def _render(
+    *, mode: str, parent: str, child: str, T: np.ndarray, note: str
+) -> str:
+    xyz = T[:3, 3]
+    return (
+        f"# 仿真手眼真值 —— **自动生成，不要手改**\n"
+        f"# mode: {mode}\n"
+        f"# 来源: Gazebo/URDF 的 TF 真值 `{parent}` ← `{child}`（仿真不需要标定）\n"
+        f"# 生成: {time.strftime('%Y-%m-%dT%H:%M:%S%z')}  生成器: scripts/gen_sim_handeye.py\n"
+        f"# {note}\n"
+        f"translation_m: [{float(xyz[0]):.8f}, {float(xyz[1]):.8f}, {float(xyz[2]):.8f}]\n"
+        f"T_4x4:\n{_matrix_to_yaml(T)}\n"
+    )
+
+
+class _TfGrab(Node):
+    """只做一件事：等到能查到某对坐标系，然后把 4x4 拿出来。"""
+
+    def __init__(self, timeout_s: float) -> None:
+        super().__init__("gen_sim_handeye")
+        self._buf = Buffer()
+        self._listener = TransformListener(self._buf, self)
+        self._timeout = timeout_s
+
+    def look(self, parent: str, child: str) -> np.ndarray:
+        deadline = time.monotonic() + self._timeout
+        last = ""
+        while time.monotonic() < deadline:
+            self._spin(0.1)
+            try:
+                # 用 Time()（=最新可用）而不是 Time(0)：0 在 ROS 里也表示最新，
+                # 但语义上容易被读成"仿真时刻 0"，这里写明意图。
+                tf = self._buf.lookup_transform(parent, child, Time())
+            except Exception as exc:  # noqa: BLE001
+                last = f"{type(exc).__name__}: {exc}"
+                continue
+            return _tf_to_matrix(tf)
+        raise HandeyeError(
+            f"{self._timeout:.0f}s 内查不到 TF `{parent}` ← `{child}`：{last}\n"
+            "  仿真栈没起来？先 bash scripts/run_sim_gz.sh\n"
+            "  名字对不上？site.yaml 的仿真台条目里 ee_frame / camera.name 要与 URDF 一致"
+        )
+
+    def _spin(self, dt: float) -> None:
+        rclpy.spin_once(self, timeout_sec=dt)
+
+
+def _tf_to_matrix(tf) -> np.ndarray:
+    from arm_application.handeye_lib import quat_xyzw_to_R
+
+    t = tf.transform.translation
+    q = tf.transform.rotation
+    T = np.eye(4)
+    T[:3, :3] = quat_xyzw_to_R([q.x, q.y, q.z, q.w])
+    T[:3, 3] = [t.x, t.y, t.z]
+    return T
+
+
+def _read_matrix(path: Path) -> Optional[np.ndarray]:
+    """读回文件里的 T_4x4（与 ``handeye_lib.read_transform_yaml`` 同一份解析）。"""
+    from arm_application.handeye_lib import read_transform_yaml
+
+    if not path.is_file():
+        return None
+    xyz, quat = read_transform_yaml(str(path))
+    from arm_application.handeye_lib import quat_xyzw_to_R
+
+    T = np.eye(4)
+    T[:3, :3] = quat_xyzw_to_R(list(quat))
+    T[:3, 3] = list(xyz)
+    return T
+
+
+def _same(a: np.ndarray, b: np.ndarray) -> bool:
+    if not np.allclose(a[:3, 3], b[:3, 3], atol=TOL_M):
+        return False
+    # 旋转差用"作用到单位向量上的最大偏差"衡量，比逐元素比更直观
+    R = a[:3, :3].T @ b[:3, :3]
+    ang = math.acos(max(-1.0, min(1.0, (float(np.trace(R)) - 1.0) / 2.0)))
+    return ang <= TOL_RAD
+
+
+def _write_atomic(path: Path, text: str) -> None:
+    """原子落盘：脚本/节点可能正读这个文件，写一半会被读到半截。"""
+    path.parent.mkdir(parents=True, exist_ok=True)
+    fd, tmp = tempfile.mkstemp(dir=str(path.parent), prefix=path.name, suffix=".tmp")
+    try:
+        with os.fdopen(fd, "w", encoding="utf-8") as f:
+            f.write(text)
+            f.flush()
+            os.fsync(f.fileno())
+        os.replace(tmp, path)
+    except BaseException:
+        Path(tmp).unlink(missing_ok=True)
+        raise
+
+
+def _targets(arm: str) -> Sequence[tuple[str, str, str, str, Path]]:
+    """返回 (mode, parent, child, note, 输出路径)。
+
+    parent/child 全部从 site.yaml 现取 —— 名字写在配置里，脚本不猜。
+    """
+    data = site_env.load_site()
+    # 注意第二个返回值才是"这台臂"自己的字典（含 sn / base_frame / ee_frame / camera）；
+    # 第一个是整份 site，顶层没有 sn（顶层的是 robot.sn = 默认臂）。
+    try:
+        _merged, robot = site_env.apply_active_arm(data, arm)
+    except ValueError as exc:
+        raise HandeyeError(str(exc)) from exc
+    sn = str(robot.get("sn") or "")
+    if not sn:
+        raise HandeyeError(f"site.yaml 里选不出机械臂（--arm {arm}）")
+
+    prefix = f"{sn}_"
+    # 关节/坐标系命名沿用应用的口径：base/ee 未显式配就用 <sn>_ 前缀（见 stage2）
+    base = str(robot.get("base_frame") or f"{prefix}base_link")
+    ee = str(robot.get("ee_frame") or f"{prefix}flange")
+    cam = str((robot.get("camera") or {}).get("name") or "").strip().strip("/")
+    if not cam:
+        raise HandeyeError(f"`--arm {arm}` 的 camera.name 是空的，没法确定腕部相机")
+
+    # 手外相机：与 gauge_assembly_node._d435_ns 同一套判据（role 前缀 + 同臂）
+    coarse = ""
+    arm_name = str(robot.get("arm") or "").strip()
+    for cfg in (data.get("cameras") or {}).values():
+        if not isinstance(cfg, dict):
+            continue
+        if not str(cfg.get("role") or "").startswith("overhead"):
+            continue
+        if arm_name and str(cfg.get("arm") or "").strip() != arm_name:
+            continue
+        coarse = str(cfg.get("name") or "").strip().strip("/")
+        break
+
+    root = site_env._root()
+    out = [
+        (
+            "eye_in_hand",
+            ee,
+            f"{cam}_color_optical_frame",
+            f"{note_who(robot)}：腕部相机装在法兰上，位姿来自 URDF",
+            root / "handeye" / "results" / f"{sn}_eye_in_hand.yaml",
+        )
+    ]
+    if coarse:
+        out.append(
+            (
+                "eye_to_hand",
+                base,
+                f"{coarse}_color_optical_frame",
+                f"{note_who(robot)}：手外相机固定在工位/世界上（不随臂动）",
+                root / "handeye" / "results" / f"{coarse}_eye_to_hand.yaml",
+            )
+        )
+    return out
+
+
+def note_who(merged: dict) -> str:
+    return str(merged.get("label") or merged.get("sn") or "仿真台")
+
+
+def main(argv: Optional[Sequence[str]] = None) -> int:
+    ap = argparse.ArgumentParser(description="把仿真手眼真值导出成手眼文件")
+    ap.add_argument("--arm", default="sim", help="site.yaml 里仿真台的别名（默认 sim）")
+    ap.add_argument("--check", action="store_true", help="只校验，不写文件")
+    ap.add_argument("--timeout", type=float, default=20.0, help="等 TF 出现的秒数")
+    args = ap.parse_args(argv)
+
+    try:
+        targets = _targets(args.arm)
+    except HandeyeError as exc:
+        print(f"ERROR: {exc}", file=sys.stderr)
+        return 2
+
+    rclpy.init()
+    node = _TfGrab(args.timeout)
+    rc = 0
+    try:
+        for mode, parent, child, note, path in targets:
+            try:
+                T = node.look(parent, child)
+            except HandeyeError as exc:
+                print(f"FAIL: [{mode}] {exc}", file=sys.stderr)
+                rc = 1
+                continue
+            old = _read_matrix(path)
+            if args.check:
+                if old is None:
+                    print(f"FAIL: [{mode}] 文件不存在：{path}")
+                    rc = 1
+                elif _same(old, T):
+                    print(f"OK:   [{mode}] {path.name} 与当前真值一致")
+                else:
+                    d = float(np.linalg.norm(old[:3, 3] - T[:3, 3]))
+                    print(
+                        f"FAIL: [{mode}] {path.name} 已过期：平移差 {d * 1000:.3f}mm"
+                        f"（URDF 改过？重跑本脚本刷新）"
+                    )
+                    rc = 1
+                continue
+            if old is not None and _same(old, T):
+                print(f"OK:   [{mode}] {path} 已是最新（无需重写）")
+                continue
+            _write_atomic(path, _render(mode=mode, parent=parent, child=child, T=T, note=note))
+            print(f"写入: [{mode}] {path}  ({parent} ← {child})")
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    return rc
+
+
+if __name__ == "__main__":
+    sys.exit(main())

src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py

@@ -441,6 +441,78 @@ def depth_m_to_mm16(depth_m: np.ndarray, max_mm: int = MAX_MM) -> np.ndarray:
     return out
 
 
+#: 腕部相机的视轴与法兰轴线的允许夹角 [度]。
+#: 真机装支架时也不会完全正装，但"看得到工件"要求它基本沿法兰轴；
+#: 20° 是给手工装配留的余量，同时足以把"少转 90°"这类错误拦下来。
+MOUNT_TOL_DEG = 20.0
+
+#: 手外相机的视轴在世界系里的 z 分量上限（-1 = 正朝下）。松于 -0.5 就算没在朝下看。
+WORLD_LOOK_DOWN_COS = -0.5
+
+
+def flange_frame_for(spec: CameraSpec) -> Optional[str]:
+    """从相机的 link_frame 反推它所在臂的法兰 frame：sim_d405_link → sim_flange。
+
+    只对**挂在臂上**的相机有意义（``world_model`` 非空的是世界相机，返回 None）。
+    为什么要反推而不是读配置：``site.yaml`` 的 ``sim.joint_prefix`` 自己都写着
+    "✘ 未接线"，拿一个没接线的值当路径判据会静默失效。
+    link_frame 里的前缀才是真正生效的那个（由 xacro 的 arm_prefix 生成）。
+    """
+    if spec.world_model:
+        return None
+    model = spec.ros_ns.rsplit("_", 1)[-1]          # sim_d405 → d405
+    suffix = f"{model}_link"
+    if not spec.link_frame.endswith(suffix):
+        return None
+    return spec.link_frame[: -len(suffix)] + "flange"
+
+
+def mount_axis_deg(R_flange_optical: np.ndarray) -> float:
+    """光学系视轴（+Z）与**法兰 +Z**的夹角 [度]。0 = 正装在法兰轴线上。
+
+    这是个可以**脱离姿态**核对的量，所以特别有用：不管机械臂当时摆成什么样，
+    "相机有没有沿法兰轴线装"都只在安装处决定。
+
+    为什么它必须接近 0（而不是"调多少都行"）：Gz 的相机视轴是模型/连杆的 +X，
+    而 OPTICAL_RPY(-90°,0,-90°) 把"机身系（X 前/Y 左/Z 上）"转成光学系。
+    两者串起来 ⇒ 相机沿法兰轴线装 ⇔ 光学 +Z ∥ 法兰 +Z。
+    少转那 90° 时：话题有数据、预览有图像、内参也对，只是整个视野搬到了侧面，
+    应用侧表现成"目标永远不在相机前方"（曾真实踩过，见 sensors.xacro 的说明）。
+    """
+    v = np.asarray(R_flange_optical, dtype=float) @ np.array([0.0, 0.0, 1.0])
+    return float(np.degrees(np.arccos(float(np.clip(v[2], -1.0, 1.0)))))
+
+
+def world_look_down_cos(R_world_optical: np.ndarray) -> float:
+    """光学系视轴在世界系里的 z 分量（-1 = 正朝下看，0 = 平视）。"""
+    return float((np.asarray(R_world_optical, dtype=float) @ np.array([0.0, 0.0, 1.0]))[2])
+
+
+def rpy_to_R(rpy: Sequence[float]) -> np.ndarray:
+    """URDF/SDF 的 (roll, pitch, yaw) → 3x3 旋转矩阵。
+
+    复合顺序是 ``R = Rz(yaw)·Ry(pitch)·Rx(roll)``（URDF 规范）—— 顺序不能想当然，
+    反了在"只有一个角非零"时看不出来，两个角同时非零就会错得莫名其妙。
+    """
+    r, p, y = (float(v) for v in rpy)
+    cr, sr = math.cos(r), math.sin(r)
+    cp, sp = math.cos(p), math.sin(p)
+    cy, sy = math.cos(y), math.sin(y)
+    Rx = np.array([[1.0, 0.0, 0.0], [0.0, cr, -sr], [0.0, sr, cr]])
+    Ry = np.array([[cp, 0.0, sp], [0.0, 1.0, 0.0], [-sp, 0.0, cp]])
+    Rz = np.array([[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]])
+    return Rz @ Ry @ Rx
+
+
+def mount_rpy_axis_deg(rpy: Sequence[float]) -> float:
+    """腕相机的**安装 rpy**（URDF 里那个 fixed joint 的 rpy）→ 视轴与法兰 +Z 的夹角。
+
+    与运行期查 TF 的 :func:`mount_axis_deg` 是同一件事，只是输入不同：
+    没接 TF 时（比如核对 xacro 里的默认值）用这个算。
+    """
+    return mount_axis_deg(rpy_to_R(rpy) @ rpy_to_R(OPTICAL_RPY))
+
+
 def optical_rpy() -> Tuple[float, float, float]:
     """本体 → 光学系的固定旋转，供静态 TF 使用。"""
     return OPTICAL_RPY

src/arm_sim_gazebo/arm_sim_gazebo/sim_home_service.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+"""仿真里的 ``/home`` —— 把真机 Flexiv 驱动的**同名服务**补齐，契约一致。
+
+为什么需要它
+------------
+真机上 ``/home``（``std_srvs/Trigger``）由 ``arm_driver_flexiv`` 提供，
+它的实现就是"切工具系（可选）+ 转发给 ``/set_named_pose``"。
+仿真里没有 Flexiv 驱动，于是通止规装配（S01 回 home、S11 复位）在 Gazebo 里
+第一步就过不了自检 —— 而 S01 本身在两种环境下**没有区别**。
+
+所以这里的做法不是"让上层代码判断自己在不在仿真"，而是**补一份同样的契约**：
+``/home`` → ``/set_named_pose``（``arm_msgs/srv/SetNamedPose``）。
+
+有意保留的差异（写在这里，免得有人以为两边完全一样）：
+
+* 真机的 ``/home`` 会顺带切工具系（``switch_tool_on_home``）；
+  仿真里没有工具坐标系的概念，**不切**（S00 也把 ``/switch_tool`` 列为选检）。
+* 真机 ``/home`` 转发时用的 ``home_pose`` / ``home_speed`` 来自 ``site.yaml``
+  （``go_home.pose`` / ``go_home.speed``）；这里用同名参数，默认值与 site 一致，
+  launch 不必再传。
+
+用法（由 ``gazebo_sim.launch.py`` 拉起）::
+
+    ros2 service call /home std_srvs/srv/Trigger '{}'
+"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from std_srvs.srv import Trigger
+
+from arm_msgs.srv import SetNamedPose
+
+
+class SimHomeService(Node):
+    """``/home`` → ``/set_named_pose``。"""
+
+    def __init__(self) -> None:
+        super().__init__("sim_home_service")
+        # 与 site.yaml 的 go_home 段保持一致：仿真与真机用**同一份默认值**，
+        # 免得"仿真里回零是 4 秒、真机是 1 秒"这种口径差异。
+        self.declare_parameter("home_pose", "home")
+        self.declare_parameter("home_speed", 0.25)
+        self.declare_parameter("set_named_pose_service", "/set_named_pose")
+        #: 等 ``/set_named_pose`` 出现的超时 [s]。仿真栈刚起来时它可能还没就绪，
+        #: 直接失败会让人误以为"服务没接线"。
+        self.declare_parameter("wait_service_s", 5.0)
+        #: 等 `</set_named_pose` 回应的超时 [s]。回零是慢动作，别设太小。
+        self.declare_parameter("call_timeout_s", 60.0)
+
+        self._pose = str(self.get_parameter("home_pose").value or "home")
+        self._speed = float(self.get_parameter("home_speed").value or 0.25)
+        self._timeout = float(self.get_parameter("call_timeout_s").value)
+        # Reentrant + 多线程：本节点的回调里要**同步等**另一个服务的应答，
+        # 非重入的互斥组会自锁（真机驱动里踩过，注释同样留着）。
+        self._cb = ReentrantCallbackGroup()
+        self._named = self.create_client(
+            SetNamedPose,
+            str(self.get_parameter("set_named_pose_service").value),
+            callback_group=self._cb,
+        )
+        self.create_service(Trigger, "/home", self._on_home, callback_group=self._cb)
+        self.get_logger().info(
+            f"仿真 /home 就绪 → {self._named.srv_name} 位姿[{self._pose}] "
+            f"速度 {self._speed}（真机由 Flexiv 驱动提供同名服务）"
+        )
+
+    def _on_home(self, request: Trigger.Request, response: Trigger.Response):
+        wait_s = float(self.get_parameter("wait_service_s").value)
+        if not self._named.wait_for_service(timeout_sec=wait_s):
+            response.success = False
+            response.message = (
+                f"{self._named.srv_name} 不在 —— 命名位姿服务没起来。"
+                "仿真是 gazebo_sim.launch.py 里的 named_pose_server，"
+                "真机是 arm_bringup/stack.launch.py"
+            )
+            self.get_logger().error(response.message)
+            return response
+
+        req = SetNamedPose.Request()
+        req.pose_name = self._pose
+        req.velocity_scale = self._speed
+        fut = self._named.call_async(req)
+        deadline = time.monotonic() + self._timeout
+        while not fut.done():
+            if time.monotonic() > deadline:
+                response.success = False
+                response.message = f"回位姿[{self._pose}]跟丢：{self._timeout:.0f}s 内没结果"
+                self.get_logger().error(response.message)
+                return response
+            # 这里是**回调线程**（MultiThreadedExecutor），可以 sleep；
+            # 不能 spin（会与执行器抢同一节点的回调）。
+            time.sleep(0.02)
+        res = fut.result()
+        response.success = bool(res.success)
+        response.message = res.message or f"已回位姿[{self._pose}]"
+        (self.get_logger().info if response.success else self.get_logger().error)(
+            f"/home → {response.message}"
+        )
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = SimHomeService()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py

@@ -112,8 +112,23 @@ class SceneSpec:
 # 读配置
 # ---------------------------------------------------------------------------
 def repo_root() -> Path:
-    """仓库根。本模块在 src/arm_sim_gazebo/arm_sim_gazebo/ 下，向上走四层。"""
+    """仓库根。**实现只有一份**：``robot_description.repo_root()``。
-    return Path(__file__).resolve().parents[3]
+
+    以前这里是 ``Path(__file__).resolve().parents[3]`` —— 那在**源码树**里对，
+    在**安装后**是错的：``install_sim/`` 底下是**拷贝**（不是软链），
+    所以装出来的模块算出来的"仓库根"是 ``install_sim/arm_sim_gazebo/lib``，
+    而 ``devices/`` 根本不在那儿。现象是节点一启动就报
+    "找不到场景配置 .../install_sim/arm_sim_gazebo/lib/devices/sim/gazebo/config/scene.yaml"。
+
+    而**节点永远是从安装路径启动的** —— 也就是说 parents[N] 这种写法在
+    "脚本里手工跑"时看着没问题，一到正式启动就坏，属于最容易被漏掉的一类。
+
+    ``robot_description.repo_root()`` 处理了三种情形（``ROS2_WS`` 环境变量、
+    标记文件向上查找、cwd 向上查找），并且找不到时会说清怎么修。
+    """
+    from arm_sim_gazebo.robot_description import repo_root as _robust_root
+
+    return _robust_root()
 
 
 def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> SceneSpec:
@@ -170,7 +185,7 @@ def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> Scen
 
     op_raw = doc.get("operating") or {}
     operating = Operating(
-        camera=str(op_raw.get("camera") or "d405").strip().strip("/"),
+        camera=str(op_raw.get("camera") or "sim_d405").strip().strip("/"),
         camera_to_part_m=float(op_raw.get("camera_to_part_m") or 0.20),
         camera_offset_below_flange_m=float(op_raw.get("camera_offset_below_flange_m") or 0.0),
         part_object=str(op_raw.get("part_object") or "workpiece"),
@@ -758,6 +773,81 @@ def pose_mismatches(
     return out
 
 
+@dataclass(frozen=True)
+class SceneRing:
+    """工件上的一个嵌件亮环（模型里 ``<visual name="*_ring">`` 的几何）。
+
+    坐标是**物体局部系、单位米**（SDF 原生的量纲）：``z`` 是环心高度，
+    ``radius`` 是亮环外半径。用途有两个——投影成像素（要 x/y/z）和算像素半径
+    （要 radius）。
+
+    为什么单独建一个类型而不是返回元组：下游要按下标取值，写错不会报错，
+    只会安静地算偏 —— 而"孔位总偏一点"是最难归因的一类症状。
+    """
+
+    name: str
+    x: float
+    y: float
+    z: float
+    radius: float
+
+
+def _visual_geometry(tree: ET.ElementTree) -> Tuple[List[SceneRing], Dict[str, float]]:
+    """一次遍历解出（亮环列表, 孔半径表）。名字约定见 :func:`features_from_workpiece`。"""
+    rings: List[SceneRing] = []
+    bores: Dict[str, float] = {}
+    for vis in tree.iter("visual"):
+        name = vis.get("name") or ""
+        cyl = vis.find("geometry/cylinder")
+        if cyl is None:
+            continue                      # 板面是 box，不是"环/孔" —— 不看
+        r_el = cyl.find("radius")
+        if r_el is None or not r_el.text:
+            continue
+        try:
+            r = float(r_el.text)
+        except ValueError:
+            continue
+        if name.endswith("_ring"):
+            pose_el = vis.find("pose")
+            if pose_el is None or not pose_el.text:
+                # 没有 pose 就没有圆心。**宁可不收**，也不要拿 (0,0,0) 当圆心：
+                # 那会让"两孔间距"算成 0，看起来像"检测不到孔"，白查半天。
+                continue
+            parts = pose_el.text.split()
+            if len(parts) < 3:
+                continue
+            try:
+                x, y, z = float(parts[0]), float(parts[1]), float(parts[2])
+            except ValueError:
+                continue
+            rings.append(SceneRing(name=name, x=x, y=y, z=z, radius=r))
+        elif name == "center_bore":
+            # ⚠️ 必须先判 center_bore 再判 _bore 后缀：名字 "center_bore" 也以
+            # "_bore" 结尾，先判后缀会让中心大孔的半径覆盖掉小孔的。
+            # （踩过：insert_bore_radius 报出 0.02，看起来"孔比环大"，很费解。）
+            bores["center_bore"] = r
+        elif name.endswith("_bore"):
+            bores["insert_bore"] = r
+    return rings, bores
+
+
+def rings_local(
+    spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
+) -> List[SceneRing]:
+    """工件模型里所有嵌件亮环（物体局部系，单位米）。
+
+    与 :func:`features_from_workpiece` **共用同一份解析**（``_visual_geometry``），
+    因为两处必须永远一致：前者算"该看到多大"，后者算"该看到在哪" ——
+    漂移了就会出现"像素门槛说能看到、真值却投影不到那儿"的怪现象。
+    """
+    try:
+        tree = ET.parse(model_sdf_path(spec, obj, root))
+    except (OSError, ET.ParseError):
+        return []
+    return _visual_geometry(tree)[0]
+
+
 def features_from_workpiece(
     spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
 ) -> Dict[str, float]:
@@ -780,47 +870,18 @@ def features_from_workpiece(
     except (OSError, ET.ParseError):
         return {}
 
+    rings, bores = _visual_geometry(tree)
     out: Dict[str, float] = {}
-    rings: List[float] = []
+    if rings:
-    for vis in tree.iter("visual"):
+        out["insert_ring_radius"] = rings[-1].radius
-        name = vis.get("name") or ""
+    if "center_bore" in bores:
-        cyl = vis.find("geometry/cylinder")
+        out["center_bore_radius"] = bores["center_bore"]
-        if cyl is None:
+    if "insert_bore" in bores:
-            continue
+        out["insert_bore_radius"] = bores["insert_bore"]
-        r_el = cyl.find("radius")
+    # 两孔间距 = 两个环心之间的距离
-        if r_el is None or not r_el.text:
+    if len(rings) == 2:
-            continue
-        try:
-            r = float(r_el.text)
-        except ValueError:
-            continue
-        if name.endswith("_ring"):
-            out["insert_ring_radius"] = r
-            rings.append(r)
-        elif name == "center_bore":
-            # ⚠️ 必须先判 center_bore 再判 _bore 后缀：名字 "center_bore" 也以
-            # "_bore" 结尾，先判后缀会让中心大孔的半径覆盖掉小孔的。
-            # （踩过：insert_bore_radius 报出 0.02，看起来"孔比环大"，很费解。）
-            out["center_bore_radius"] = r
-        elif name.endswith("_bore"):
-            out["insert_bore_radius"] = r
-
-    # 两孔间距 = 两个环心之间的距离（从 visual 的 pose 里取）
-    centers: List[Tuple[float, float]] = []
-    for vis in tree.iter("visual"):
-        if not (vis.get("name") or "").endswith("_ring"):
-            continue
-        pose_el = vis.find("pose")
-        if pose_el is not None and pose_el.text:
-            parts = pose_el.text.split()
-            if len(parts) >= 3:
-                try:
-                    centers.append((float(parts[0]), float(parts[1])))
-                except ValueError:
-                    pass
-    if len(centers) == 2:
         out["hole_spacing"] = math.hypot(
-            centers[0][0] - centers[1][0], centers[0][1] - centers[1][1]
+            rings[0].x - rings[1].x, rings[0].y - rings[1].y
         )
     return out
 

src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py

@@ -101,8 +101,14 @@ def load_sim_site(root: Optional[Path] = None) -> SimSite:
 
 
 def repo_root() -> Path:
-    """仓库根。本模块在 src/arm_sim_gazebo/arm_sim_gazebo/ 下，向上走四层。"""
+    """仓库根。**实现只有一份**：``robot_description.repo_root()``。
-    return Path(__file__).resolve().parents[3]
+
+    这里曾经也是 ``parents[3]`` —— 源码树里对、安装后错（install_sim 是拷贝）。
+    两处各写一份还会各自漂移，所以都改成委托（见 sim_scene.repo_root 的注释）。
+    """
+    from arm_sim_gazebo.robot_description import repo_root as _robust_root
+
+    return _robust_root()
 
 
 def resolve_world(

src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py

@@ -50,10 +50,20 @@ from rclpy.time import Time
 
 from arm_msgs.msg import TactileStatus
 from geometry_msgs.msg import Wrench
-from ros_gz_interfaces.msg import Contacts
 from sensor_msgs.msg import Image
 from tf2_ros import Buffer, TransformListener
 
+# ros_gz_interfaces 只在仿真里用得到，而且是 conda 装的（不在本工作区）。
+# 万一它的 typesupport 加载不了（见下），我们只丢掉"接触点"这一路信息 ——
+# 力本身走 geometry_msgs/Wrench，不受影响。所以这里**容错导入**：
+# 宁可用退化模式跑，也不要整个适配器起不来。
+_contacts_err: Optional[str] = None
+try:
+    from ros_gz_interfaces.msg import Contacts
+except Exception as _exc:  # pragma: no cover - 取决于运行环境
+    Contacts = None  # type: ignore[assignment]
+    _contacts_err = f"{type(_exc).__name__}: {_exc}"
+
 from .sim_tactile import (
     DEFAULT_TACTILE_REL,
     BaselineTracker,
@@ -128,6 +138,7 @@ class SimTactileAdapter(Node):
 
         self._pub_status = {}
         self._pub_image: Dict[str, Tuple[object, object, float, float]] = {}
+        self._contact_unavailable = False
         for s in self.spec.sides:
             self._got_wrench[s.side] = False
             self._got_contact[s.side] = False
@@ -140,12 +151,15 @@ class SimTactileAdapter(Node):
                 self._make_wrench_cb(s.side),
                 10,
             )
-            self.create_subscription(
+            if Contacts is not None:
-                Contacts,
+                self.create_subscription(
-                s.contact_topic,
+                    Contacts,
-                self._make_contact_cb(s.side),
+                    s.contact_topic,
-                10,
+                    self._make_contact_cb(s.side),
-            )
+                    10,
+                )
+            else:
+                self._contact_unavailable = True
             if self.spec.image_hz > 0.0:
                 self._pub_image[s.side] = (
                     self.create_publisher(Image, s.image_topic, 3),
@@ -165,6 +179,20 @@ class SimTactileAdapter(Node):
         self._check_at = time.monotonic() + TOPIC_CHECK_DELAY_S
         self._checked = False
 
+        if self._contact_unavailable:
+            detail = f"（导入失败：{_contacts_err}）" if _contacts_err else ""
+            self.get_logger().warn(
+                f"ros_gz_interfaces/msg/Contacts 用不了{detail}，"
+                f"本节点进入退化模式：\n"
+                f"  仍会发 TactileStatus（fnormal/fshearx/fsheary、connected、"
+                f"接触去基线都正常），\n"
+                f"  但 contact 恒为 False、centroid_u/v 恒为 0"
+                f"（这两项依赖接触点消息）。\n"
+                f"  三力判据（本项目 S07 用的那个）不受影响。\n"
+                f"  要修：在**干净 shell** 里 source scripts/sim_env.sh（它会清掉真机"
+                f"环境残留的 PYTHONPATH/LD_LIBRARY_PATH 并补上 $CONDA_PREFIX/lib）。"
+            )
+
     # --- 订阅回调 ---------------------------------------------------------
     def _make_wrench_cb(self, side: str):
         def cb(msg: Wrench) -> None:

src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py

@@ -0,0 +1,328 @@
+#!/usr/bin/env python3
+"""仿真"真值感知"：把 Gazebo 里**已知的真实位姿**当成视觉测量发给伺服。
+
+为什么要这个节点
+----------------
+真机的视觉链是 ``perception_node``（YOLO/ONNX）→ 话题
+``visual_servo/filtered_point``。仿真环境（ros_gz_sim conda）里没有
+torch/ultralytics，而且这台机器**离线**装不上，于是 perception_node 一启动就死。
+
+死法还很误导人：S00 只看"权重文件在不在"（在），S04 一阶段则一直等
+``/visual_servo/locked``，等满 150s 报"没锁定（视野里没工件？）"——
+现场看到的是"算法不收敛/标定不准"，真因是"仿真里根本没有推理后端"。
+
+仿真里其实**不需要推理**：工件摆在哪、长什么样都是已知的
+（``scene.yaml`` 的绝对位姿 + 模型自己的几何）。伺服要的那个"相机系下的
+目标点"可以直接算出来：
+
+    世界系目标点 = 物体世界 XY 轮廓中心 + 高度（模型顶面）  ＋ 可选局部偏移
+    相机系目标点 = T_cam_world · 世界系目标点                （T 由 TF 查，不自己算）
+
+    发布 → ``PointStamped(visual_servo/filtered_point)``
+          与 perception_node **同话题、同类型**，所以下游（servo_node、
+          整条装配流程）一个字都不用改。
+
+⚠️ 它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排），
+   不是感知那一半。真值箱体没有像素噪声、没有遮挡、没有光照变化，也没有
+   "算法把孔认成别的"这类错。所以：
+
+     能跑通 = "控制侧与接口是对的"
+     不能跑通 ≠ "真机上也能跑通"          ← 规律写在
+     devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节
+
+绝不做的事
+----------
+* 目标在相机背后 / 相机查不到 TF 时**不发假点**（和 perception_node 的
+  "没有可用深度就不发点"同一条规矩）。宁可让伺服看门狗超时，也不给它一个
+  幻影目标 —— 后者会让机械臂朝错误方向走，而且看起来像"标定坏了"。
+* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。
+  这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是
+  "总能到附近、永远差一点"。数学实现只能有一份。
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Optional, Sequence, Tuple
+
+import numpy as np
+import rclpy
+from geometry_msgs.msg import PointStamped
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from tf2_ros import Buffer, TransformListener
+
+from arm_sim_gazebo import sim_scene
+
+#: 与 perception_node / servo_node 一致：点话题用 BEST_EFFORT + KEEP_LAST(5)。
+#: 匹配不上 QoS 的后果是"话题在、消息发了、订阅方一条也收不到"，
+#: 而两边单独看都正常 —— 这类问题排查成本极高，所以直接抄同一份配置。
+POINT_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+)
+
+#: 目标点离相机光心太近（或跑到背后）就不发：z<=0 在相机系里是"背后"。
+MIN_CAM_Z_M = 0.01
+
+
+class TruthError(RuntimeError):
+    """真值目标解不出来（场景 / 模型 / 参数有问题）。
+
+    单独一个异常类型是为了让"配置错了"和"TF 还没到"分开处理：
+    前者要直接起不来（早报），后者只该刷限流告警（等机器人动起来就有了）。
+    """
+
+
+@dataclass(frozen=True)
+class TruthTarget:
+    """真值目标：一个**世界系**点，以及它是怎么算出来的（给人看）。"""
+
+    object_name: str
+    world_xyz: Tuple[float, float, float]
+    note: str
+
+
+def target_point_world(
+    spec: sim_scene.SceneSpec,
+    obj: sim_scene.SceneObject,
+    offset_m: Sequence[float] = (0.0, 0.0, 0.0),
+    root: Optional[Path] = None,
+) -> TruthTarget:
+    """物体顶面中心（世界系）＋ 物体局部系偏移。
+
+    为什么取"顶面中心"：真机 perception_node 默认 ``feature_mode: plane``，
+    发的是检测框在**工件上表面**上的几何中心。真值要想替得住它，就得发同一个
+    物理点；差一个板厚（12mm）会让对比结论完全反过来。
+
+    为什么用 ``sim_scene.footprint`` 而不是自己按 yaw 算：
+    轮廓换算里已经含了 yaw（``world_boxes`` 里有且只有一份实现），
+    自己再写一遍迟早不一致，而"哪个才是对的"很难判断。
+
+    ``offset_m`` 是**物体局部系**里的偏移（会跟着 yaw 转），用途例如：
+    把目标从"板中心"挪到 k1 孔 ``[0, -0.029, 0]``，用来单独验证二阶段对孔。
+    """
+    fp = sim_scene.footprint(spec, obj, root)
+    height = sim_scene.model_height(spec, obj, root)
+    if fp is None or height is None:
+        raise TruthError(
+            f"读不出 {obj.name!r} 的几何（模型 {obj.model}）。\n"
+            f"  期望：{sim_scene.model_sdf_path(spec, obj, root)} 里有 <collision>\n"
+            f"  修法：确认 models_dir 指对了、模型目录里有 model.sdf 且至少一个碰撞体。"
+        )
+    vals = [float(v) for v in offset_m]
+    if len(vals) != 3:
+        # 长度不对必须报错，不能"缺的当 0"：把 [0.03] 当成 (0.03,0,0) 还是
+        # (0,0.03,0) 全凭猜，而猜错的表现是"偏了一点"，很难归因。
+        raise TruthError(
+            f"offset_m 要给 3 个数（物体局部系 xyz，单位米），现在给的是 {list(offset_m)!r}。"
+        )
+    ox, oy, oz = vals
+    cx = 0.5 * (fp[0] + fp[1])
+    cy = 0.5 * (fp[2] + fp[3])
+    c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
+    x = cx + c * ox - s * oy
+    y = cy + s * ox + c * oy
+    z = float(obj.z) + float(height) + oz
+    note = (
+        f"{obj.name}（模型 {obj.model}）"
+        f"轮廓中心 ({cx:.3f},{cy:.3f}) + 高 {height * 1000:.1f}mm"
+        f" + 局部偏移 ({ox * 1000:+.0f},{oy * 1000:+.0f},{oz * 1000:+.0f})mm"
+        f"  yaw={math.degrees(float(obj.yaw)):.1f}°"
+    )
+    return TruthTarget(object_name=obj.name, world_xyz=(x, y, z), note=note)
+
+
+def pick_object(spec: sim_scene.SceneSpec, name: str) -> sim_scene.SceneObject:
+    """按名字找物体；找不到时把**现有名字**列出来（改配置时不用再翻文件）。"""
+    want = (name or "").strip() or spec.operating.part_object
+    for obj in spec.objects:
+        if obj.name == want:
+            return obj
+    raise TruthError(
+        f"场景 {spec.source} 里没有名为 {want!r} 的物体。\n"
+        f"  现有物体：{', '.join(o.name for o in spec.objects) or '（空）'}\n"
+        f"  修法：改 site.yaml 的 perception.truth.object，"
+        f"或确认 scene.yaml 的 objects 里确实有它。"
+    )
+
+
+def load_truth_target(
+    scene_file: Optional[Path],
+    object_name: str,
+    offset_m: Sequence[float] = (0.0, 0.0, 0.0),
+    root: Optional[Path] = None,
+) -> Tuple[sim_scene.SceneSpec, TruthTarget]:
+    """读场景 + 解出真值目标。返回 ``(场景, 目标)``，场景用于启动日志。
+
+    ``object_name`` 空 = 用场景里的 ``operating.part_object``：**"哪个是工件"
+    是场景的知识，不是流程的知识**，所以默认跟着场景走，换场景不用改流程参数。
+    """
+    spec = sim_scene.load_scene(scene_file, root)
+    return spec, target_point_world(spec, pick_object(spec, object_name), offset_m, root)
+
+
+def apply_transform(T: np.ndarray, p: Sequence[float]) -> Tuple[float, float, float]:
+    """4×4 齐次变换作用于点。"""
+    q = np.asarray(T, dtype=float) @ np.array([p[0], p[1], p[2], 1.0], dtype=float)
+    return (float(q[0]), float(q[1]), float(q[2]))
+
+
+def matrix_from_transform(tr) -> np.ndarray:
+    """``TransformStamped`` → 4×4 齐次矩阵。
+
+    四元数→旋转矩阵**不在这里实现**：复用 ``handeye_lib.quat_xyzw_to_R``
+    （那是全仓库唯一一份，且有回环测试钉着）。重复实现的下场见模块 docstring。
+    """
+    from arm_application.handeye_lib import quat_xyzw_to_R
+
+    t = tr.transform.translation
+    q = tr.transform.rotation
+    T = np.eye(4, dtype=float)
+    T[:3, :3] = quat_xyzw_to_R([q.x, q.y, q.z, q.w])
+    T[0, 3] = float(t.x)
+    T[1, 3] = float(t.y)
+    T[2, 3] = float(t.z)
+    return T
+
+
+class SimTruthPerception(Node):
+    """把真值目标点按固定频率发到 ``visual_servo/filtered_point``。"""
+
+    def __init__(self) -> None:
+        super().__init__("sim_truth_perception")
+        self.declare_parameter("scene_file", "")
+        """场景文件（空 = 仓库根下 devices/sim/gazebo/config/scene.yaml）。"""
+
+        self.declare_parameter("target_object", "")
+        """目标物体名（空 = 场景里的 operating.part_object）。"""
+
+        self.declare_parameter("offset_m", [0.0, 0.0, 0.0])
+        """物体局部系偏移 [m]，例如挪到某个孔：``[0.0, -0.029, 0.0]``。"""
+
+        self.declare_parameter("camera_frame", "")
+        """相机光学系（由 launch 按当前臂/相机传进来，不写死）。"""
+
+        self.declare_parameter("world_frame", "world")
+        """场景的世界系。Gazebo 里模型位姿就是相对它，TF 树上也有。"""
+
+        self.declare_parameter("point_topic", "visual_servo/filtered_point")
+        self.declare_parameter("publish_hz", 20.0)
+        """发布频率：不必跟着相机帧率，伺服 100Hz 控制会自己插值/外推。"""
+
+        scene_file = str(self.get_parameter("scene_file").value or "").strip()
+        self._scene_file = Path(scene_file) if scene_file else None
+        offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0]
+        # 长度的校验在 target_point_world 里（纯函数，有测试钉着），这里不重复。
+        self._camera_frame = str(self.get_parameter("camera_frame").value or "").strip()
+        if not self._camera_frame:
+            raise TruthError(
+                "camera_frame 没给。它由 visual_servo.launch.py 按当前臂的相机传进来"
+                "（site.yaml → cameras/ perception.camera_frame）。"
+                "手工跑这个节点时必须显式指定，例如 "
+                "--ros-args -p camera_frame:=sim_d405_color_optical_frame"
+            )
+        self._world_frame = str(self.get_parameter("world_frame").value or "world").strip()
+
+        # 目标点在启动时算一次就够：场景是静态的（工件被推动后位置会变，
+        # 但那种情况下"真值"本身也就失效了 —— 此时应该重跑 gen_sim_handeye
+        # 或重启，而不是悄悄跟着一个被撞歪的位姿走）。
+        self._spec, self._target = load_truth_target(
+            self._scene_file,
+            str(self.get_parameter("target_object").value or ""),
+            offset,
+        )
+
+        self._pub = self.create_publisher(
+            PointStamped, str(self.get_parameter("point_topic").value), POINT_QOS
+        )
+        self._tf = Buffer()
+        TransformListener(self._tf, self, spin_thread=True)
+        self._sent = 0
+        self._warned_tf = 0
+
+        hz = max(1.0, min(100.0, float(self.get_parameter("publish_hz").value)))
+        self.create_timer(1.0 / hz, self._tick)
+        self.get_logger().warn(
+            "真值感知已启动（仿真专用）：不跑任何推理，直接把已知位姿当测量发出去。\n"
+            f"  场景 {self._spec.source}\n"
+            f"  目标 {self._target.note}\n"
+            f"  世界系点 ({self._target.world_xyz[0]:.3f}, "
+            f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f})  "
+            f"→ 相机系 {self._camera_frame}（TF: {self._world_frame} → "
+            f"{self._camera_frame}）  频率 {hz:.0f}Hz\n"
+            f"  ⚠️ 它验证的是控制与接口，不是检测算法 —— 见 "
+            f"devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」"
+        )
+
+    # ------------------------------------------------------------------
+    def _tick(self) -> None:
+        try:
+            tr = self._tf.lookup_transform(self._camera_frame, self._world_frame, Time())
+        except Exception as exc:  # noqa: BLE001 —— tf2 的异常类型很杂，统一当"还没有"
+            # 前几拍必然查不到（TF 要等 robot_state_publisher 起来），
+            # 所以只刷限流告警，不报错。
+            self._warned_tf += 1
+            if self._warned_tf in (1, 20, 100) or self._warned_tf % 200 == 0:
+                self.get_logger().warn(
+                    f"还查不到 TF {self._world_frame} → {self._camera_frame}"
+                    f"（已试 {self._warned_tf} 次）：{exc}",
+                    throttle_duration_sec=5.0,
+                )
+            return
+        p = apply_transform(matrix_from_transform(tr), self._target.world_xyz)
+        if p[2] < MIN_CAM_Z_M:
+            # 目标在相机背后或贴到镜头上：不发！发出去伺服会朝反方向走。
+            self.get_logger().warn(
+                f"真值目标不在相机前方（相机系 z={p[2] * 1000:.0f}mm < "
+                f"{MIN_CAM_Z_M * 1000:.0f}mm）—— 不发点。"
+                f"世界系点 ({self._target.world_xyz[0]:.3f}, "
+                f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f})"
+                f" 不在 {self._camera_frame} 视野里；先让臂摆到能看到它的位置。",
+                throttle_duration_sec=5.0,
+            )
+            return
+
+        msg = PointStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self._camera_frame
+        msg.point.x, msg.point.y, msg.point.z = p
+        self._pub.publish(msg)
+        self._sent += 1
+        if self._sent == 1:
+            self.get_logger().info(
+                f"首个真值目标点已发出：相机系 ({p[0]:.4f}, {p[1]:.4f}, {p[2]:.4f}) m"
+            )
+        elif self._sent % 200 == 0:
+            self.get_logger().debug(f"已发 {self._sent} 个真值点")
+
+
+def main(argv=None) -> int:
+    import traceback
+
+    rclpy.init(args=argv)
+    node = None
+    try:
+        node = SimTruthPerception()
+        rclpy.spin(node)
+        return 0
+    except TruthError as exc:
+        # 配置错：把原因原样打出来（它是给人看的），别让 ROS 的异常栈盖住。
+        print(f"真值感知起不来：{exc}", flush=True)
+        return 2
+    except Exception:  # noqa: BLE001
+        traceback.print_exc()
+        return 1
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py

@@ -0,0 +1,214 @@
+#!/usr/bin/env python3
+"""真值孔位：把场景里**已知的孔几何**投影成二阶段要的那两个像素点。
+
+为什么需要它
+------------
+二阶段（S05 精拍 → 对准）原本的输入是 ``hole_detect_hough`` 在图上找**亮环**。
+那条路要求工件检测模型先给出工件框（ROI），否则连"找哪儿"都不知道。
+
+仿真的处境是：合成图过不了真机训练出来的工件模型（实测最高分 0.017），
+于是 S05 只能退回"整幅画面当 ROI"—— 而整幅画面里有**夹爪、通止规、台面**，
+HoughCircles 会在那儿找到"看起来像一对孔"的东西（实测两孔间距 108mm，
+真值是 58mm），随后被间距门槛丢掉，每拍都丢，最后超时。
+
+真值路径直接把"孔在图上应该在哪"算出来：
+
+    孔世界点 = 物体世界位姿（scene.yaml）+ 环在物体局部系的位置（模型 <visual>）
+    孔像素   = K · T_cam_world · 孔世界点
+
+它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排），
+不是感知那一半 —— 与 ``sim_truth_perception`` 同一条设计（见
+``devices/sim/gazebo/README.md`` 的「仿真里哪些模块用真值替代」）。
+
+绝不做的事
+----------
+* 孔在相机背后（z<=0）时**不返回**结果。给伺服一个幻影目标会让机械臂朝错误
+  方向走，而且看起来像"标定坏了" —— 和 ``sim_truth_perception`` 同一条规矩。
+* 不自己算四元数→旋转矩阵：T 由调用方（节点）用 TF 查好传进来。
+  本模块只做**纯几何**，因此在没有 ROS 的环境里也能测。
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import List, Optional, Sequence, Tuple
+
+import numpy as np
+
+from arm_sim_gazebo import sim_scene
+
+#: 孔（环）离相机光心太近或跑到背后就不返回：z 太小除法会放大误差，
+#: z<=0 在相机系里就是"背后"。取 2cm，与真值感知节点的取值同量级。
+MIN_CAM_Z_M = 0.02
+
+#: 两种孔排序的判据：图像上 |dx| 大于这个比例倍数就按 x 排，否则按 y 排。
+#: 与 ``hole_detect_hough._detect_insert_pair`` 的规则一致（k1=左/上，
+#: k2=右/下）—— 换后端不该换语义，否则"哪边是 k1"会随环境变。
+_PAIR_HORIZONTAL_RATIO = 1.0
+
+
+@dataclass(frozen=True)
+class HoleTruth:
+    """一次真值定位的结果（``ok=False`` 时只有 ``why`` 有意义）。"""
+
+    ok: bool
+    why: str = ""
+    hole_points: List[Tuple[float, float]] = field(default_factory=list)
+    #: 两孔的像素半径（环的物理半径按透视缩放；Hough 路径给的是它自己的估计，
+    #: 真值路径给它一个**可信的**值 —— 上下游只把它当"尺度参考"用）
+    hole_radii: List[float] = field(default_factory=list)
+    #: 工件在画面里占的框（孔检测的 ROI 用；也是"工件中心"的来源）
+    bbox: Tuple[int, int, int, int] = (0, 0, 0, 0)
+    #: 给人看的说明（进了日志才知道这个数是哪来的）
+    note: str = ""
+
+
+def _object_world_xy(obj: sim_scene.SceneObject, lx: float, ly: float) -> Tuple[float, float]:
+    """物体局部 (x,y) → 世界 (x,y)（含物体自身 yaw）。"""
+    c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
+    return (float(obj.x) + c * lx - s * ly, float(obj.y) + s * lx + c * ly)
+
+
+def project_point(
+    T_cam_world: np.ndarray, intr: Sequence[float], xyz: Sequence[float]
+) -> Optional[Tuple[float, float, float]]:
+    """世界点 → ``(u, v, z_cam)``；在相机背后/太近时返回 None。"""
+    T = np.asarray(T_cam_world, dtype=float)
+    p = T @ np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
+    z = float(p[2])
+    if not math.isfinite(z) or z <= MIN_CAM_Z_M:
+        return None
+    fx, fy, cx, cy = (float(intr[0]), float(intr[1]), float(intr[2]), float(intr[3]))
+    return (fx * float(p[0]) / z + cx, fy * float(p[1]) / z + cy, z)
+
+
+def object_bbox_px(
+    T_cam_world: np.ndarray,
+    intr: Sequence[float],
+    boxes: Sequence[Sequence[float]],
+    image_shape: Tuple[int, int],
+) -> Optional[Tuple[int, int, int, int]]:
+    """物体各碰撞体世界 AABB 的角点投影 → 像素包围框（裁剪到画面内）。
+
+    返回 None = 一个角点都投不出来（跑到背后去了），调用方应当放弃这一拍。
+    """
+    h, w = int(image_shape[0]), int(image_shape[1])
+    us: List[float] = []
+    vs: List[float] = []
+    for b in boxes:
+        for x in (b[0], b[1]):
+            for y in (b[2], b[3]):
+                for z in (b[4], b[5]):
+                    got = project_point(T_cam_world, intr, (x, y, z))
+                    if got is None:
+                        continue
+                    us.append(got[0])
+                    vs.append(got[1])
+    if not us:
+        return None
+    x1 = max(0, min(w - 1, int(math.floor(min(us)))))
+    y1 = max(0, min(h - 1, int(math.floor(min(vs)))))
+    x2 = max(0, min(w - 1, int(math.ceil(max(us)))))
+    y2 = max(0, min(h - 1, int(math.ceil(max(vs)))))
+    if x2 <= x1 or y2 <= y1:
+        return None
+    return (x1, y1, x2, y2)
+
+
+def locate_holes(
+    T_cam_world: Optional[np.ndarray],
+    intr: Optional[Sequence[float]],
+    image_shape: Tuple[int, int],
+    *,
+    spec: Optional[sim_scene.SceneSpec] = None,
+    object_name: str = "",
+    root: Optional[Path] = None,
+) -> HoleTruth:
+    """算这一拍的两个孔（像素 + ROI 框）。
+
+    ``T_cam_world`` 为 None（TF 还没到）时返回 ``ok=False``，由调用方决定是
+    继续用检测器还是报错 —— 本函数不猜、不兜底。
+    """
+    if T_cam_world is None or intr is None:
+        return HoleTruth(ok=False, why="还没有 T_cam_world / 相机内参（TF 或 camera_info 未到）")
+    try:
+        spec = spec if spec is not None else sim_scene.load_scene(None, root)
+    except Exception as exc:  # noqa: BLE001 —— 场景读不出来要说清是场景的问题
+        return HoleTruth(ok=False, why=f"读不到场景配置：{exc}")
+    want = (object_name or "").strip() or spec.operating.part_object
+    obj = next((o for o in spec.objects if o.name == want), None)
+    if obj is None:
+        names = ", ".join(o.name for o in spec.objects) or "（空）"
+        return HoleTruth(ok=False, why=f"场景里没有物体 {want!r}（现有：{names}）")
+    rings = sim_scene.rings_local(spec, obj, root)
+    if len(rings) != 2:
+        return HoleTruth(
+            ok=False,
+            why=(
+                f"工件 {obj.name!r} 的模型里有 {len(rings)} 个嵌件亮环，"
+                f"真值孔位要求正好 2 个（模型 <visual name=\"*_ring\"> 的名字约定）"
+            ),
+        )
+
+    pts: List[Tuple[float, float]] = []
+    rad: List[float] = []
+    for ring in rings:
+        wx, wy = _object_world_xy(obj, ring.x, ring.y)
+        got = project_point(T_cam_world, intr, (wx, wy, float(obj.z) + ring.z))
+        if got is None:
+            return HoleTruth(
+                ok=False,
+                why=f"孔 {ring.name} 投影到相机背后（z<=0）—— 相机没对着工件时不该出结果",
+            )
+        u, v, z = got
+        pts.append((u, v))
+        rad.append(float(intr[0]) * float(ring.radius) / z)
+
+    # k1/k2 的左右（上下）规则与 Hough 路径一致：水平对按 x，竖直对按 y。
+    (p, q) = pts
+    horizontal = abs(p[0] - q[0]) >= _PAIR_HORIZONTAL_RATIO * abs(p[1] - q[1])
+    key = 0 if horizontal else 1
+    if pts[0][key] > pts[1][key]:
+        pts = [pts[1], pts[0]]
+        rad = [rad[1], rad[0]]
+
+    bbox = object_bbox_px(T_cam_world, intr, sim_scene.world_boxes(spec, obj, root), image_shape)
+    if bbox is None:
+        return HoleTruth(ok=False, why="工件整体投影不出画面框（角点都在相机背后）")
+
+    dist_px = math.hypot(pts[0][0] - pts[1][0], pts[0][1] - pts[1][1])
+    note = (
+        f"{obj.name}（模型 {obj.model}） 环心距 {dist_px:.1f}px "
+        f"环半径 {rad[0]:.1f}/{rad[1]:.1f}px 框 {bbox}"
+    )
+    return HoleTruth(
+        ok=True,
+        why="",
+        hole_points=[(float(pts[0][0]), float(pts[0][1])), (float(pts[1][0]), float(pts[1][1]))],
+        hole_radii=[float(rad[0]), float(rad[1])],
+        bbox=bbox,
+        note=note,
+    )
+
+
+def draw(bgr: np.ndarray, res: HoleTruth) -> np.ndarray:
+    """把真值结果画在图上（叠图/日志用）。失败时只写一行原因。"""
+    import cv2
+
+    img = bgr.copy()
+    h, w = img.shape[:2]
+    if not res.ok:
+        cv2.putText(img, "TRUTH: n/a", (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
+        return img
+    x1, y1, x2, y2 = res.bbox
+    cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 255), 2)
+    for i, (u, v) in enumerate(res.hole_points):
+        r = int(round(res.hole_radii[i])) if i < len(res.hole_radii) else 8
+        cv2.circle(img, (int(round(u)), int(round(v))), max(3, r), (0, 0, 255), 2)
+        cv2.putText(
+            img, f"T{i + 1}", (int(u) + 8, int(v)), cv2.FONT_HERSHEY_SIMPLEX, 0.60, (0, 0, 255), 2
+        )
+    cv2.putText(img, "TRUTH", (12, min(h - 8, 28)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
+    return img

src/arm_sim_gazebo/launch/gazebo_sim.launch.py

@@ -541,6 +541,58 @@ def _prepare(context, *args, **kwargs):
             )
         )
 
+    # --- 6c) 机械臂的"应用接口"对齐真机 ---------------------------------------
+    # 仿真里唯一的控制通路是 ros2_control 的 JTC（control_msgs）。但应用层在
+    # 真机上用的是另一层接口 —— 这里把它们**按真机的名字与类型**补齐，
+    # 于是通止规装配（以及 stage2/stage3）在仿真里能不改一行代码就跑：
+    #
+    #   /follow_joint_trajectory   arm_msgs/FollowJointTrajectory → JTC
+    #       真机由 real_trajectory_adapter 提供（名字带 real，但实现只做 action
+    #       桥接、与硬件无关；这里**复用同一个节点**而不是再写一份 ——
+    #       两份实现迟早会不一致）。
+    #   /set_named_pose            arm_msgs/SetNamedPose（命名位姿）→ 上面那个
+    #       真机由 arm_bringup/stack.launch.py 起，仿真起同一份，只是把
+    #       joint_prefix 换成 sim_（关节名不同，点位数值是同一台机器人）。
+    #   /home                      std_srvs/Trigger
+    #       真机由 flexiv_arm_driver 提供（= 切工具系 + 转 /set_named_pose）。
+    #       仿真没有 Flexiv 驱动，用 sim_home_service 补同一份契约（不切工具系）。
+    actions.append(
+        Node(
+            package="arm_control",
+            executable="real_trajectory_adapter",
+            name="real_trajectory_adapter",
+            output="screen",
+            parameters=[{"use_sim_time": True}],
+        )
+    )
+    actions.append(
+        Node(
+            package="arm_control",
+            executable="named_pose_server",
+            name="named_pose_server",
+            output="screen",
+            parameters=[
+                # 命名位姿用**包内默认那份**（src/arm_control/config/named_poses.yaml）：
+                # 它只有 home/ready/inspect 这几个点，与"仿真里是哪台臂"无关；
+                # 真机走 site.yaml 的 robots[].paths.named_poses_file（那份带该臂的点位）。
+                str(root / "src" / "arm_control" / "config" / "named_poses.yaml"),
+                # 关节名：仿真里是 sim_joint1..7（前缀来自 urdf 的 arm_prefix，
+                # site.yaml 的 sim.joint_prefix 目前是"声明了但没接线"，见那一段注释）。
+                {"joint_prefix": "sim_"},
+                {"use_sim_time": True},
+            ],
+        )
+    )
+    actions.append(
+        Node(
+            package="arm_sim_gazebo",
+            executable="sim_home_service",
+            name="sim_home_service",
+            output="screen",
+            parameters=[{"use_sim_time": True}],
+        )
+    )
+
     actions.append(
         LogInfo(
             msg=(

src/arm_sim_gazebo/setup.py

@@ -50,6 +50,16 @@ setup(
                 # 7 个夹爪关节的目标位置（含 mimic 耦合换算），让仿真能走
                 # 真机那一套夹爪接口。由 gazebo_sim.launch.py 拉起。
                 'sim_gripper_adapter = arm_sim_gazebo.sim_gripper_adapter:main',
+                # 仿真 /home：真机上这个服务由 Flexiv 驱动提供（包装
+                # /set_named_pose），仿真里没有驱动，用它补同一份契约，
+                # 让装配流程（S01 回 home / S11 复位）在两处都不改代码。
+                'sim_home_service = arm_sim_gazebo.sim_home_service:main',
+                # 仿真"真值感知"：不跑推理，直接把场景里已知的物体位姿算成
+                # 相机系目标点，发到 visual_servo/filtered_point（与
+                # perception_node 同话题同类型）。仿真环境没有 torch/ultralytics
+                # 时用它顶替感知，让整条装配流程能在仿真里跑通。
+                # 由 visual_servo.launch.py 按 site.yaml 的 perception.backend 选择。
+                'sim_truth_perception = arm_sim_gazebo.sim_truth_perception:main',
                 # 独立小工具：只看归一化做了什么，不启动 Gazebo。
                 # 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
                 'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',

src/arm_sim_gazebo/test/test_check_sim_scene.py

@@ -363,6 +363,60 @@ def test_workspace_z_limits_degrades_to_none(chk, tmp_path):
     assert chk.workspace_z_limits(root2) is None
 
 
+def test_workspace_limits_use_the_sim_robots_override(chk, tmp_path):
+    """工作盒要取**仿真那台臂**的覆盖值，不能只读顶层。
+
+    顶层那套是真机的（台面与基座同高）；仿真里一切 Z 大 0.412。
+    只读顶层 → 拿真机的界核对仿真的悬停高，看起来过了，而生效配置可能不对。
+    """
+    root = _write_tree(
+        tmp_path,
+        pkg={},
+        site={
+            "servo": {
+                "workspace_xyz_min": [0.2, -0.5, 0.18],
+                "workspace_xyz_max": [0.8, 0.5, 0.65],
+            },
+            "robot": {
+                "robots": [
+                    {"sn": "Rizon4-063989", "arm": "left"},
+                    {
+                        "sn": "sim",
+                        "arm": "sim",
+                        "servo": {
+                            "workspace_xyz_min": [0.08, -0.55, 0.55],
+                            "workspace_xyz_max": [0.85, 0.55, 0.75],
+                        },
+                    },
+                ]
+            },
+        },
+    )
+    assert chk.workspace_z_limits(root) == (0.55, 0.75)
+
+
+def test_tracking_height_is_checked_against_the_workbox(chk, tmp_path, monkeypatch):
+    """一阶段跟踪高（悬停 + 50mm）超上界必须报错。
+
+    否则会出现：二阶段位姿检查能过、S04 一跟踪就被「超出工作盒」截断 ——
+    现象是"越跟踪越偏"，很难往配置上想（真机 0.65 上界对仿真 0.632 悬停
+    只差 18mm，一抬 5cm 就超了）。
+    """
+    root = _write_tree(
+        tmp_path,
+        pkg={},
+        site={"servo": {"workspace_xyz_min": [0.0, 0.0, 0.0], "workspace_xyz_max": [1.0, 1.0, 0.65]}},
+    )
+    import arm_sim_gazebo.sim_scene as sim_scene
+
+    monkeypatch.setattr(sim_scene, "operating_flange_z", lambda spec, root=None: 0.632)
+    probs = chk.check_hover_height(_fake_scene_with_part(), root)
+    assert len(probs) == 1
+    assert "0.682" in probs[0]          # 悬停 + 0.05
+    assert "workspace_xyz_max.z" in probs[0]
+    assert "camera_to_part_m" in probs[0]
+
+
 def test_check_hover_height_flags_above_workspace(chk, tmp_path, monkeypatch):
     """悬停高度超出工作空间上界必须报错，并指明两个可调的旋钮。
 
@@ -380,9 +434,12 @@ def test_check_hover_height_flags_above_workspace(chk, tmp_path, monkeypatch):
 
     monkeypatch.setattr(sim_scene, "operating_flange_z", lambda spec, root=None: 0.632)
     probs = chk.check_hover_height(_fake_scene_with_part(), root)
-    assert len(probs) == 1
+    # 悬停本身就超界时，跟踪高必然也超 —— 两条都要报（都是不同修法：
+    # 一条要抬界/拉近相机，另一条只要抬 workspace_xyz_max.z）。
+    assert len(probs) == 2
     assert "0.632" in probs[0] and "0.2" in probs[0]
     assert "camera_to_part_m" in probs[0]
+    assert "0.682" in probs[1]
 
 
 def test_check_hover_height_silent_when_inside_workspace(chk, tmp_path, monkeypatch):

src/arm_sim_gazebo/test/test_repo_root.py

@@ -0,0 +1,98 @@
+"""仓库根定位：装好的包也**必须**找得到仓库根。
+
+为什么单独一套测试
+------------------
+``parents[N]`` 这类写法在"从源码树里手工跑"时看着完全正常，一到正式启动就坏：
+``install_sim/`` 底下是**拷贝**（不是软链），于是装出来的模块算出来的
+"仓库根"是 ``install_sim/arm_sim_gazebo/lib``，而 ``devices/`` 不在那儿。
+现场表现是节点一启动就报
+
+    找不到场景配置 .../install_sim/arm_sim_gazebo/lib/devices/sim/gazebo/config/scene.yaml
+
+—— 而这跟"场景文件不存在"完全是两件事，很容易查错方向。
+
+本文件钉三件事：
+  1. 三个模块的 ``repo_root()`` 都委托给同一份实现（不许再冒出 parents[N]）；
+  2. ``ROS2_WS`` 环境变量优先；
+  3. 标记文件查找真的能找到（而不是"永远返回某个路径"）。
+"""
+
+from __future__ import annotations
+
+import inspect
+from pathlib import Path
+
+import pytest
+
+from arm_sim_gazebo import robot_description, sim_scene, sim_site
+
+MARKER = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro"
+
+
+def test_repo_root_implementations_all_delegate():
+    """三个入口都必须委托给 robot_description（路径实现只能有一份）。
+
+    只看**函数体**（去掉 docstring）：docstring 里正当地写着"以前是
+    parents[3]"这段历史，把它算进判据会误报。
+    """
+    import ast
+
+    for mod in (sim_scene, sim_site):
+        tree = ast.parse(inspect.getsource(mod.repo_root))
+        func = tree.body[0]
+        assert isinstance(func, ast.FunctionDef)
+        body = [
+            n for n in func.body
+            if not (isinstance(n, ast.Expr) and isinstance(n.value, ast.Constant))
+        ]
+        src = ast.dump(ast.Module(body=body, type_ignores=[]))
+        assert "robot_description" in src, f"{mod.__name__}.repo_root 必须委托，不能自己算"
+        assert "parents" not in src, f"{mod.__name__}.repo_root 里不许再出现 parents[N]"
+
+
+def test_repo_root_finds_the_real_repo():
+    root = sim_scene.repo_root()
+    assert (root / MARKER).is_file(), f"{root} 里没有 {MARKER} —— 这不是仓库根"
+    # 顺带确认框架自带的场景文件确实在（它在 git 里，不属于站点数据）
+    assert (root / "devices/sim/gazebo/config/scene.yaml").is_file()
+
+
+def test_ros2_ws_env_wins(tmp_path, monkeypatch):
+    """ROS2_WS 指向一个像仓库的目录时必须直接采信它。"""
+    (tmp_path / MARKER).parent.mkdir(parents=True, exist_ok=True)
+    (tmp_path / MARKER).write_text("<robot/>", encoding="utf-8")
+    monkeypatch.setenv("ROS2_WS", str(tmp_path))
+    assert robot_description.repo_root() == tmp_path
+    assert sim_scene.repo_root() == tmp_path
+
+
+def test_ros2_ws_is_ignored_when_it_does_not_look_like_a_repo(tmp_path, monkeypatch):
+    """环境变量指向一个不存在的仓库时要**继续找**，而不是返回一个错路径。
+
+    返回错路径是最坏的一种：后续报"文件不存在"，而人会去查文件，
+    不会想到是环境变量指错了。
+    """
+    monkeypatch.setenv("ROS2_WS", str(tmp_path / "nope"))
+    root = sim_scene.repo_root()
+    assert (root / MARKER).is_file()
+
+
+def test_marker_search_from_a_nested_cwd(tmp_path, monkeypatch):
+    """从任意深度的子目录启动（cwd 在仓库内）也能找到根。"""
+    monkeypatch.delenv("ROS2_WS", raising=False)
+    root = sim_scene.repo_root()
+    nested = root / "src" / "arm_sim_gazebo"
+    monkeypatch.chdir(nested)
+    assert sim_scene.repo_root() == root
+
+
+def test_missing_repo_root_error_tells_how_to_fix(tmp_path, monkeypatch):
+    """找不到时必须说清怎么修（导出 ROS2_WS / 在仓库根下执行）。"""
+    monkeypatch.delenv("ROS2_WS", raising=False)
+    monkeypatch.chdir(tmp_path)
+    monkeypatch.setattr(robot_description, "__file__", str(tmp_path / "x" / "y.py"))
+    with pytest.raises(robot_description.SimDescriptionError) as e:
+        robot_description.repo_root(start=tmp_path)
+    msg = str(e.value)
+    assert "ROS2_WS" in msg
+    assert "仓库根" in msg

src/arm_sim_gazebo/test/test_sim_cameras.py

@@ -12,12 +12,17 @@ import pytest
 
 from arm_sim_gazebo.sim_cameras import (
     MAX_MM,
+    MOUNT_TOL_DEG,
     CameraConfigError,
     CameraSpec,
     bridge_args,
     depth_m_to_mm16,
+    flange_frame_for,
     load_cameras,
+    mount_axis_deg,
+    mount_rpy_axis_deg,
     rpy_to_quat,
+    world_look_down_cos,
 )
 
 
@@ -507,3 +512,65 @@ def test_fov_check_against_real_repo_files():
         sensors_xacro=root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
     )
     assert issues == [], "视场与 SDF 不一致：\n" + "\n".join(issues)
+
+
+# ------------------------------------------------------------------ 安装朝向
+def test_mount_axis_deg_zero_when_optical_axis_is_the_flange_axis():
+    assert mount_axis_deg(np.eye(3)) == pytest.approx(0.0)
+
+
+def test_mount_axis_deg_is_ninety_when_the_camera_is_short_one_quarter_turn():
+    """少转 90° 正是踩过的那个坑：数值全正常，只是相机看着侧面。"""
+    R = np.array([[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]])  # Ry(-90°)
+    assert mount_axis_deg(R) == pytest.approx(90.0)
+
+
+def test_world_look_down_cos_sign_and_value():
+    assert world_look_down_cos(np.eye(3)) == pytest.approx(1.0)
+    flip = np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]])  # Rx(180°)
+    assert world_look_down_cos(flip) == pytest.approx(-1.0)
+
+
+def test_flange_frame_for_wrist_camera_but_not_world_camera():
+    wrist = CameraSpec(
+        gz_ns="/d405",
+        ros_ns="sim_d405",
+        link_frame="sim_d405_link",
+        optical_frame="sim_d405_color_optical_frame",
+    )
+    assert flange_frame_for(wrist) == "sim_flange"
+    world = CameraSpec(
+        gz_ns="/d435",
+        ros_ns="sim_d435",
+        link_frame="sim_d435_link",
+        optical_frame="sim_d435_color_optical_frame",
+        world_model="d435",
+    )
+    assert flange_frame_for(world) is None
+
+
+def test_wrist_camera_mount_angle_in_the_shipped_xacro_is_zero():
+    """仓库里那份 URDF 的腕相机安装角必须是"正装"。
+
+    Gazebo 的相机视轴是连杆的 +X，所以"正装在法兰上"= d405_pitch 为 -90°。
+    以前它是 0（视轴指向法兰 +X，也就是**水平**）：话题、图像、内参、深度
+    反投影全都正常，只有应用侧永远"目标不在相机前方"，极难归因。
+    这条测试把"默认值"钉住，改它必须是有意的。
+    """
+    import re
+    from pathlib import Path
+
+    root = Path(__file__).resolve().parents[3]
+    xacro = (root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro").read_text(
+        encoding="utf-8"
+    )
+    m = re.search(r"d405_pitch:=([-\d.]+)", xacro)
+    assert m, "xacro 里找不到 d405_pitch 的默认值"
+    pitch = float(m.group(1))
+    ang = mount_rpy_axis_deg((0.0, pitch, 0.0))
+    assert ang <= MOUNT_TOL_DEG, (
+        f"腕相机安装角 {ang:.1f}° 超过 {MOUNT_TOL_DEG}° —— "
+        f"法兰垂直朝下时它看不到工件（见 sensors.xacro 的说明）"
+    )
+    # 对照：老默认值（pitch=0）必须被判不合格，否则这条测试等于没测
+    assert mount_rpy_axis_deg((0.0, 0.0, 0.0)) > MOUNT_TOL_DEG

src/arm_sim_gazebo/test/test_sim_scene.py

@@ -265,7 +265,9 @@ def test_load_scene_pose_not_number_raises(tmp_path):
 def test_load_scene_operating_defaults(tmp_path):
     _write_scene(tmp_path, [], operating={})
     spec = load_scene(root=tmp_path)
-    assert spec.operating.camera == "d405"
+    # 默认值 = 仿真腕部相机名（sim_ 前缀）。改名时这里必须一起改 ——
+    # 它同时钉住"默认值就是仿真那台"，避免有人以为默认是 d405_left（真机名）。
+    assert spec.operating.camera == "sim_d405"
     assert spec.operating.part_object == "workpiece"
     assert spec.operating.camera_to_part_m == 0.20
 

src/arm_sim_gazebo/test/test_sim_truth_perception.py

@@ -0,0 +1,255 @@
+"""``sim_truth_perception`` 的测试：真值目标点怎么算、什么时候**不发**。
+
+为什么值得单独一套测试
+----------------------
+这个节点替掉的是 perception_node —— 也就是说，它一旦算错，错的不是一个
+显示值，而是**伺服的目标**：
+
+* 目标点算错 12mm（比如把"物体原点"当成"上表面"）→ 全程贴着工件下不去，
+  现象是"定位偏差"、像手眼标定不准；
+* 目标点算在相机背后还照发 → 伺服朝反方向走，现象是"越追越远"；
+* ``offset_m`` 不跟着 yaw 转 → 换了工件摆放角度后偏到工件外面。
+
+三类都是**静默错**：不报错、日志漂亮、结果错。所以这里逐条钉住，而不是
+只测"能不能算出个数"。
+
+顶部那条 golden（``test_shipped_scene_*``）会随 scene.yaml / 模型板厚变化
+而失败 —— 这是**故意**的：工件上表面高度是流程参数（悬停高、跟踪高）的
+推导依据，它改了就该有人来看一眼，而不是悄悄漂过去。
+"""
+
+from __future__ import annotations
+
+import inspect
+import math
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from arm_sim_gazebo import sim_scene
+from arm_sim_gazebo.sim_truth_perception import (
+    MIN_CAM_Z_M,
+    TruthError,
+    apply_transform,
+    load_truth_target,
+    matrix_from_transform,
+    pick_object,
+    target_point_world,
+)
+
+MODELS_REL = "devices/sim/gazebo/models"
+
+
+# ---------------------------------------------------------------------------
+# 迷你仓库（不依赖真实模型文件，数值可以先手算出来）
+# ---------------------------------------------------------------------------
+def _model(root: Path, name: str, size=(0.14, 0.10, 0.012)) -> Path:
+    """一个"原点在底面中心"的板子（本框架的建模约定）。"""
+    d = root / MODELS_REL / name
+    d.mkdir(parents=True, exist_ok=True)
+    (d / "model.sdf").write_text(
+        f"""<?xml version="1.0"?>
+<sdf version="1.6">
+  <model name="{name}">
+    <link name="body">
+      <collision name="body">
+        <pose>0 0 {size[2] / 2.0} 0 0 0</pose>
+        <geometry><box><size>{size[0]} {size[1]} {size[2]}</size></box></geometry>
+      </collision>
+    </link>
+  </model>
+</sdf>
+""",
+        encoding="utf-8",
+    )
+    return d
+
+
+def _scene(root: Path, objects, operating=None) -> Path:
+    import yaml
+
+    cfg = root / "devices/sim/gazebo/config"
+    cfg.mkdir(parents=True, exist_ok=True)
+    doc = {
+        "models_dir": MODELS_REL,
+        "table_top_z": 0.40,
+        "table_bounds": {"x": [0.15, 0.95], "y": [-0.60, 0.60]},
+        "objects": objects,
+    }
+    if operating is not None:
+        doc["operating"] = operating
+    p = cfg / "scene.yaml"
+    p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
+    return p
+
+
+def _obj(name="workpiece", x=0.5, y=0.0, z=0.40, yaw=0.0) -> dict:
+    return {"name": name, "model": name, "pose": {"x": x, "y": y, "z": z, "yaw": yaw}}
+
+
+# ---------------------------------------------------------------------------
+# 目标点 = 顶面中心
+# ---------------------------------------------------------------------------
+def test_target_is_top_center_not_origin(tmp_path):
+    """原点在**底面**，真值点要在**顶面** —— 差一个板厚，正是最容易漏的地方。"""
+    _model(tmp_path, "workpiece")  # 板厚 12mm
+    p = _scene(tmp_path, [_obj()])
+    _spec, tgt = load_truth_target(p, "", (0.0, 0.0, 0.0), root=tmp_path)
+    assert tgt.world_xyz[0] == pytest.approx(0.5)
+    assert tgt.world_xyz[1] == pytest.approx(0.0)
+    # 0.40（原点/台面）+ 0.012（板厚）= 0.412；写成 0.40 就会低一个板厚
+    assert tgt.world_xyz[2] == pytest.approx(0.412)
+    assert "12.0mm" in tgt.note
+
+
+def test_default_object_follows_scene_part_object(tmp_path):
+    """``target_object`` 空 = 场景里的 operating.part_object（不是写死 'workpiece'）。"""
+    _model(tmp_path, "plate")
+    p = _scene(
+        tmp_path,
+        [_obj("plate", y=0.2)],
+        operating={"camera": "sim_d405", "part_object": "plate"},
+    )
+    _spec, tgt = load_truth_target(p, "", (0.0, 0.0, 0.0), root=tmp_path)
+    assert tgt.object_name == "plate"
+    assert tgt.world_xyz[1] == pytest.approx(0.2)
+
+
+def test_offset_is_in_object_frame_so_yaw_rotates_it(tmp_path):
+    """偏移必须跟着 yaw 转：否则换了摆放角度就偏到工件外面。
+
+    局部 +x 偏 30mm，yaw=90° ⇒ 世界系里应落在 +y 方向。
+    """
+    _model(tmp_path, "workpiece")
+    p = _scene(tmp_path, [_obj(yaw=math.pi / 2.0)])
+    _spec, tgt = load_truth_target(p, "", (0.03, 0.0, 0.0), root=tmp_path)
+    assert tgt.world_xyz[0] == pytest.approx(0.5, abs=1e-9)
+    assert tgt.world_xyz[1] == pytest.approx(0.03, abs=1e-9)
+
+
+def test_offset_can_target_a_hole(tmp_path):
+    """把目标挪到孔上（验证二阶段对孔用），z 偏移是相对顶面的。"""
+    _model(tmp_path, "workpiece")
+    p = _scene(tmp_path, [_obj()])
+    _spec, tgt = load_truth_target(p, "", (0.0, -0.029, 0.0), root=tmp_path)
+    assert tgt.world_xyz[2] == pytest.approx(0.412)
+    assert tgt.world_xyz[1] == pytest.approx(-0.029)
+
+
+# ---------------------------------------------------------------------------
+# 出错时要给出可执行的修法
+# ---------------------------------------------------------------------------
+def test_unknown_object_lists_what_is_available(tmp_path):
+    _model(tmp_path, "workpiece")
+    p = _scene(tmp_path, [_obj()])
+    with pytest.raises(TruthError) as e:
+        load_truth_target(p, "not_there", (0.0, 0.0, 0.0), root=tmp_path)
+    msg = str(e.value)
+    assert "not_there" in msg
+    assert "workpiece" in msg  # 现有物体列出来，改配置不用翻文件
+    assert "perception.truth.object" in msg
+
+
+def test_model_without_collision_is_an_error_not_a_guess(tmp_path):
+    """没有碰撞体就**没有**几何 → 报错，不要拿别的数凑一个点出来。"""
+    d = tmp_path / MODELS_REL / "empty"
+    d.mkdir(parents=True)
+    (d / "model.sdf").write_text(
+        '<sdf version="1.6"><model name="empty"><link name="l"/></model></sdf>',
+        encoding="utf-8",
+    )
+    p = _scene(tmp_path, [_obj("empty")])
+    with pytest.raises(TruthError) as e:
+        load_truth_target(p, "empty", (), root=tmp_path)
+    assert "model.sdf" in str(e.value)
+
+
+def test_pick_object_reports_scene_path(tmp_path):
+    _model(tmp_path, "workpiece")
+    p = _scene(tmp_path, [_obj()])
+    spec = sim_scene.load_scene(p, root=tmp_path)
+    with pytest.raises(TruthError) as e:
+        pick_object(spec, "nope")
+    assert str(p) in str(e.value)
+
+
+# ---------------------------------------------------------------------------
+# 变换数学
+# ---------------------------------------------------------------------------
+def test_apply_transform_identity_translation_rotation():
+    import numpy as np
+
+    I = np.eye(4)
+    assert apply_transform(I, (1.0, 2.0, 3.0)) == pytest.approx((1.0, 2.0, 3.0))
+
+    T = np.eye(4)
+    T[:3, 3] = [0.1, -0.2, 0.3]
+    assert apply_transform(T, (1.0, 2.0, 3.0)) == pytest.approx((1.1, 1.8, 3.3))
+
+    R = np.eye(4)
+    R[:3, :3] = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]  # 绕 z 转 +90°
+    assert apply_transform(R, (1.0, 0.0, 0.0)) == pytest.approx((0.0, 1.0, 0.0))
+
+
+def test_matrix_from_transform_uses_the_shared_quaternion_math():
+    """守住"数学只有一份"：四元数→旋转矩阵必须走 handeye_lib，不许在这里再抄一遍。
+
+    以前这段在 4 个 launch 文件里各写一遍、错了 3 处，症状是"总能到附近、
+    永远差一点"，被当成手眼标定不准查了很久。
+    """
+    src = inspect.getsource(matrix_from_transform)
+    assert "quat_xyzw_to_R" in src, "四元数换算必须复用 handeye_lib.quat_xyzw_to_R"
+    assert "cos(" not in src and "sin(" not in src, "这里不该出现手写的三角换算"
+
+
+def test_matrix_from_transform_matches_rotation_about_z():
+    pytest.importorskip("arm_application.handeye_lib")
+    import numpy as np
+
+    s = math.sin(math.pi / 4.0)  # 90° → 四元数 (0,0,sin45,cos45)
+    tr = SimpleNamespace(
+        transform=SimpleNamespace(
+            translation=SimpleNamespace(x=1.0, y=2.0, z=3.0),
+            rotation=SimpleNamespace(x=0.0, y=0.0, z=s, w=s),
+        )
+    )
+    T = matrix_from_transform(tr)
+    assert T[:3, 3] == pytest.approx((1.0, 2.0, 3.0))
+    assert (T[:3, :3] @ np.array([1.0, 0.0, 0.0])) == pytest.approx((0.0, 1.0, 0.0), abs=1e-9)
+
+
+# ---------------------------------------------------------------------------
+# 发行版自带的场景：钉住"工件上表面到底在哪"
+# ---------------------------------------------------------------------------
+def test_shipped_scene_workpiece_top_is_12mm_above_the_table():
+    """官方场景里真值点必须落在**工件上表面**（台面 + 板厚 12mm）。
+
+    这条会随 scene.yaml / 模型板厚变化而失败 —— 故意的：
+    上表面高度是悬停高、跟踪高的推导依据，动了就该有人看一眼再更新别处。
+    """
+    spec, tgt = load_truth_target(None, "workpiece", (0.0, 0.0, 0.0))
+    obj = pick_object(spec, "workpiece")
+    assert tgt.world_xyz[2] == pytest.approx(obj.z + 0.012, abs=2e-3), (
+        f"真值点 z={tgt.world_xyz[2]:.4f} 与 scene.yaml 里工件位姿 "
+        f"z={obj.z:.4f} + 板厚 12mm 对不上：模型的板厚或摆放改了？"
+    )
+    # 台面之上、工作空间之内（真值是"看得见、够得着"的点）
+    assert spec.table_top_z < tgt.world_xyz[2] < 0.65
+    b = spec.table_bounds
+    assert b.x_min < tgt.world_xyz[0] < b.x_max
+    assert b.y_min < tgt.world_xyz[1] < b.y_max
+
+
+def test_offset_length_is_checked_not_guessed(tmp_path):
+    """长度不对必须报错，不能"缺的当 0" —— 猜错的表现是"偏了一点"，很难归因。"""
+    _model(tmp_path, "workpiece")
+    p = _scene(tmp_path, [_obj()])
+    with pytest.raises(TruthError) as e:
+        load_truth_target(p, "", (0.03,), root=tmp_path)
+    assert "3 个数" in str(e.value)
+
+
+def test_camera_backside_guard_is_positive():
+    """背后判据用 z<=0，且阈值是个正数（写成 0 会把贴镜头的点也发出去）。"""
+    assert MIN_CAM_Z_M > 0.0

src/arm_sim_gazebo/test/test_truth_holes.py

@@ -0,0 +1,295 @@
+"""``truth_holes`` 的测试：真值孔位怎么投影、什么时候**不给**结果。
+
+为什么这套测试重要：真值孔位是仿真里二阶段唯一的"测量来源"。它算错**不会
+报错**，只会让 S05 对准一个偏掉的点，然后现象出现在很远的地方（拧不进去 /
+掉件重放 / "标定好像不准"）。所以这里用一支手搓相机把几何关系钉死：
+相机正对工件时，孔必须落在哪、间距必须等于多少像素，全都算得出来。
+
+另外两类必须钉住：
+  * **不给幻影**：孔在相机背后时返回 ok=False（给伺服一个背后的目标会让
+    机械臂往错误方向走）。
+  * **环数不是 2 就不猜**：模型改了（多一个环/少一个环）必须报"几个环"，
+    而不是默默取前两个。
+"""
+
+from __future__ import annotations
+
+import math
+
+import numpy as np
+import pytest
+import yaml
+
+from arm_sim_gazebo import sim_scene
+from arm_sim_gazebo.sim_scene import SceneObject
+from arm_sim_gazebo.truth_holes import locate_holes, object_bbox_px, project_point
+
+MODELS_REL = "devices/sim/gazebo/models"
+
+
+# ---------------------------------------------------------------- 迷你仓库
+def _box_on_floor(name: str, size, xy=(0.0, 0.0)) -> str:
+    x, y = xy
+    return (
+        f'      <collision name="{name}">\n'
+        f'        <pose>{x} {y} {size[2] / 2.0} 0 0 0</pose>\n'
+        f"        <geometry><box><size>{size[0]} {size[1]} {size[2]}</size></box></geometry>\n"
+        f"      </collision>"
+    )
+
+
+def _cyl_visual(name: str, radius: float, pose="0 0 0.001 0 0 0") -> str:
+    return (
+        f'      <visual name="{name}">\n'
+        f"        <pose>{pose}</pose>\n"
+        f"        <geometry><cylinder><radius>{radius}</radius><length>0.001</length>"
+        f"</cylinder></geometry>\n"
+        f"      </visual>"
+    )
+
+
+def _write_model(root, name: str, collisions, visuals) -> None:
+    d = root / MODELS_REL / name
+    d.mkdir(parents=True, exist_ok=True)
+    (d / "model.sdf").write_text(
+        f"""<?xml version="1.0"?>
+<sdf version="1.6">
+  <model name="{name}">
+    <static>true</static>
+    <link name="link">
+{chr(10).join(collisions)}
+{chr(10).join(visuals)}
+    </link>
+  </model>
+</sdf>
+""",
+        encoding="utf-8",
+    )
+    (d / "model.config").write_text(
+        f"""<?xml version="1.0"?>
+<model>
+  <name>{name}</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+</model>
+""",
+        encoding="utf-8",
+    )
+
+
+def _write_scene(root, objects) -> None:
+    cfg = root / "devices/sim/gazebo/config"
+    cfg.mkdir(parents=True, exist_ok=True)
+    (cfg / "scene.yaml").write_text(
+        yaml.safe_dump(
+            {
+                "models_dir": MODELS_REL,
+                "table_top_z": 0.40,
+                "table_bounds": {"x": [0.15, 0.95], "y": [-0.60, 0.60]},
+                "operating": {"camera": "d405", "part_object": "workpiece",
+                              "camera_to_part_m": 0.17, "camera_offset_below_flange_m": 0.05},
+                "objects": objects,
+            },
+            allow_unicode=True,
+        ),
+        encoding="utf-8",
+    )
+
+
+# 工件：140×100×12 的板，两个嵌件环在局部 x=∓29mm、半径 7mm（与真机一致）
+PART_SIZE = (0.140, 0.100, 0.012)
+RING_R = 0.007
+RING_X = 0.029
+
+
+def _build_repo(root, rings=((-RING_X, 0.0), (RING_X, 0.0)), model="workpiece"):
+    """建一个迷你仓库：模型 + 场景。``rings`` 是各环的局部 (x, y)。"""
+    visuals = [_cyl_visual(f"insert_{i}_ring", RING_R, f"{x} {y} 0.010 0 0 0")
+               for i, (x, y) in enumerate(rings)]
+    visuals.append(_cyl_visual("center_bore", 0.020, "0 0 0.007 0 0 0"))
+    _write_model(root, model, [_box_on_floor("body", PART_SIZE)], visuals)
+    _write_scene(root, [{"name": "workpiece", "model": model,
+                         "pose": {"x": 0.50, "y": 0.0, "z": 0.40, "yaw": 0.0}}])
+    return sim_scene.load_scene(root=root)
+
+
+# ---------------------------------------------------------------- 手搓相机
+#: D405 的内参与图像尺寸（与仿真里一致），这样像素数可以直接对比现场日志。
+FX = FY = 446.819
+CX, CY = 424.0, 240.0
+IMAGE = (480, 848)
+INTR = (FX, FY, CX, CY)
+
+
+def _cam_looking_down(pos_world) -> np.ndarray:
+    """一支正朝下看的相机（光轴 = 世界 -z，像面 x = 世界 x）→ T_cam_world。
+
+    为什么用这支"理想相机"而不是从 TF 查：这是**纯几何测试**，要的是
+    "算出来的像素是否符合解析预期"，TF 正确性由现场脚本
+    （``scripts/check_sim_cameras.py``）负责。
+    """
+    R = np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]])
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = -R @ np.array([float(pos_world[0]), float(pos_world[1]), float(pos_world[2])])
+    return T
+
+
+# ---------------------------------------------------------------- 环的解析
+def test_rings_local_reads_only_cylindrical_ring_visuals(tmp_path):
+    _build_repo(tmp_path)
+    spec = sim_scene.load_scene(root=tmp_path)
+    obj = spec.objects[0]
+    rings = sim_scene.rings_local(spec, obj, tmp_path)
+    assert [r.name for r in rings] == ["insert_0_ring", "insert_1_ring"]
+    assert rings[0].x == pytest.approx(-RING_X)
+    assert rings[1].x == pytest.approx(RING_X)
+    assert rings[0].z == pytest.approx(0.010)
+    assert rings[0].radius == pytest.approx(RING_R)
+
+
+def test_rings_local_skips_ring_without_pose(tmp_path):
+    """没有 pose 的环**不收**：拿 (0,0,0) 当圆心会让"两孔间距"变成 0。"""
+    _write_model(
+        tmp_path,
+        "workpiece",
+        [_box_on_floor("body", PART_SIZE)],
+        [
+            '<visual name="insert_left_ring"><geometry>'
+            "<cylinder><radius>0.007</radius><length>0.001</length></cylinder>"
+            "</geometry></visual>",
+            _cyl_visual("insert_right_ring", RING_R, f"{RING_X} 0 0.010 0 0 0"),
+        ],
+    )
+    _write_scene(tmp_path, [{"name": "workpiece", "model": "workpiece",
+                             "pose": {"x": 0.5, "y": 0.0, "z": 0.4, "yaw": 0.0}}])
+    spec = sim_scene.load_scene(root=tmp_path)
+    rings = sim_scene.rings_local(spec, spec.objects[0], tmp_path)
+    assert [r.name for r in rings] == ["insert_right_ring"]
+
+
+def test_features_and_rings_agree_on_spacing(tmp_path):
+    """两处解析必须一致：``features_from_workpiece`` 说"该看到 58mm"，
+    ``rings_local`` 就得摆出 58mm —— 漂移了会出现"门槛说看得到、却投影不到那儿"。"""
+    _build_repo(tmp_path)
+    spec = sim_scene.load_scene(root=tmp_path)
+    obj = spec.objects[0]
+    feats = sim_scene.features_from_workpiece(spec, obj, tmp_path)
+    rings = sim_scene.rings_local(spec, obj, tmp_path)
+    assert feats["hole_spacing"] == pytest.approx(
+        math.hypot(rings[0].x - rings[1].x, rings[0].y - rings[1].y)
+    )
+    assert feats["insert_ring_radius"] == pytest.approx(rings[-1].radius)
+
+
+# ---------------------------------------------------------------- 投影
+def test_project_point_refuses_behind_camera():
+    T = _cam_looking_down((0.0, 0.0, 1.0))
+    # 相机朝下看：世界 z=0.5 的点在相机下方 → 正常；z=1.5 的点在相机**背后**
+    assert project_point(T, INTR, (0.0, 0.0, 0.5)) is not None
+    assert project_point(T, INTR, (0.0, 0.0, 1.5)) is None
+
+
+def test_locate_holes_projects_the_expected_pixels(tmp_path):
+    spec = _build_repo(tmp_path)
+    # 相机在工件上方 0.222m（= 环 z 0.410 与相机 0.632 的差，仿真里的作业距离）
+    T = _cam_looking_down((0.50, 0.0, 0.632))
+    res = locate_holes(T, INTR, IMAGE, spec=spec, root=tmp_path)
+    assert res.ok, res.why
+
+    # 间距：58mm 在 0.222m 处 = fx * 0.058 / 0.222
+    want = FX * (2 * RING_X) / 0.222
+    got = math.hypot(
+        res.hole_points[0][0] - res.hole_points[1][0],
+        res.hole_points[0][1] - res.hole_points[1][1],
+    )
+    assert got == pytest.approx(want, rel=0.01)
+    # 环半径：7mm 在 0.222m 处
+    assert res.hole_radii[0] == pytest.approx(FX * RING_R / 0.222, rel=0.01)
+    # 水平对 → 按图像 x 排序（k1=左）
+    assert res.hole_points[0][0] < res.hole_points[1][0]
+    assert res.hole_points[0][1] == pytest.approx(res.hole_points[1][1], abs=1.0)
+    # 两个孔都在画框里，而且近画面中心（相机正对工件中心）
+    for (u, v) in res.hole_points:
+        assert 0 <= u < IMAGE[1] and 0 <= v < IMAGE[0]
+        assert abs(u - CX) < 0.35 * IMAGE[1]
+
+
+def test_locate_holes_bbox_covers_the_part(tmp_path):
+    spec = _build_repo(tmp_path)
+    res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE,
+                       spec=spec, root=tmp_path)
+    assert res.ok, res.why
+    x1, y1, x2, y2 = res.bbox
+    # 140mm × 100mm 的板在 0.22m 处 ≈ 284px × 203px
+    assert (x2 - x1) == pytest.approx(FX * PART_SIZE[0] / 0.222, rel=0.06)
+    assert (y2 - y1) == pytest.approx(FY * PART_SIZE[1] / 0.222, rel=0.06)
+    # 孔必须落在工件的框里（否则 ROI 会把孔切掉）
+    for (u, v) in res.hole_points:
+        assert x1 < u < x2 and y1 < v < y2
+
+
+def test_locate_holes_orders_vertical_pair_by_y(tmp_path):
+    """工件转 90° 后两孔在图上变成竖排 → 按 y 排序（k1=上），与 Hough 路径同规则。"""
+    _build_repo(tmp_path)
+    cfg = tmp_path / "devices/sim/gazebo/config/scene.yaml"
+    doc = yaml.safe_load(cfg.read_text(encoding="utf-8"))
+    doc["objects"][0]["pose"]["yaw"] = math.pi / 2
+    cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
+    spec = sim_scene.load_scene(root=tmp_path)
+    res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE,
+                       spec=spec, root=tmp_path)
+    assert res.ok, res.why
+    assert res.hole_points[0][1] < res.hole_points[1][1]      # 上者在前
+    assert res.hole_points[0][0] == pytest.approx(res.hole_points[1][0], abs=1.0)
+
+
+def test_locate_holes_says_why_when_camera_is_below_the_part(tmp_path):
+    spec = _build_repo(tmp_path)
+    # 相机位置比工件上表面还低 → 环在相机背后
+    res = locate_holes(_cam_looking_down((0.50, 0.0, 0.30)), INTR, IMAGE,
+                       spec=spec, root=tmp_path)
+    assert not res.ok
+    assert "背后" in res.why
+
+
+def test_locate_holes_requires_exactly_two_rings(tmp_path):
+    spec = _build_repo(tmp_path, rings=((0.0, 0.0),))
+    res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE,
+                       spec=spec, root=tmp_path)
+    assert not res.ok
+    assert "1 个" in res.why
+
+
+def test_locate_holes_without_tf_says_so(tmp_path):
+    _build_repo(tmp_path)
+    res = locate_holes(None, INTR, IMAGE, root=tmp_path)
+    assert not res.ok
+    assert "TF" in res.why
+
+
+def test_locate_holes_unknown_object_lists_existing(tmp_path):
+    spec = _build_repo(tmp_path)
+    res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE,
+                       spec=spec, object_name="没有这个", root=tmp_path)
+    assert not res.ok
+    assert "workpiece" in res.why           # 现有名字要列出来，省得再去翻场景文件
+
+
+def test_object_bbox_px_returns_none_when_everything_is_behind(tmp_path):
+    spec = _build_repo(tmp_path)
+    obj = spec.objects[0]
+    boxes = sim_scene.world_boxes(spec, obj, tmp_path)
+    T = _cam_looking_down((0.50, 0.0, -1.0))     # 相机在台面下、朝下 → 工件在背后
+    assert object_bbox_px(T, INTR, boxes, IMAGE) is None
+
+
+def test_object_bbox_px_clips_to_image():
+    """框必须裁到画面内：给 Hough 一个超出画面的 ROI，裁剪会静默改变 ROI 尺寸。"""
+    spec_box = [(0.0, 10.0, 0.0, 10.0, 0.0, 0.01)]
+    T = _cam_looking_down((0.0, 0.0, 1.0))
+    bbox = object_bbox_px(T, INTR, spec_box, IMAGE)
+    assert bbox is not None
+    x1, y1, x2, y2 = bbox
+    assert 0 <= x1 <= x2 <= IMAGE[1] - 1
+    assert 0 <= y1 <= y2 <= IMAGE[0] - 1