P5c-6 场景物料落地 + Gazebo 中文手册：工件/通止规模型、场景校验、两处静默失效修复

一、场景物料（新增）
  devices/sim/gazebo/models/workpiece/   带三个孔的铝板。碰撞体是"带方孔的平板"，
    视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔，
    只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。
  devices/sim/gazebo/models/gauge/       通止规：通端/套环/握柄/止端四段，各段异色
    便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"，
    用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。
  devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒，
    加个方孔底座把它立住。
  三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。

二、场景配置与校验（新增）
  config/scene.yaml     物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。
    理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。
    动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。
    含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。
  sim_scene.py          摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移），
    检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。
    重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 ——
    正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报，
    而误报的后果是人把检查整个关掉，之后真重叠也抓不到。
    另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。
  scripts/check_sim_scene.py  验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 +
    悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。

三、修掉两处静默失效（都是"看起来一切正常"）
  1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析，
     而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 →
     调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**，
     而日志只说"跳过"，看起来像正常降级。
     现在两种格式都支持，并用真实抓下来的输出样本做回归测试。
     教训：宽容降级会把"没检查"伪装成"检查通过"。
  2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）：
     脚本报"已停止"，它却还在发上一份 /robot_description 与 TF，
     下一次启动就会"改了描述却不生效"。
     顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报
     一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。
     现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**：
     有残留就报出来并非零退出 —— 下次再有漏项，不再静默。
     实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。

四、相机：D405 分辨率对齐真机
  仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是
  58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 ——
  孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。
  已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场"
  扩成"视场 + 图幅"（sim_cameras.check_camera_consistency），
  不一致时说明它对像素门槛的影响。

五、文档
  devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 /
    配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。
  manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。
  scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。
  cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。

六、测试
  新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是
    位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条），
  含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、
  center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。
  全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。

同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步，
它的 parity 测试是红的）。

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

scripts/README.md

@@ -15,10 +15,15 @@
   - 规则写在 [models/README.md](../models/README.md)。`export_yolo_trt.sh` 现在是它的兼容转发壳。
 **配置：** `show_site_config.sh` · `lib/export_site_env.py`  
 **仿真：** `run_sim_gz.sh`（Gazebo 物理仿真，唯一能做接触/力控验证的入口）
+  停：`stop_sim_gz.sh`（**别敲 `pkill -f gz`** —— 会杀掉调用它的 shell，见脚本文件头）
   - 环境是独立 conda 环境 `ros_gz_sim`（`install_sim_env.sh` 装、`sim_env.sh` 激活），与真机环境隔离
   - 相机验收：`python3 scripts/check_sim_cameras.py` —— **别用 `ros2 topic hz` 或肉眼看图验收相机**，
     仿真相机的错误几乎全是静默的（深度单位、frame_id、内参、位姿方向），都能出图却是错的。
     这条命令把深度反投影到 world，要求命中已知平面（地面/台面）才算通过。
