arm_dev
b29339e522
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
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>
473 lines
20 KiB
Python
473 lines
20 KiB
Python
#!/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_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())
|