#!/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 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):
    """机械臂法兰的工作空间 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 = sim_robot_servo(doc)
    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。")

    # 一阶段跟踪高还要再抬 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


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_table_slot(scene, root) -> tuple[list[str], str]:
    """台面通槽 vs 工件：既要"规的通端过得去"，又要"工件掉不下去"。

    两个方向都会出事，而且都是**静默**的（2026-09-24 各栽过一次）：

      a) 槽开得不够 **窄**（< 规的通端）：规下压到 12mm 就顶在台面上，
         S07 一路压到上限都等不到台肩落面 → 报"没触发接触判据"，
         看起来像传感器/阈值坏了。
      b) 槽开得**太宽**：工件跨在槽上时承压面太小，一有扰动就翻进槽里、
         落到地面 —— 于是 `perception.source=truth` 开始撒谎：孔位按名义位姿
         投到像素上，而孔位正下方已经不是工件（是槽底/地面），
         深度反投影把 58mm 孔距算成 169mm，S05 连丢 9 拍超时。

    所以这里同时查两件事：孔位必须落在**开口**内，承压宽度必须**够**。
    """
    import xml.etree.ElementTree as ET

    from arm_sim_gazebo.sim_scene import rings_local, world_boxes
    from arm_sim_gazebo.sim_site import resolve_world

    world_path, _ = resolve_world("", root)
    try:
        tree = ET.parse(world_path)
    except (OSError, ET.ParseError) as exc:
        return [f"读不出 world（{world_path}）：{exc}"], ""
    table = None
    for m in tree.getroot().iter("model"):
        if m.get("name") == "work_table":
            table = m
            break
    if table is None:
        return [f"{world_path} 里没有 <model name=\"work_table\"> —— 台面被改名了？"], ""

    def _pose_xyz(node) -> tuple[float, float, float]:
        p = node.find("pose")
        if p is None or not (p.text or "").strip():
            return (0.0, 0.0, 0.0)
        v = [float(x) for x in p.text.split()[:3]]
        while len(v) < 3:
            v.append(0.0)
        return (v[0], v[1], v[2])

    my = _pose_xyz(table)[1]
    spans: list[tuple[float, float]] = []
    for link in table.findall("link"):
        ly = my + _pose_xyz(link)[1]
        for col in link.findall("collision"):
            box = col.find("geometry/box/size")
            if box is None or not (box.text or "").strip():
                continue
            cy = ly + _pose_xyz(col)[1]
            half = float(box.text.split()[1]) / 2.0
            spans.append((cy - half, cy + half))
    if not spans:
        return ["work_table 里读不到任何 box 碰撞体 —— 台面几何解析不出来"], ""
    spans.sort()

    b = scene.table_bounds
    # 开口 = 台面范围内的"没被碰撞体盖住"的 y 区间。
    gaps: list[tuple[float, float]] = []
    cur = b.y_min
    for lo, hi in spans:
        if lo > cur + 1e-9:
            gaps.append((cur, min(lo, b.y_max)))
        cur = max(cur, hi)
    if cur < b.y_max - 1e-9:
        gaps.append((cur, b.y_max))
    gaps = [(lo, hi) for lo, hi in gaps if hi - lo > 1e-6]

    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}"], ""
    boxes = world_boxes(scene, part, root)
    if not boxes:
        return ["读不出工件的碰撞体"], ""
    wy0 = min(float(v) for bx in boxes for v in (bx[2], bx[3]))
    wy1 = max(float(v) for bx in boxes for v in (bx[2], bx[3]))

    # 承压宽度 = 工件在 y 方向上被台面碰撞体盖住的长度（区间求交后累加）。
    supported = 0.0
    for lo, hi in spans:
        supported += max(0.0, min(hi, wy1) - max(lo, wy0))

    rings = rings_local(scene, part, root)
    hole_ys: list[float] = []
    for r in rings:
        c, s = math.cos(float(part.yaw)), math.sin(float(part.yaw))
        hole_ys.append(float(part.y) + s * float(r.x) + c * float(r.y))

    probs: list[str] = []
    info = (
        f"台面沿 y 的支撑区间 {[(round(lo, 4), round(hi, 4)) for lo, hi in spans]}；"
        f"开口 {[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}；"
        f"工件 y∈[{wy0:.4f},{wy1:.4f}] 承压 {supported * 1000:.1f}mm"
    )

    # (a) 孔位必须落在开口里 —— 否则规的通端顶在台面上，S07 永远等不到台肩落面。
    for i, hy in enumerate(hole_ys):
        ok = any(lo < hy < hi for lo, hi in gaps)
        if not ok:
            probs.append(
                f"第 {i + 1} 个孔（世界 y={hy:+.4f}）**不在台面开口内** —— "
                f"规的通端会顶在台面上。\n"
                f"      后果：S07 下压 12mm 就到底，一路到上限都等不到台肩落面，"
                f"报「没触发接触判据」，看起来像传感器/阈值坏了。\n"
                f"      修法：把开口挪到孔位下方（worlds/assembly.world 的 work_table "
                f"碰撞体 y 区间），或改工件摆放。\n"
                f"      当前开口：{[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}"
            )

    # (b) 承压宽度必须够 —— 否则工件会翻进槽里掉下去，而真值感知会继续按
    #     名义位姿算，把 58mm 孔距算成 169mm（见 truth_holes.HoleTruth 的说明）。
    min_support = 0.020
    if supported < min_support:
        probs.append(
            f"工件跨在台面开口上的**承压宽度只有 {supported * 1000:.1f}mm**"
            f"（要求 ≥ {min_support * 1000:.0f}mm）。\n"
            f"      后果：板一有扰动就翻进槽里、落到地面；而 "
            f"perception.source=truth 仍按 scene.yaml 的**名义位姿**投孔 → "
            f"孔位下方已经不是工件而是槽底/地面 →\n"
            f"      深度反投影把 58mm 孔距算成 169mm → S05 每拍都丢、最后超时。"
            f"现象看起来是「检测坏了」，根因却是**工件掉了**。\n"
            f"      修法：把开口收窄（改 worlds/assembly.world 里 work_table 碰撞体的 "
            f"y 尺寸/位姿，开口只要够规的通端过就行，不需要覆盖两个孔所在的整个 y 带）。"
        )
    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] 台面通槽 vs 工件（规过得去 + 工件掉不下去）")
    p, info = check_table_slot(scene, root)
    problems += p
    if info:
        print(f"    {info}")
    if p:
        print("\n".join(f"    ✘ {x}" for x in p))
    else:
        print("    ✓ 孔位都在开口内，且承压宽度够")

    print("\n[6] 粗定位可见性（工件是否完整落在手外相机画面内）")
    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[7] 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())