仿真感知：真值孔位 + 真值相机 + 补 /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>

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:
-            problems.append(
-                f"只看见 {name} 的 {got:.1f}%，要求 ≥{want_min:.0f}%。\n"
-                f"      相机很可能是**背对**目标（位姿 rpy 写错）—— "
-                f"这种错几何自洽、不报错，只会让相机拍不到该拍的东西。"
+        if got >= want_min:
+            continue
+        if abs(want_z - table_z) < 1e-9 and above_table is False:
+            # 腕相机不在台面上方时，"没看见台面"是正常的（它看见的是地面）。
+            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,