+  - 场景验收：`python3 scripts/check_sim_scene.py` —— 同样别靠肉眼。它管四件事：
+    物体的摆放（悬空/穿模/出界/互相嵌入）、场景里的像素门槛与**真机生效值**是否漂移、
+    孔检测的成像几何（亮环/孔间距换算成像素后够不够门槛）、以及粗定位用的手外相机
+    能不能**完整**看见工件。第二条尤其重要：门槛抄了两份，不一致时仿真会"通过"而真机不认。
   - 注意 `run_sim.sh` 不是 Gazebo，它走 mock 驱动（假数据 + RViz），永远不会产生接触力
 **环境：** `env.sh` · `build_all.sh`  
 **RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh` · `rdk_drag_bridge.py`  

scripts/check_sim_scene.py

@@ -0,0 +1,438 @@
+#!/usr/bin/env python3
+"""仿真场景验收：摆放对不对、孔检测的成像几何够不够、粗定位看不看得见工件。
+
+=============================================================================
+为什么需要这个工具
+=============================================================================
+场景里的错误和相机一样，几乎都是**静默**的 —— 跑得起来、报不出错，
+只是"结果不对"。这个脚本把四类问题在启动阶段就变成可打印的结论：
+
+  1. 摆放讲不通：悬空 / 穿进台面 / 超出台面 / 两个物体互相嵌入。
+     表现是"工件自己会动、会飞"，而人会怀疑控制或标定。
+
+  2. 场景文件里的像素门槛与真机配置**漂移**。
+     scene.yaml 为了能在启动时就算出"能不能看见"，抄了一份门槛值；
+     抄一份就有漂移风险。这里读 hole_detect.yaml + site.yaml 的生效值做核对，
+     不一致直接判失败 —— 不允许"仿真按旧门槛通过、真机按新门槛不认"。
+
+  3. 成像几何不够用：亮环太小 / 两孔间距不够像素门槛。
+     这些门槛是**像素**量，只有在仿真侧用几何算出来才能提前发现；
+     等上真机再发现要贵得多（要重新调悬停高度、甚至改工件工装）。
+
+  4. 二阶段的悬停高度**超出机械臂工作空间**。
+     这个坑很实在：相机到工件的距离决定了像素尺度，而"距离够近"与
+     "法兰别抬太高"是一对矛盾。脚本会把两者都算出来并核对工作空间上下界。
+
+  5. 粗定位（D435）看不看得见工件。
+     二阶段粗居中靠 D435 出工件的 bbox；工件被裁掉一角时检测仍会出结果
+     （框不全而已），于是算出的中心偏掉 —— 这属于"能出结果但结果是错的"。
+
+用法（需要 ROS 环境以读 cameras.yaml 内参；查 Gazebo 位姿需要仿真在跑）：
+    source scripts/sim_env.sh
+    python3 scripts/check_sim_scene.py
+    python3 scripts/check_sim_scene.py --no-poses      # 跳过"查 Gazebo 实际位姿"
+
+退出码 0 = 通过；非 0 = 有问题（可直接用于启动自检 / CI）。
+=============================================================================
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import subprocess
+import sys
+from pathlib import Path
+from typing import Optional
+
+REPO_ROOT = Path(__file__).resolve().parents[1]
+
+
+# ---------------------------------------------------------------------------
+# 小工具
+# ---------------------------------------------------------------------------
+def rpy_to_R(r: float, p: float, y: float):
+    """rpy（ZYX 复合顺序：R = Rz·Ry·Rx）→ 3x3 旋转矩阵。
+
+    顺序不能想当然：URDF/SDF 的 rpy 都是 Rz·Ry·Rx。
+    "先俯 50° 再偏航 180°" 与 "先偏航再俯" 的结果**不同**，
+    这个项目就为此把 D435 的视线翻到背后过（见 assembly.world 的注释）。
+    """
+    cr, sr = math.cos(r), math.sin(r)
+    cp, sp = math.cos(p), math.sin(p)
+    cy, sy = math.cos(y), math.sin(y)
+    return [
+        [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
+        [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
+        [-sp, cp * sr, cp * cr],
+    ]
+
+
+def matmul3(a, b):
+    return [[sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
+
+
+def matvec3(a, v):
+    return [sum(a[i][k] * v[k] for k in range(3)) for i in range(3)]
+
+
+def transpose3(a):
+    return [[a[j][i] for j in range(3)] for i in range(3)]
+
+
+# ---------------------------------------------------------------------------
+# 门槛核对：scene.yaml 抄的那份 vs 真机真正生效的那份
+# ---------------------------------------------------------------------------
+def effective_gates(root: Optional[Path] = None) -> dict:
+    """读出真机**实际生效**的孔检测像素门槛。
+
+    生效规则（见 arm_application/hole_detector.config_from_site）：
+    包内 config/hole_detect.yaml 给默认值，config/site.yaml 的 hole_detect.* 覆盖它。
+
+    半径取两个区间的**交集**：HoughCircles 的搜索范围（hough_min/max_radius）
+    与工业校验的接受范围（min_radius/max_radius）必须同时满足才叫"能被用上"。
+    只按其中一个判，会得出"能检测到"的乐观结论，而实际在另一个环节被滤掉。
+    """
+    import yaml
+
+    base_root = Path(root) if root is not None else REPO_ROOT
+    pkg = base_root / "src/arm_application/config/hole_detect.yaml"
+    site = base_root / "config/site.yaml"
+    if not site.is_file():
+        site = base_root / "config/site.example.yaml"
+
+    base = {}
+    if pkg.is_file():
+        base = yaml.safe_load(pkg.read_text(encoding="utf-8")) or {}
+    over = {}
+    if site.is_file():
+        doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
+        over = (doc.get("hole_detect") or {}) if isinstance(doc, dict) else {}
+
+    def val(key, default):
+        v = over.get(key, base.get(key, default))
+        try:
+            return float(v)
+        except (TypeError, ValueError):
+            return float(default)
+
+    lo = max(val("hough_min_radius", 10), val("min_radius", 10))
+    hi = min(val("hough_max_radius", 30), val("max_radius", 30))
+    return {
+        "insert_ring_radius": (lo, hi),
+        "hole_spacing": (val("hole_distance_min", 150), val("hole_distance_max", 700)),
+        "_source": (str(site), str(pkg)),
+    }
+
+
+def workspace_z_limits(root: Optional[Path] = None):
+    """从 site.yaml 读机械臂法兰的工作空间 Z 上下界（用于核对悬停高度）。"""
+    import yaml
+
+    base_root = Path(root) if root is not None else REPO_ROOT
+    site = base_root / "config/site.yaml"
+    if not site.is_file():
+        site = base_root / "config/site.example.yaml"
+    if not site.is_file():
+        return None
+    doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
+    servo = doc.get("servo") or {}
+    try:
+        return float(servo["workspace_xyz_min"][2]), float(servo["workspace_xyz_max"][2])
+    except (KeyError, IndexError, TypeError, ValueError):
+        return None
+
+
+# ---------------------------------------------------------------------------
+# 检查项
+# ---------------------------------------------------------------------------
+def check_placement(scene, root) -> list[str]:
+    from arm_sim_gazebo.sim_scene import validate_scene
+
+    return validate_scene(scene, root)
+
+
+def check_gate_drift(scene, root=None) -> list[str]:
+    """scene.yaml 的 gates_px 是否与真机生效值一致。"""
+    eff = effective_gates(root)
+    out = []
+    for key, gate in scene.gates_px.items():
+        want = eff.get(key)
+        if want is None:
+            continue
+        if abs(gate[0] - want[0]) > 1e-6 or abs(gate[1] - want[1]) > 1e-6:
+            out.append(
+                f"gates_px.{key} 写的是 [{gate[0]:g}, {gate[1]:g}]，"
+                f"而真机生效值是 [{want[0]:g}, {want[1]:g}]。\n"
+                f"      真机值来自 {eff['_source'][0]}（覆盖 {eff['_source'][1]}）。\n"
+                f"      这不是小事：两边不一致时，仿真会按旧门槛判'能检测到'，"
+                f"而真机按新门槛不认 —— 属于最费时间的一类问题。\n"
+                f"      修法：把 devices/sim/gazebo/config/scene.yaml 的 gates_px 改成真机值。"
+            )
+    missing = set(eff) - {"_source"} - set(scene.gates_px)
+    for key in sorted(missing):
+        out.append(
+            f"gates_px 里缺 {key}（真机有这个门槛 {eff[key]}）—— "
+            f"缺了就不会校验它，属于「静默放行」。补上它。"
+        )
+    return out
+
+
+def check_geometry(scene, root) -> tuple[list[str], dict]:
+    """孔检测的像素几何：算出实际像素值，与门槛比。"""
+    from arm_sim_gazebo.sim_cameras import intrinsics, load_cameras
+    from arm_sim_gazebo.sim_scene import check_pixel_gates, operating_pixel_report
+
+    specs = load_cameras(root=root)
+    cam = next((s for s in specs if s.ros_ns == scene.operating.camera), None)
+    if cam is None:
+        return [f"cameras.yaml 里没有相机 {scene.operating.camera}"], {}
+    k, _ = intrinsics(cam, cam.width or 640, cam.height or 480)
+    px = operating_pixel_report(scene, k[0], root)
+    if not px:
+        return ["读不出工件模型里的检测特征（visual 名字约定变了？见 sim_scene.features_from_workpiece）"], {}
+    return check_pixel_gates(px, scene.gates_px), px
+
+
+def check_hover_height(scene, root) -> list[str]:
+    """悬停高度是否落在机械臂工作空间的 Z 区间内。"""
+    from arm_sim_gazebo.sim_scene import operating_flange_z
+
+    fz = operating_flange_z(scene, root)
+    if fz is None:
+        return ["读不出工件高度，算不出建议悬停高度"]
+    lim = workspace_z_limits(root)
+    if lim is None:
+        return []
+    lo, hi = lim
+    out = []
+    if fz > hi:
+        out.append(
+            f"按当前 camera_to_part_m={scene.operating.camera_to_part_m}m 算出的悬停法兰高度 "
+            f"z≈{fz:.3f}m **超出工作空间上界 {hi}m**。\n"
+            f"      二阶段的位姿检查会直接拒绝执行。\n"
+            f"      两个旋钮：调小 camera_to_part_m（相机更贴近工件 → 像素尺度更大、"
+            f"更容易过像素门槛），或把工件/台面改矮。\n"
+            f"      注意这两者是对立的：离得越近像素越大（检测更好），但法兰越高（越够不着）。"
+        )
+    elif fz < lo:
+        out.append(f"算出的悬停法兰高度 z≈{fz:.3f}m 低于工作空间下界 {lo}m。")
+    return out
+
+
+def check_d435_visibility(scene, root) -> tuple[list[str], str]:
+    """把工件顶面的四角投影到 D435 画面，看是否完整可见。
+
+    这一步对应二阶段的**粗定位**：D435 出工件的 bbox，取其中心作为粗目标。
+    工件被裁掉一角时检测照样出结果（框小了而已），算出的中心就偏了 ——
+    "能出结果但结果是错的"，正是这个项目反复强调的那一类。
+    """
+    from arm_sim_gazebo.sim_cameras import intrinsics, load_cameras
+    from arm_sim_gazebo.sim_scene import world_boxes
+    from arm_sim_gazebo.robot_description import model_link_pose
+    from arm_sim_gazebo.sim_site import resolve_world
+
+    specs = load_cameras(root=root)
+    cam = next((s for s in specs if s.is_world_mounted), None)
+    if cam is None:
+        return [], "（cameras.yaml 里没有世界相机，跳过）"
+
+    part = next((o for o in scene.objects if o.name == scene.operating.part_object), None)
+    if part is None:
+        return [f"场景里没有 {scene.operating.part_object!r}，无法核对粗定位可见性"], ""
+
+    world_path, _ = resolve_world("", root)
+    try:
+        (px, py, pz), (rr, rp, ry) = model_link_pose(Path(world_path), cam.world_model, cam.world_link)
+    except Exception as exc:
+        return [f"读不出 {cam.world_model} 在世界里的位姿：{exc}"], ""
+
+    boxes = world_boxes(scene, part, root)
+    if not boxes:
+        return ["读不出工件的碰撞体，无法投影"], ""
+    top = max(b[5] for b in boxes)
+    x0 = min(b[0] for b in boxes)
+    x1 = max(b[1] for b in boxes)
+    y0 = min(b[2] for b in boxes)
+    y1 = max(b[3] for b in boxes)
+    corners = [(x0, y0, top), (x0, y1, top), (x1, y0, top), (x1, y1, top)]
+
+    w, h = cam.width or 640, cam.height or 480
+    k, _ = intrinsics(cam, w, h)
+    fx, fy, cx, cy = k[0], k[4], k[2], k[5]
+
+    R_wb = rpy_to_R(rr, rp, ry)
+    # 本体 → 光学：相机本体是"X 朝前/Y 左/Z 上"，光学系是"Z 前/X 右/Y 下"
+    R_bo = rpy_to_R(-math.pi / 2, 0.0, -math.pi / 2)
+    R_wo = matmul3(R_wb, R_bo)
+    R_ow = transpose3(R_wo)
+    t = [px, py, pz]
+
+    probs: list[str] = []
+    us, vs = [], []
+    for c in corners:
+        d = [c[i] - t[i] for i in range(3)]
+        o = matvec3(R_ow, d)
+        if o[2] <= 1e-6:
+            probs.append(
+                f"工件有一个角在 {cam.ros_ns} 的**背后**（光学系 z={o[2]:.3f}）——"
+                f"相机朝向反了或工件跑到相机后面了。"
+            )
+            return probs, ""
+        us.append(cx + fx * o[0] / o[2])
+        vs.append(cy + fy * o[1] / o[2])
+
+    u0, u1, v0, v1 = min(us), max(us), min(vs), max(vs)
+    info = (f"{cam.ros_ns}: 工件顶面四角投影 u∈[{u0:.0f},{u1:.0f}] v∈[{v0:.0f},{v1:.0f}] "
+            f"（画面 {w}x{h}）")
+    if u0 < 0 or u1 > w or v0 < 0 or v1 > h:
+        probs.append(
+            f"工件没有完整落在 {cam.ros_ns} 画面内（{info}）。\n"
+            f"      粗定位会框不全工件 → bbox 中心偏 → **照样出结果但结果是错的**。\n"
+            f"      修法：挪工件（scene.yaml 的 pose）、挪相机（world 里的 pose），"
+            f"或确认这台相机的视野本来就该覆盖整个台面。"
+        )
+    return probs, info
+
+
+def check_gazebo_poses(scene, root) -> tuple[list[str], str]:
+    """查 Gazebo 里物体的实际位姿，与 scene.yaml 对照。
+
+    这一步能抓到"spawn 了但掉下去/被顶飞"这类只在运行后暴露的问题。
+    需要仿真在跑；不在跑就跳过（不算失败）。
+    """
+    from arm_sim_gazebo.sim_scene import parse_spawned_poses, pose_mismatches
+
+    try:
+        r = subprocess.run(
+            ["ign", "model", "--list"],
+            capture_output=True,
+            text=True,
+            timeout=20,
+        )
+    except (OSError, subprocess.TimeoutExpired) as exc:
+        return [], f"（跳过：调不动 ign（{exc}））"
+    if r.returncode != 0 or "Requesting state" not in (r.stdout + r.stderr):
+        return [], "（跳过：Gazebo 没在跑）"
+
+    actual = {}
+    for obj in scene.objects:
+        try:
+            rr = subprocess.run(
+                ["ign", "model", "-m", obj.name, "-p"],
+                capture_output=True,
+                text=True,
+                timeout=20,
+            )
+        except (OSError, subprocess.TimeoutExpired):
+            continue
+        got = parse_spawned_poses(rr.stdout + rr.stderr)
+        if obj.name in got:
+            actual[obj.name] = got[obj.name]
+    if not actual:
+        return [], "（跳过：解析不出 Gazebo 的位姿输出）"
+    return pose_mismatches(scene, actual), f"（已在 Gazebo 里核对 {len(actual)} 个物体）"
+
+
+# ---------------------------------------------------------------------------
+def main() -> int:
+    ap = argparse.ArgumentParser(
+        description="仿真场景验收：摆放 + 孔检测成像几何 + 粗定位可见性。",
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+    )
+    ap.add_argument("--scene", default="", help="场景文件路径（留空=仓库默认）")
+    ap.add_argument("--no-poses", action="store_true", help="跳过「查 Gazebo 实际位姿」")
+    args = ap.parse_args()
+
+    try:
+        from arm_sim_gazebo.sim_scene import SceneError, load_scene
+    except ImportError as exc:
+        print(
+            f"✘ 导入 arm_sim_gazebo 失败：{exc}\n"
+            f"  先执行： source scripts/sim_env.sh && . install_sim/setup.bash",
+            file=sys.stderr,
+        )
+        return 2
+
+    root = REPO_ROOT
+    try:
+        scene = load_scene(Path(args.scene) if args.scene else None, root=root)
+    except SceneError as exc:
+        print(f"✘ 场景配置读不了：\n{exc}", file=sys.stderr)
+        return 2
+
+    print(f"场景文件: {scene.source}")
+    print(f"模型目录: {scene.models_dir}   台面顶面 z={scene.table_top_z}")
+    print("物体:")
+    for o in scene.objects:
+        print(f"    {o.name:<14} model={o.model:<14} pose=({o.x:+.3f}, {o.y:+.3f}, {o.z:+.3f}) yaw={o.yaw:+.3f}")
+
+    problems: list[str] = []
+
+    print("\n[1] 摆放校验（悬空/穿模/出界/嵌入）")
+    p = check_placement(scene, root)
+    problems += p
+    print("    ✓ 通过" if not p else "\n".join(f"    ✘ {x}" for x in p))
+
+    print("\n[2] 像素门槛与真机配置核对")
+    p = check_gate_drift(scene, root)
+    problems += p
+    print("    ✓ 与真机生效值一致" if not p else "\n".join(f"    ✘ {x}" for x in p))
+
+    print("\n[3] 孔检测成像几何")
+    p, px = check_geometry(scene, root)
+    problems += p
+    for k, v in sorted(px.items()):
+        gate = scene.gates_px.get(k)
+        tag = f"门槛 [{gate[0]:g}, {gate[1]:g}]" if gate else "无门槛"
+        ok = "" if (gate is None or gate[0] <= v <= gate[1]) else "  ← 不合格"
+        print(f"    {k:<22} = {v:7.1f}px   {tag}{ok}")
+    if p:
+        print("\n".join(f"    ✘ {x}" for x in p))
+
+    print("\n[4] 二阶段悬停高度 vs 工作空间")
+    p = check_hover_height(scene, root)
+    problems += p
+    from arm_sim_gazebo.sim_scene import operating_flange_z
+
+    fz = operating_flange_z(scene, root)
+    lim = workspace_z_limits(root)
+    if fz is not None:
+        print(f"    建议悬停法兰 z≈{fz:.3f}m"
+              + (f"   工作空间 z∈[{lim[0]}, {lim[1]}]" if lim else ""))
+        print(
+            "    （真机 site.yaml 写的是 0.23 —— 两者不同是已知的："
+            "决定成像的是相机到工件的距离，不是绝对高度）"
+        )
+    if p:
+        print("\n".join(f"    ✘ {x}" for x in p))
+
+    print("\n[5] 粗定位可见性（工件是否完整落在手外相机画面内）")
+    p, info = check_d435_visibility(scene, root)
+    problems += p
+    if info:
+        print(f"    {info}")
+    if p:
+        print("\n".join(f"    ✘ {x}" for x in p))
+    elif info and not info.startswith("（"):
+        print("    ✓ 完整可见")
+
+    if not args.no_poses:
+        print("\n[6] Gazebo 实际位姿核对")
+        p, info = check_gazebo_poses(scene, root)
+        problems += p
+        if info:
+            print(f"    {info}")
+        print("    ✓ 一致" if not p else "\n".join(f"    ✘ {x}" for x in p))
+
+    print()
+    if problems:
+        print(f"=== ✘ 不通过：{len(problems)} 个问题 ===")
+        print("    场景与成像的问题大多是静默的：跑得起来、报不出错，只是结果不对。")
+        return 1
+    print("=== ✓ 全部通过 ===")
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

scripts/stop_sim_gz.sh

@@ -0,0 +1,155 @@
+#!/usr/bin/env bash
+# =============================================================================
+# scripts/stop_sim_gz.sh —— 干净地停掉 Gazebo 仿真栈
+#
+# 为什么要专门写一个脚本，而不是让你敲 pkill：
+#
+#   ⚠️ `pkill -f <模式>` 在"用脚本启动仿真"的场景下会**把自己的启动壳一起杀掉**。
+#      因为那条命令自己的命令行里就含那个模式字符串，pkill -f 匹配整条命令行，
+#      于是它先匹配到自己所在的 shell。表现是：命令返回得飞快、什么输出都没有、
+#      仿真也没停成 —— 而这看起来像脚本写错了，极难往"进程匹配"上想。
+#
+#   本脚本的做法：扫描候选 PID → 排除自己/祖先/扫描自身的临时进程 → kill，
+#   最后**再扫一遍复查**（杀掉后仍有残留就报出来）。
+#   所以它可以在任何调用链里安全使用（包括被其它脚本 `source`/调用）。
+#
+# 用法：
+#     bash scripts/stop_sim_gz.sh            # 停仿真（TERM，等 5s，仍在则 KILL）
+#     bash scripts/stop_sim_gz.sh --list     # 只列出会被停掉的进程，不动手
+# =============================================================================
+set -uo pipefail
+
+LIST_ONLY=0
+[ "${1:-}" = "--list" ] && LIST_ONLY=1
+
+# 这些模式覆盖仿真栈的全部进程：
+#   gz sim / ign gazebo   服务端与 GUI（conda 的 Fortress 里可执行名是 ign gazebo）
+#   gazebo_sim.launch.py   ros2 launch 的壳
+#   gazebo_sim_launcher    入口可执行文件（展开描述后转交 launch）
+#   parameter_bridge       相机/触觉/clock 的桥接
+#   sim_camera_adapter / sim_tactile_adapter  两个适配器
+#   robot_state_publisher  发 TF 与 /robot_description
+#   controller_manager.*spawner / gz_ros2_control  控制器
+#
+# ⚠️ 这里漏过一个（robot_state_publisher），症状是"脚本说已停止，但它还在跑"：
+#    下一次启动时它继续发上一份 /robot_description 与 TF，而两个进程的
+#    参数可能不同 —— 表现成"改了描述却不生效"，极难往"有个残留进程"上想。
+#    所以脚本末尾加了一次**复查**：杀完再扫一遍，有残留就报出来并非零退出。
+#    加进新进程时，务必确认复查也能扫到它。
+#
+# 有意**不**包含 rviz2：本仿真栈的 headless 模式不起 RViz，而用户可能同时
+# 开着 RViz 看别的东西。把它写进来会误杀无关进程，比漏杀更糟。
+PATTERNS='ign gazebo|gz sim|gazebo_sim\.launch\.py|gazebo_sim_launcher|parameter_bridge|sim_camera_adapter|sim_tactile_adapter|robot_state_publisher|controller_manager.*spawner|gz_ros2_control'
+
+# ---------------------------------------------------------------------------
+# 扫描：输出 "PID<TAB>命令行"，供两次调用（收集 / 复查）共用。
+#
+# 两处必须共用同一个实现，否则"复查用的模式"和"杀的时候用的模式"会各自漂移，
+# 而漂移的表现就是"说已停止、其实有残留"——正是这个脚本要防的事。
+#
+# ⚠️ 模式串用**环境变量**传给 awk，不用 `-v pat=...`：
+#    用了 -v 的话，awk 自己的命令行里就含整串模式，于是它匹配到自己。
+#    这是 pkill -f 那个坑的同款，只是换了个位置 —— 实测表现为每次复查都会
+#    多报出一个"没有命令行"的幽灵 PID（awk 已经退出，ps 查不到它了）。
+#
+# 另外用 ps 而不是 pgrep -f：ps 的输出里可以用 awk 精确控制字段与排除规则。
+scan_pids() {
+    # ⚠️ 必须 export：`VAR=x ps ... | awk` 只会把 VAR 给 ps，awk 看不到。
+    export PAT="$PATTERNS"
+    ps -eo pid=,ppid=,args= | awk '
+        BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
+        $0 ~ pat {
+            if (NF < 3) next                 # 没有 args 字段（僵尸/内核线程）：跳过
+            pid = $1; ppid = $2
+            args = substr($0, index($0, $3))
+            if (ppid == me) next             # 本脚本的直接子进程（进程替换 fork 出的子 shell）
+            if (args ~ /stop_sim_gz\.sh/) next   # 本脚本自己 / 调用它的壳的文本
+            if (args ~ /^\s*awk/) next       # 扫描用的 awk 本身
+            print pid "\t" args
+        }'
+}
+export ME=$$
+
+# 排除自己与全部祖先 —— 这是本脚本存在的核心理由（见文件头）。
+exclude=()
+p=$$
+while [ "$p" -gt 1 ] 2>/dev/null; do
+    exclude+=("$p")
+    p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
+    [ -z "$p" ] && break
+done
+
+# 从扫描结果里剔掉自己/祖先，只输出 PID。
+# 用 `cut -f1` 而不是 read 到数组再拼接，避免命令行里再出现长的进程描述。
+filter_targets() {
+    local pid
+    while read -r pid _rest; do
+        [ -z "$pid" ] && continue
+        local skip=0
+        for e in "${exclude[@]}"; do
+            [ "$pid" = "$e" ] && skip=1 && break
+        done
+        [ "$skip" = "1" ] && continue
+        echo "$pid"
+    done
+}
+
+mapfile -t SCAN < <(scan_pids)
+mapfile -t targets < <(printf '%s\n' "${SCAN[@]:-}" | filter_targets)
+
+if [ "${#targets[@]}" = "0" ]; then
+    echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
+    exit 0
+fi
+
+echo "将停止以下进程："
+for pid in "${targets[@]}"; do
+    printf '  %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
+done
+
+if [ "$LIST_ONLY" = "1" ]; then
+    echo "（--list：未动手）"
+    exit 0
+fi
+
+for pid in "${targets[@]}"; do
+    kill -TERM "$pid" 2>/dev/null
+done
+
+# 给 HAL 一点时间退出。Gazebo 的渲染线程与 DART 的求解线程不会立刻收尾，
+# 立刻 KILL 有可能留下共享内存段，下一次启动表现成"渲染初始化失败"。
+for _ in $(seq 1 10); do
+    alive=0
+    for pid in "${targets[@]}"; do
+        kill -0 "$pid" 2>/dev/null && alive=1
+    done
+    [ "$alive" = "0" ] && break
+    sleep 0.5
+done
+
+for pid in "${targets[@]}"; do
+    if kill -0 "$pid" 2>/dev/null; then
+        echo "  PID $pid 没响应 TERM，用 KILL"
+        kill -KILL "$pid" 2>/dev/null
+    fi
+done
+
+# ---------------------------------------------------------------------------
+# 复查：杀完再扫一遍。
+# 这一步是为了让"模式漏了某个进程"变成**看得见的告警**，而不是
+# "脚本说已停止、实际有残留"。robot_state_publisher 就这么漏过一次。
+# ---------------------------------------------------------------------------
+sleep 0.5
+mapfile -t SCAN2 < <(scan_pids)
+mapfile -t leftover < <(printf '%s\n' "${SCAN2[@]:-}" | filter_targets)
+
+if [ "${#leftover[@]}" != "0" ]; then
+    echo "⚠ 仍有仿真相关进程存活（说明上面的模式没覆盖它，请把它加进 PATTERNS）："
+    for pid in "${leftover[@]}"; do
+        printf '  %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
+    done
+    echo "仿真已停止，但有残留 —— 残留会与下一次启动抢话题/TF。"
+    exit 3
+fi
+
+echo "仿真已停止。"