ros_contorl/scripts/check_sim_scene.py

#!/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())