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>
This commit is contained in:
arm_dev
@@ -239,25 +239,32 @@ def intrinsics(spec: CameraSpec, width: int, height: int) -> Tuple[List[float],
|
||||
return k, p
|
||||
|
||||
|
||||
def check_fov_consistency(
|
||||
def check_camera_consistency(
|
||||
specs: Sequence[CameraSpec],
|
||||
world_file: Optional[Path] = None,
|
||||
sensors_xacro: Optional[Path] = None,
|
||||
) -> List[str]:
|
||||
"""比对 cameras.yaml 里声明的视场与 SDF 里真正渲染用的视场。
|
||||
"""比对 cameras.yaml 的声明与 SDF 里真正渲染用的值（视场 + 图幅）。
|
||||
|
||||
为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 算出，
|
||||
而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 ——
|
||||
为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 与 width/height
|
||||
算出，而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 ——
|
||||
症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头
|
||||
那一长串说明所警告的那类错误，所以要主动比对。
|
||||
|
||||
⚠️ 图幅（width/height）为什么必须一起比：它决定 fx = W/(2·tan(hfov/2))，
|
||||
也就是"每毫米多少像素"。阶段二的孔检测在原始分辨率图上跑 HoughCircles，
|
||||
hole_detect.yaml 的像素门槛（hole_distance_min 等）只在图幅与真机相同时
|
||||
才有意义。图幅写错不会报任何错，只会让孔检测"莫名其妙不认孔" ——
|
||||
比视场写错更难查（视场错还会让点位整体偏，图幅错只是尺度不对）。
|
||||
|
||||
返回人类可读的告警列表（空 = 没问题）。
|
||||
|
||||
精度说明（有意为之，不做过度解析）：
|
||||
- 世界相机：按 <sensor> 里的 <topic> 精确匹配到对应相机，读到的是精确值。
|
||||
- 腕上相机：它在 xacro 宏里、值由参数传入，展开前无法可靠地归属到具体
|
||||
相机。所以退化成"文件里出现过这个值就算过"—— 偏宽松，但足以抓住
|
||||
"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。
|
||||
相机。所以视场退化成"文件里出现过这个值就算过"，图幅则读宏的
|
||||
**参数默认值**（image_width:=/image_height:=，launch 不会覆盖它）——
|
||||
偏宽松，但足以抓住"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。
|
||||
"""
|
||||
warnings: List[str] = []
|
||||
|
||||
@@ -268,6 +275,7 @@ def check_fov_consistency(
|
||||
continue
|
||||
# 找到 <topic> 等于本相机 gz_ns 的那个 <sensor> 块
|
||||
fov = None
|
||||
size = (None, None)
|
||||
for m in re.finditer(r"<sensor\b[^>]*>(.*?)</sensor>", text, re.S):
|
||||
body = m.group(1)
|
||||
tm = re.search(r"<topic>\s*([^<\s]+)\s*</topic>", body)
|
||||
@@ -276,6 +284,12 @@ def check_fov_consistency(
|
||||
fm = re.search(r"<horizontal_fov>\s*([0-9.eE+-]+)\s*</horizontal_fov>", body)
|
||||
if fm:
|
||||
fov = float(fm.group(1))
|
||||
wm = re.search(r"<width>\s*(\d+)\s*</width>", body)
|
||||
hm = re.search(r"<height>\s*(\d+)\s*</height>", body)
|
||||
size = (
|
||||
int(wm.group(1)) if wm else None,
|
||||
int(hm.group(1)) if hm else None,
|
||||
)
|
||||
break
|
||||
if fov is None:
|
||||
warnings.append(
|
||||
@@ -291,6 +305,7 @@ def check_fov_consistency(
|
||||
f" 内参按前者算、图像按后者渲染 → 三维点位会整体错，"
|
||||
f"而单目预览看不出来。请把两处改成一致。"
|
||||
)
|
||||
_check_size(s, size[0], size[1], Path(world_file).name, warnings)
|
||||
|
||||
if sensors_xacro is not None and Path(sensors_xacro).is_file():
|
||||
text = Path(sensors_xacro).read_text(encoding="utf-8", errors="replace")
|
||||
@@ -310,10 +325,61 @@ def check_fov_consistency(
|
||||
f"{s.horizontal_fov}，而 {Path(sensors_xacro).name} 里出现的值是 "
|
||||
f"{found}。若渲染用的不是 {s.horizontal_fov}，三维点位会整体错。"
|
||||
)
|
||||
# 腕上相机的图幅：读宏的参数默认值（launch 不覆盖它们）
|
||||
mw = re.search(r"image_width:=(\d+)", text)
|
||||
mh = re.search(r"image_height:=(\d+)", text)
|
||||
for s in specs:
|
||||
if s.is_world_mounted:
|
||||
continue
|
||||
_check_size(
|
||||
s,
|
||||
int(mw.group(1)) if mw else None,
|
||||
int(mh.group(1)) if mh else None,
|
||||
Path(sensors_xacro).name,
|
||||
warnings,
|
||||
)
|
||||
|
||||
return warnings
|
||||
|
||||
|
||||
def _check_size(
|
||||
spec: "CameraSpec",
|
||||
width: Optional[int],
|
||||
height: Optional[int],
|
||||
where: str,
|
||||
warnings: List[str],
|
||||
) -> None:
|
||||
"""比对声明图幅与实际渲染图幅，不一致就追加一条告警。
|
||||
|
||||
单独抽出来是因为世界相机与腕上相机的"值从哪读"完全不同（前者读 <sensor>，
|
||||
后者读 xacro 宏默认值），但**比对逻辑与文案必须一致** —— 两处各写一遍
|
||||
迟早会漂移成两套说法。
|
||||
|
||||
``spec`` 未声明图幅（width/height <= 0）时直接跳过：那表示"这份清单不管尺寸"，
|
||||
此时 intrinsics() 会退回 640x480。没有声明就没有"不一致"可言，
|
||||
报出来只会变成噪声，反而让人忽略真正的告警。
|
||||
"""
|
||||
if spec.width <= 0 or spec.height <= 0:
|
||||
return
|
||||
if width is None or height is None:
|
||||
warnings.append(
|
||||
f"相机 {spec.ros_ns}: 在 {where} 里读不到图像尺寸（<width>/<height> 或 "
|
||||
f"image_width:=），无法比对。cameras.yaml 声明 "
|
||||
f"{spec.width}x{spec.height} —— 若渲染的实际尺寸与它不同，"
|
||||
f"内参算出的每毫米像素数就是错的，孔检测的像素门槛会静默失效。"
|
||||
)
|
||||
return
|
||||
if (width, height) != (spec.width, spec.height):
|
||||
warnings.append(
|
||||
f"相机 {spec.ros_ns}: 图像尺寸不一致！cameras.yaml 写 "
|
||||
f"{spec.width}x{spec.height}，而 {where} 里渲染用的是 {width}x{height}。\n"
|
||||
f" 内参按前者算、图像按后者渲染 → fx 的尺度不对（差 "
|
||||
f"{width / max(spec.width, 1):.3f} 倍），"
|
||||
f"所有以像素为单位的门槛（如 hole_distance_min）都会静默失效。\n"
|
||||
f" 注意：真机 D405 是 848x480、D435 是 1280x720；仿真侧要对齐 D405。"
|
||||
)
|
||||
|
||||
|
||||
def bridge_args(specs: Sequence[CameraSpec]) -> List[str]:
|
||||
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
|
||||
|
||||
|
||||
@@ -0,0 +1,851 @@
|
||||
"""读 `devices/sim/gazebo/config/scene.yaml`：场景里摆了什么、摆在哪。
|
||||
|
||||
=============================================================================
|
||||
这个模块解决的两个问题
|
||||
=============================================================================
|
||||
1) "模型在哪"这件事必须有个**单一数据源**。
|
||||
之前摆在台面上的东西（通止规标记）是直接写死在 `assembly.world` 里的，
|
||||
改一次位置要动 world 文件 —— 而 world 里还有物理参数、插件、眼在手外的相机，
|
||||
动它就有一堆无关风险。所以场景物体统一挪到 scene.yaml，
|
||||
由启动器在 spawn 机械臂之后再 spawn 它们。
|
||||
|
||||
2) 位姿写对了但**物理上讲不通**的情况，必须能自动查出来。
|
||||
最典型的三类，全都是"看起来正常、跑起来不对"：
|
||||
- 模型悬空（z 比台面高）→ 一松手就掉，表现为"工件自己会动"
|
||||
- 模型穿进台面（z 比台面低）→ 求解器把它顶出来，表现为"工件自己弹起来"
|
||||
- 两个模型重叠 → 互相弹开，表现为"工件被推到别处去了"
|
||||
所以本模块除了读配置，还提供 validate_scene()：
|
||||
用**从 SDF 里真解析出来的碰撞体包围盒**去核对位置，而不是信注释。
|
||||
|
||||
⚠️ 为什么一定要解析 SDF 而不信 scene.yaml 里写的尺寸：
|
||||
尺寸属于模型（model.sdf），位置属于场景（scene.yaml）。
|
||||
如果在 scene.yaml 里再抄一份尺寸，两者迟早不一致 ——
|
||||
而且是那种"改小了模型但场景校验还按老尺寸过"，最难发现。
|
||||
=============================================================================
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import yaml
|
||||
|
||||
SCENE_REL = "devices/sim/gazebo/config/scene.yaml"
|
||||
|
||||
# 浮点比较容差。1e-6 m = 1μm —— 远小于任何有物理意义的装配误差，
|
||||
# 所以"差 1μm 以内"一律算相等，避免把 sqrt/字符串解析的尾差报成错误。
|
||||
EPS = 1e-6
|
||||
|
||||
|
||||
class SceneError(RuntimeError):
|
||||
"""scene.yaml 读不了、或模型文件缺失/不合法 —— 属于"起不来"的错误。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SceneObject:
|
||||
"""场景里的一个物体。pose 是**世界系绝对坐标**，z 是物体原点（我们约定在底面中心）。"""
|
||||
|
||||
name: str
|
||||
model: str
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
yaw: float = 0.0
|
||||
note: str = ""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Bounds:
|
||||
"""轴对齐包围盒，用来核对摆放。"""
|
||||
|
||||
x_min: float
|
||||
x_max: float
|
||||
y_min: float
|
||||
y_max: float
|
||||
|
||||
def contains(self, other: "Bounds", tol: float = EPS) -> bool:
|
||||
return (
|
||||
other.x_min >= self.x_min - tol
|
||||
and other.x_max <= self.x_max + tol
|
||||
and other.y_min >= self.y_min - tol
|
||||
and other.y_max <= self.y_max + tol
|
||||
)
|
||||
|
||||
def overlaps(self, other: "Bounds") -> bool:
|
||||
return not (
|
||||
other.x_min >= self.x_max - EPS
|
||||
or other.x_max <= self.x_min + EPS
|
||||
or other.y_min >= self.y_max - EPS
|
||||
or other.y_max <= self.y_min + EPS
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Operating:
|
||||
"""作业条件：校验"成像几何是否与真机可比"。"""
|
||||
|
||||
camera: str
|
||||
camera_to_part_m: float
|
||||
camera_offset_below_flange_m: float
|
||||
# 哪个物体是"工件"（相机到工件表面的距离以它的上表面为基准）
|
||||
part_object: str = "workpiece"
|
||||
|
||||
|
||||
@dataclass
|
||||
class SceneSpec:
|
||||
"""scene.yaml 的解析结果。"""
|
||||
|
||||
models_dir: str
|
||||
table_top_z: float
|
||||
table_bounds: Bounds
|
||||
operating: Operating
|
||||
objects: List[SceneObject] = field(default_factory=list)
|
||||
# 特征名 → (下限, 上限) 像素门槛。没写 = 不设门槛（只报告不判定）。
|
||||
gates_px: Dict[str, Tuple[float, float]] = field(default_factory=dict)
|
||||
source: Optional[Path] = None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 读配置
|
||||
# ---------------------------------------------------------------------------
|
||||
def repo_root() -> Path:
|
||||
"""仓库根。本模块在 src/arm_sim_gazebo/arm_sim_gazebo/ 下，向上走四层。"""
|
||||
return Path(__file__).resolve().parents[3]
|
||||
|
||||
|
||||
def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> SceneSpec:
|
||||
"""读场景配置。
|
||||
|
||||
**文件缺失时抛错**，不像 sim_site 那样给默认值 —— 区别在于：
|
||||
site.yaml 是站点数据（含真实序列号，本来就不入库），缺了是正常的；
|
||||
而 scene.yaml 是**框架自带**的场景定义，缺了说明仓库不完整，
|
||||
这时候静默用默认值会让人对着一个空台面调半天。
|
||||
"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
p = Path(path) if path is not None else base / SCENE_REL
|
||||
|
||||
if not p.is_file():
|
||||
raise SceneError(
|
||||
f"找不到场景配置 {p}。\n"
|
||||
f" 它是框架自带的（不是站点数据），正常应该随仓库一起存在。\n"
|
||||
f" 若是刚克隆下来的仓库，确认 {SCENE_REL} 在；"
|
||||
f"或先用 --scene 指定别的场景文件。"
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise SceneError(f"{p} 解析失败: {exc}") from exc
|
||||
|
||||
if not isinstance(doc, dict):
|
||||
raise SceneError(f"{p} 的顶层不是映射（键值对）。")
|
||||
|
||||
missing = [k for k in ("models_dir", "table_top_z", "table_bounds", "objects") if k not in doc]
|
||||
if missing:
|
||||
raise SceneError(
|
||||
f"{p} 缺少必填键 {missing}。\n"
|
||||
f" 最小可用的场景长这样：\n"
|
||||
f" models_dir: devices/sim/gazebo/models\n"
|
||||
f" table_top_z: 0.40\n"
|
||||
f" table_bounds: {{x: [0.15, 0.95], y: [-0.60, 0.60]}}\n"
|
||||
f" objects: []"
|
||||
)
|
||||
|
||||
tb = doc.get("table_bounds") or {}
|
||||
try:
|
||||
table = Bounds(
|
||||
x_min=float(tb["x"][0]),
|
||||
x_max=float(tb["x"][1]),
|
||||
y_min=float(tb["y"][0]),
|
||||
y_max=float(tb["y"][1]),
|
||||
)
|
||||
except (KeyError, IndexError, TypeError, ValueError) as exc:
|
||||
raise SceneError(
|
||||
f"{p} 的 table_bounds 写错了（{exc}）。期望形如 "
|
||||
f"table_bounds: {{x: [0.15, 0.95], y: [-0.60, 0.60]}}"
|
||||
) from exc
|
||||
|
||||
op_raw = doc.get("operating") or {}
|
||||
operating = Operating(
|
||||
camera=str(op_raw.get("camera") or "d405").strip().strip("/"),
|
||||
camera_to_part_m=float(op_raw.get("camera_to_part_m") or 0.20),
|
||||
camera_offset_below_flange_m=float(op_raw.get("camera_offset_below_flange_m") or 0.0),
|
||||
part_object=str(op_raw.get("part_object") or "workpiece"),
|
||||
)
|
||||
|
||||
objects: List[SceneObject] = []
|
||||
raw_objs = doc.get("objects") or []
|
||||
if not isinstance(raw_objs, list):
|
||||
raise SceneError(f"{p} 的 objects 不是列表。每一项形如 "
|
||||
f"{{name: workpiece, model: workpiece, pose: {{x: 0.52, y: 0, z: 0.40, yaw: 0}}}}")
|
||||
for i, raw in enumerate(raw_objs):
|
||||
if not isinstance(raw, dict):
|
||||
raise SceneError(f"{p} 的 objects[{i}] 不是映射。")
|
||||
pose = raw.get("pose") or {}
|
||||
name = str(raw.get("name") or "").strip()
|
||||
model = str(raw.get("model") or "").strip()
|
||||
if not name or not model:
|
||||
raise SceneError(
|
||||
f"{p} 的 objects[{i}] 缺 name 或 model。"
|
||||
f"两者都要有：name 是进 Gazebo 后的实体名，model 是模型目录名。"
|
||||
)
|
||||
try:
|
||||
objects.append(
|
||||
SceneObject(
|
||||
name=name,
|
||||
model=model,
|
||||
x=float(pose.get("x", 0.0)),
|
||||
y=float(pose.get("y", 0.0)),
|
||||
z=float(pose.get("z", 0.0)),
|
||||
yaw=float(pose.get("yaw", 0.0)),
|
||||
note=str(raw.get("note") or "").strip(),
|
||||
)
|
||||
)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise SceneError(f"{p} 的 objects[{i}]（{name}）的 pose 不是数字: {exc}") from exc
|
||||
|
||||
return SceneSpec(
|
||||
models_dir=str(doc["models_dir"]).strip(),
|
||||
table_top_z=float(doc["table_top_z"]),
|
||||
table_bounds=table,
|
||||
operating=operating,
|
||||
objects=objects,
|
||||
gates_px=_parse_gates(p, doc.get("gates_px")),
|
||||
source=p,
|
||||
)
|
||||
|
||||
|
||||
def _parse_gates(path: Path, raw: object) -> Dict[str, Tuple[float, float]]:
|
||||
"""解析 gates_px。写成 {名字: [下限, 上限]}。
|
||||
|
||||
单独抽出来是因为这里的错误信息要能指出"哪一项写错了、该怎么写" ——
|
||||
门槛写错会静默地把校验变成永远通过，那比没校验更糟。
|
||||
"""
|
||||
if raw is None:
|
||||
return {}
|
||||
if not isinstance(raw, dict):
|
||||
raise SceneError(
|
||||
f"{path} 的 gates_px 不是映射。期望形如：\n"
|
||||
f" gates_px:\n insert_ring_radius: [10, 30]"
|
||||
)
|
||||
out: Dict[str, Tuple[float, float]] = {}
|
||||
for key, val in raw.items():
|
||||
try:
|
||||
lo, hi = float(val[0]), float(val[1])
|
||||
except (TypeError, IndexError, ValueError) as exc:
|
||||
raise SceneError(
|
||||
f"{path} 的 gates_px.{key} 写错了（{exc}）。"
|
||||
f"期望 [下限, 上限]，例如 [10, 30]。"
|
||||
) from exc
|
||||
if lo > hi:
|
||||
raise SceneError(
|
||||
f"{path} 的 gates_px.{key} 是 [{lo}, {hi}]，下限比上限大 —— "
|
||||
f"这样任何值都会被判为不合格。检查是不是写反了。"
|
||||
)
|
||||
out[str(key)] = (lo, hi)
|
||||
return out
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 从模型文件里读出真实几何
|
||||
# ---------------------------------------------------------------------------
|
||||
def model_sdf_path(spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None) -> Path:
|
||||
"""模型 SDF 的绝对路径：<仓库根>/<models_dir>/<model>/model.sdf。"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
md = Path(spec.models_dir)
|
||||
if not md.is_absolute():
|
||||
md = base / md
|
||||
return md / obj.model / "model.sdf"
|
||||
|
||||
|
||||
def _local_extent(sdf_path: Path) -> Optional[Tuple[float, float, float, float, float, float]]:
|
||||
"""把一个模型所有 <collision> 的几何合成一个局部 AABB。
|
||||
|
||||
返回 (x_min, x_max, y_min, y_max, z_min, z_max)；文件读不了/没有碰撞体返回 None。
|
||||
|
||||
只看 collision 不看 visual：碰撞体才是"物理上占多大地方"。
|
||||
（本框架的模型里两者刻意做成一致，但检验摆放时以碰撞体为准 ——
|
||||
视觉偏出去一点不影响物理，碰撞体偏出去就会把别的东西顶飞。）
|
||||
"""
|
||||
boxes = collision_boxes(sdf_path)
|
||||
if not boxes:
|
||||
return None
|
||||
return (
|
||||
min(b[0] for b in boxes), max(b[1] for b in boxes),
|
||||
min(b[2] for b in boxes), max(b[3] for b in boxes),
|
||||
min(b[4] for b in boxes), max(b[5] for b in boxes),
|
||||
)
|
||||
|
||||
|
||||
def collision_boxes(sdf_path: Path) -> List[Tuple[float, float, float, float, float, float]]:
|
||||
"""模型里每个 <collision> 的局部 AABB，逐条返回（不合并）。
|
||||
|
||||
⚠️ 为什么必须**逐条**保留、不能只留一个总包围盒：
|
||||
有些物体是"套在一起"的 —— 通止规立在规座的方孔里就是典型。
|
||||
用总包围盒判"会不会撞"会把这种**正常嵌套**判成重叠（规座的包围盒
|
||||
是 60×60，规的包围盒是 30×30，两者必然相交），于是要么误报，
|
||||
要么就得加一个"允许重叠"的白名单把检查废掉。
|
||||
逐条比才可以正确得出"规身与规座的四个方块都不相交"。
|
||||
"""
|
||||
try:
|
||||
tree = ET.parse(sdf_path)
|
||||
except (OSError, ET.ParseError):
|
||||
return []
|
||||
|
||||
out: List[Tuple[float, float, float, float, float, float]] = []
|
||||
for coll in tree.iter("collision"):
|
||||
# 碰撞体自身的 <pose>：x y z roll pitch yaw（旋转一概忽略）
|
||||
px = py = pz = 0.0
|
||||
pose_el = coll.find("pose")
|
||||
if pose_el is not None and pose_el.text:
|
||||
parts = pose_el.text.split()
|
||||
if len(parts) >= 3:
|
||||
try:
|
||||
px, py, pz = (float(v) for v in parts[:3])
|
||||
except ValueError:
|
||||
px = py = pz = 0.0
|
||||
|
||||
geom = coll.find("geometry")
|
||||
if geom is None:
|
||||
continue
|
||||
half = None # (hx, hy, hz)
|
||||
box = geom.find("box")
|
||||
cyl = geom.find("cylinder")
|
||||
sph = geom.find("sphere")
|
||||
if box is not None:
|
||||
size_el = box.find("size")
|
||||
if size_el is not None and size_el.text:
|
||||
try:
|
||||
sx, sy, sz = (float(v) for v in size_el.text.split()[:3])
|
||||
half = (sx / 2.0, sy / 2.0, sz / 2.0)
|
||||
except ValueError:
|
||||
half = None
|
||||
elif cyl is not None:
|
||||
r_el, l_el = cyl.find("radius"), cyl.find("length")
|
||||
try:
|
||||
r = float(r_el.text) if r_el is not None and r_el.text else 0.0
|
||||
ln = float(l_el.text) if l_el is not None and l_el.text else 0.0
|
||||
half = (r, r, ln / 2.0)
|
||||
except ValueError:
|
||||
half = None
|
||||
elif sph is not None:
|
||||
r_el = sph.find("radius")
|
||||
try:
|
||||
r = float(r_el.text) if r_el is not None and r_el.text else 0.0
|
||||
half = (r, r, r)
|
||||
except ValueError:
|
||||
half = None
|
||||
if half is None:
|
||||
continue
|
||||
out.append(
|
||||
(
|
||||
px - half[0], px + half[0],
|
||||
py - half[1], py + half[1],
|
||||
pz - half[2], pz + half[2],
|
||||
)
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def world_boxes(
|
||||
spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
|
||||
) -> List[Tuple[float, float, float, float, float, float]]:
|
||||
"""物体的各个碰撞体在**世界系**的 AABB。
|
||||
|
||||
yaw 的处理：绕 Z 转 θ 的矩形，外接盒的半宽是 hx·|cosθ| + hy·|sinθ|，
|
||||
半高同理；Z 不受影响。90° 整数倍会得到精确值，斜摆也仍然正确。
|
||||
|
||||
⚠️ 必须用"**中心 + 半宽**"变换，不能对 min/max 取绝对值再对称化 ——
|
||||
局部 AABB 里已经含了碰撞体 pose 的偏移量（例如规座的 four 块在
|
||||
x=∓16.75mm 处），把它对称化会把偏移吃掉、盒子凭空变大
|
||||
（实测：规座的一侧块从 x∈[-30,-3.5]mm 变成 x∈[-30,+30]mm，
|
||||
于是判出"规座四块全都和规的轴相交"的假重叠）。
|
||||
"""
|
||||
import math
|
||||
|
||||
boxes = collision_boxes(model_sdf_path(spec, obj, root))
|
||||
c, s = math.cos(obj.yaw), math.sin(obj.yaw)
|
||||
ac, as_ = abs(c), abs(s)
|
||||
out: List[Tuple[float, float, float, float, float, float]] = []
|
||||
for b in boxes:
|
||||
cx, cy = (b[0] + b[1]) / 2.0, (b[2] + b[3]) / 2.0
|
||||
hx, hy = (b[1] - b[0]) / 2.0, (b[3] - b[2]) / 2.0
|
||||
# 中心绕模型原点转 yaw
|
||||
wx = obj.x + cx * c - cy * s
|
||||
wy = obj.y + cx * s + cy * c
|
||||
# 旋转后的外接盒半宽
|
||||
rx = hx * ac + hy * as_
|
||||
ry = hx * as_ + hy * ac
|
||||
out.append((wx - rx, wx + rx, wy - ry, wy + ry, obj.z + b[4], obj.z + b[5]))
|
||||
return out
|
||||
|
||||
|
||||
def _boxes_overlap(
|
||||
a: Tuple[float, float, float, float, float, float],
|
||||
b: Tuple[float, float, float, float, float, float],
|
||||
tol: float = 1e-4,
|
||||
) -> bool:
|
||||
"""两个 AABB 是否真的相交（三轴都要相交才算）。
|
||||
|
||||
容差 0.1mm：**恰好贴合不算相交**。这条很关键 ——
|
||||
"规的台肩正好坐在规座顶面上"是正常装配状态，不是碰撞，
|
||||
若把贴合当相交，这种模型永远过不了检查，检查就会被人关掉。
|
||||
"""
|
||||
return not (
|
||||
b[0] >= a[1] - tol
|
||||
or b[1] <= a[0] + tol
|
||||
or b[2] >= a[3] - tol
|
||||
or b[3] <= a[2] + tol
|
||||
or b[4] >= a[5] - tol
|
||||
or b[5] <= a[4] + tol
|
||||
)
|
||||
|
||||
|
||||
def overlapping_pairs(
|
||||
spec: SceneSpec, root: Optional[Path] = None
|
||||
) -> List[Tuple[str, str, str]]:
|
||||
"""逐**碰撞体**核对哪些物体的哪些块真的互相嵌入。
|
||||
|
||||
返回 (物体A, 物体B, 说明)。空 = 没有互相嵌入。
|
||||
"""
|
||||
per_obj = {o.name: world_boxes(spec, o, root) for o in spec.objects}
|
||||
out: List[Tuple[str, str, str]] = []
|
||||
names = [o.name for o in spec.objects]
|
||||
for i in range(len(names)):
|
||||
for j in range(i + 1, len(names)):
|
||||
a, b = per_obj[names[i]], per_obj[names[j]]
|
||||
for ka, ba in enumerate(a):
|
||||
for kb, bb in enumerate(b):
|
||||
if _boxes_overlap(ba, bb):
|
||||
out.append(
|
||||
(
|
||||
names[i],
|
||||
names[j],
|
||||
f"碰撞体 #{ka} 与 #{kb} 互相嵌入",
|
||||
)
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def footprint(
|
||||
spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
|
||||
) -> Optional[Tuple[float, float, float, float]]:
|
||||
"""物体摆在场景里之后，在世界系 XY 平面上占的矩形 (x_min, x_max, y_min, y_max)。
|
||||
|
||||
实现为"各碰撞体世界 AABB 在 XY 上的并"。**不再**自己写一遍 yaw 换算 ——
|
||||
那样两处换算逻辑迟早不一致，而"哪个才是对的"很难判断。
|
||||
"""
|
||||
boxes = world_boxes(spec, obj, root)
|
||||
if not boxes:
|
||||
return None
|
||||
return (
|
||||
min(b[0] for b in boxes), max(b[1] for b in boxes),
|
||||
min(b[2] for b in boxes), max(b[3] for b in boxes),
|
||||
)
|
||||
|
||||
|
||||
def model_height(spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None) -> Optional[float]:
|
||||
"""模型自身的高度（从原点到最高点）。用于推算工件上表面高度。"""
|
||||
ext = _local_extent(model_sdf_path(spec, obj, root))
|
||||
return None if ext is None else ext[5]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 校验
|
||||
# ---------------------------------------------------------------------------
|
||||
def validate_scene(spec: SceneSpec, root: Optional[Path] = None) -> List[str]:
|
||||
"""核对场景能不能真的跑起来。返回人类可读的问题列表（空 = 没问题）。
|
||||
|
||||
分两类，但都返回同一种字符串（调用方按需要决定是拦还是只告警）：
|
||||
"起不来"的错误：模型文件缺失、名字重复
|
||||
"跑起来不对"的错误：悬空 / 穿模 / 出界 / 互相重叠 / 原点不在底面
|
||||
"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
problems: List[str] = []
|
||||
|
||||
md = Path(spec.models_dir)
|
||||
if not md.is_absolute():
|
||||
md = base / md
|
||||
if not md.is_dir():
|
||||
return [
|
||||
f"模型目录不存在：{md}\n"
|
||||
f" scene.yaml 的 models_dir 写的是 {spec.models_dir!r}，相对路径按**仓库根**解析。"
|
||||
]
|
||||
|
||||
seen: Dict[str, SceneObject] = {}
|
||||
for obj in spec.objects:
|
||||
if obj.name in seen:
|
||||
problems.append(
|
||||
f"物体名重复：{obj.name!r}。Gazebo 里实体名必须唯一，"
|
||||
f"否则后 spawn 的会失败（或覆盖先前的），而日志未必明显。"
|
||||
)
|
||||
seen[obj.name] = obj
|
||||
|
||||
sdf = model_sdf_path(spec, obj, root)
|
||||
cfg = sdf.parent / "model.config"
|
||||
if not sdf.is_file():
|
||||
problems.append(
|
||||
f"物体 {obj.name}：模型文件不存在 {sdf}\n"
|
||||
f" 期望布局：{md}/<模型名>/model.sdf。"
|
||||
f"scene.yaml 里写的 model={obj.model!r} 就是那个 <模型名>。"
|
||||
)
|
||||
continue
|
||||
if not cfg.is_file():
|
||||
problems.append(
|
||||
f"物体 {obj.name}：缺少 {cfg.name}（{cfg}）。\n"
|
||||
f" 没有它，model:// 引用会静默找不到（Gazebo 不报错）。"
|
||||
f"内容照 models/workpiece/model.config 抄一份，"
|
||||
f"<name> 必须等于目录名 {obj.model!r}。"
|
||||
)
|
||||
|
||||
ext = _local_extent(sdf)
|
||||
if ext is None:
|
||||
problems.append(
|
||||
f"物体 {obj.name}：{sdf.name} 里解析不出任何 <collision> 几何。\n"
|
||||
f" 没有碰撞体 = 物理上不存在：会穿过台面掉下去，而视觉上看着还在。"
|
||||
)
|
||||
continue
|
||||
|
||||
# 1) 原点必须在底面 —— 这是本框架"z = 台面高度"约定的前提
|
||||
if abs(ext[4]) > 1e-4:
|
||||
problems.append(
|
||||
f"物体 {obj.name}：模型原点不在底面（局部 z_min = {ext[4]:.4f} m）。\n"
|
||||
f" 场景里 z 是按'物体原点'给的，原点不在底面就会整体悬空或陷入台面。\n"
|
||||
f" 修法：改模型（把原点挪到底面中心）或改 scene.yaml 的 z，"
|
||||
f"但推荐改模型 —— 约定统一后场景文件才读得懂。"
|
||||
)
|
||||
|
||||
# 2) 落在台面上，且不穿模
|
||||
if obj.z < spec.table_top_z - 1e-4:
|
||||
problems.append(
|
||||
f"物体 {obj.name}：z={obj.z} 低于台面顶面 {spec.table_top_z}，穿进台子了。\n"
|
||||
f" 求解器会把它顶出来 → 表现为'工件自己弹起来'。"
|
||||
)
|
||||
|
||||
# 3) 不能出界
|
||||
fp = footprint(spec, obj, root)
|
||||
if fp is not None:
|
||||
fb = Bounds(x_min=fp[0], x_max=fp[1], y_min=fp[2], y_max=fp[3])
|
||||
if not spec.table_bounds.contains(fb):
|
||||
problems.append(
|
||||
f"物体 {obj.name}：占位 x∈[{fb.x_min:.3f},{fb.x_max:.3f}] "
|
||||
f"y∈[{fb.y_min:.3f},{fb.y_max:.3f}] 超出台面 "
|
||||
f"x∈[{spec.table_bounds.x_min},{spec.table_bounds.x_max}] "
|
||||
f"y∈[{spec.table_bounds.y_min},{spec.table_bounds.y_max}]。\n"
|
||||
f" 悬在台外的部分会掉下去 / 让它翻倒。"
|
||||
)
|
||||
|
||||
# 4) 物体之间不能互相嵌入。
|
||||
# 逐碰撞体、三维判，而不是"总包围盒在 XY 上相交"——
|
||||
# 因为正常装配里本来就存在嵌套（规立在规座的方孔里），
|
||||
# 粗判会把正常嵌套报成碰撞，逼着人把这个检查关掉。见 collision_boxes 的说明。
|
||||
for a, b, why in overlapping_pairs(spec, root):
|
||||
problems.append(
|
||||
f"物体 {a} 与 {b} 互相嵌入（{why}）。\n"
|
||||
f" 两个刚体一开始就互相嵌入 → 求解器把它们弹开，"
|
||||
f"表现为'工件/规被推到别处、自己飞走'，而你会怀疑是控制或标定问题。\n"
|
||||
f" 注意：恰好贴合不算嵌入（例如规的台肩正好坐在规座顶面上），"
|
||||
f"会被放行；这里报的是真穿透。"
|
||||
)
|
||||
|
||||
return problems
|
||||
|
||||
|
||||
def spawn_commands(
|
||||
spec: SceneSpec, root: Optional[Path] = None
|
||||
) -> List[List[str]]:
|
||||
"""每个场景物体一条 `ros_gz_sim create` 的参数列表（不含可执行文件名）。
|
||||
|
||||
为什么用 create 服务而不是把模型写进 world：
|
||||
world 是"固定不动的场景"（地面、台子、支架相机），
|
||||
而这些是按 site 需要摆/换位的**物料**。用 create 的话，
|
||||
换一个工件不用重启 Gazebo，也不用动 world 文件（动它的风险面太大，
|
||||
见 assembly.world 里关于插件的长注释）。
|
||||
"""
|
||||
out: List[List[str]] = []
|
||||
for obj in spec.objects:
|
||||
sdf = model_sdf_path(spec, obj, root)
|
||||
out.append(
|
||||
[
|
||||
"-file", str(sdf),
|
||||
"-name", obj.name,
|
||||
"-x", f"{obj.x:.6f}",
|
||||
"-y", f"{obj.y:.6f}",
|
||||
"-z", f"{obj.z:.6f}",
|
||||
"-Y", f"{obj.yaw:.6f}",
|
||||
]
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 成像几何：把"该看见什么"变成可校验的数字
|
||||
# ---------------------------------------------------------------------------
|
||||
def px_per_m(fx_px: float, distance_m: float) -> float:
|
||||
"""每米对应多少像素（小孔成像，垂直光轴近似）。
|
||||
|
||||
已知焦距与距离时的尺度换算，孔检测的像素门槛全靠它换算成物理尺寸。
|
||||
"""
|
||||
if distance_m <= 0:
|
||||
raise ValueError(f"距离必须为正，收到 {distance_m}")
|
||||
return fx_px / distance_m
|
||||
|
||||
|
||||
def feature_pixels(
|
||||
fx_px: float, distance_m: float, features: Dict[str, float]
|
||||
) -> Dict[str, float]:
|
||||
"""把一组特征尺寸（米）换算成像素。返回同名的像素字典，便于直接打印。"""
|
||||
s = px_per_m(fx_px, distance_m)
|
||||
return {k: v * s for k, v in features.items()}
|
||||
|
||||
|
||||
def check_pixel_gates(
|
||||
px: Dict[str, float], gates: Dict[str, Tuple[float, float]]
|
||||
) -> List[str]:
|
||||
"""按像素门槛核对特征是否落在可检测区间内。返回问题列表。
|
||||
|
||||
为什么这件事要在仿真侧、而不是等真机：这些门槛（hough 半径、孔间距）
|
||||
都是**像素**量，而仿真里唯一能提前算出它们的就是几何。
|
||||
等上了真机才发现"孔间距只有 85px，低于门槛 90"要贵得多。
|
||||
"""
|
||||
out: List[str] = []
|
||||
for key, (lo, hi) in gates.items():
|
||||
if key not in px:
|
||||
continue
|
||||
v = px[key]
|
||||
if v < lo or v > hi:
|
||||
out.append(
|
||||
f"{key} = {v:.1f}px 落在可检测区间 [{lo}, {hi}] 之外。\n"
|
||||
f" 成因只有三种：相机到工件的距离不对 / 相机图幅不对 / 特征尺寸不对。\n"
|
||||
f" 分别对应：scene.yaml 的 camera_to_part_m、"
|
||||
f"cameras.yaml 的 width、模型里的特征尺寸。"
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def operating_flange_z(spec: SceneSpec, root: Optional[Path] = None) -> Optional[float]:
|
||||
"""按"相机到工件的距离"反推仿真里法兰应该悬停多高。
|
||||
|
||||
推导：工件上表面 = 台面 + 工件模型自身高度
|
||||
相机光心 = 法兰 - 伸出量（竖直朝下装配时，伸出量朝工件方向）
|
||||
法兰 = 工件上表面 + 相机到工件距离 + 伸出量
|
||||
|
||||
这个值**不等于**真机 site.yaml 的 hole_hover_z_m（0.23）——
|
||||
原因见 scene.yaml 里 operating 段的说明与 README 的"仿真 ≠ 真机"。
|
||||
"""
|
||||
part = next((o for o in spec.objects if o.name == spec.operating.part_object), None)
|
||||
if part is None:
|
||||
return None
|
||||
h = model_height(spec, part, root)
|
||||
if h is None:
|
||||
return None
|
||||
part_top = spec.table_top_z + h
|
||||
return part_top + spec.operating.camera_to_part_m + spec.operating.camera_offset_below_flange_m
|
||||
|
||||
|
||||
def object_pose_map(spec: SceneSpec) -> Dict[str, Tuple[float, float, float, float]]:
|
||||
"""name → (x, y, z, yaw)。启动日志与验收脚本都用它打印"应该摆在哪"。"""
|
||||
return {o.name: (o.x, o.y, o.z, o.yaw) for o in spec.objects}
|
||||
|
||||
|
||||
def parse_spawned_poses(text: str) -> Dict[str, Tuple[float, float, float]]:
|
||||
"""解析 `ign model --list` / `ign model -m <名>` 的输出，解出实体位姿。
|
||||
|
||||
⚠️ 这段解析**曾经是坏的，而且是静默坏的**：它按 "x: N / y: N / z: N"
|
||||
键值对来读，而 Fortress 的实际输出是
|
||||
|
||||
Model: [105]
|
||||
- Name: workpiece
|
||||
- Pose [ XYZ (m) ] [ RPY (rad) ]:
|
||||
[0.520000 -0.000000 0.400000]
|
||||
[0.000000 -0.000000 0.000000]
|
||||
|
||||
两者对不上 → 永远解析出空字典 → 调用方按设计"降级成跳过位姿核对" →
|
||||
位姿检查**从来没真正跑过**，而日志只说"跳过"，看起来像正常降级。
|
||||
教训：容器格式变了的时候，"宽容降级"会把"没检查"伪装成"检查通过"。
|
||||
两种格式现在都支持，且都有单测钉住（含真实的 Fortress 输出样本）。
|
||||
|
||||
仍然保留"解析不出来就返回空字典"的降级行为（Gazebo 的文本输出没有稳定
|
||||
契约），但调用方应当把"解析不出来"当作**告警**而不是静默通过 ——
|
||||
check_sim_scene.py 会打印"跳过"并说明原因。
|
||||
"""
|
||||
import re
|
||||
|
||||
out: Dict[str, Tuple[float, float, float]] = {}
|
||||
current: Optional[str] = None
|
||||
vals: Dict[str, float] = {}
|
||||
# 正在等 "Pose [...]:" 下面那一行方括号数值
|
||||
want_xyz = False
|
||||
|
||||
def _flush() -> None:
|
||||
if current and {"x", "y", "z"} <= set(vals):
|
||||
out[current] = (vals["x"], vals["y"], vals["z"])
|
||||
|
||||
for line in text.splitlines():
|
||||
m_name = re.search(r"^\s*(?:-\s*)?Name:\s*(\S+)", line)
|
||||
if m_name:
|
||||
_flush()
|
||||
current = m_name.group(1)
|
||||
vals = {}
|
||||
want_xyz = False
|
||||
continue
|
||||
|
||||
# `ign model --list` 的实体名单独一行，形如 " - workpiece"
|
||||
m_ent = re.match(r"^\s*-\s*(\S+)\s*$", line)
|
||||
if m_ent:
|
||||
_flush()
|
||||
current = m_ent.group(1)
|
||||
vals = {}
|
||||
want_xyz = False
|
||||
continue
|
||||
|
||||
if "Pose" in line and "[" in line and "]" in line:
|
||||
want_xyz = True
|
||||
continue
|
||||
|
||||
if want_xyz:
|
||||
nums = re.findall(r"-?\d+(?:\.\d*)?(?:[eE][-+]?\d+)?", line)
|
||||
if current and len(nums) >= 3:
|
||||
try:
|
||||
vals = {"x": float(nums[0]), "y": float(nums[1]), "z": float(nums[2])}
|
||||
except ValueError:
|
||||
vals = {}
|
||||
want_xyz = False
|
||||
continue
|
||||
|
||||
# 兼容旧式 / 其他工具的 "x: N" 键值对写法
|
||||
m = re.match(r"\s*([xyz]):\s*(-?[0-9.eE+]+)\s*$", line)
|
||||
if m and current:
|
||||
try:
|
||||
vals[m.group(1)] = float(m.group(2))
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
_flush()
|
||||
return out
|
||||
|
||||
|
||||
def pose_mismatches(
|
||||
spec: SceneSpec,
|
||||
actual: Dict[str, Tuple[float, float, float]],
|
||||
tol_m: float = 5e-3,
|
||||
) -> List[str]:
|
||||
"""把"场景里写的位姿"与"Gazebo 里的实际位姿"对比，返回不一致的条目。
|
||||
|
||||
容差 5mm 起步：物体一 spawn 就会在重力下微动（落座/回弹），
|
||||
卡到 1e-6 只会得到永远为真的告警。这里要抓的是
|
||||
"根本没 spawn 到那个位置"（差几十厘米）或"spawn 了但掉下去了"。
|
||||
"""
|
||||
out: List[str] = []
|
||||
for obj in spec.objects:
|
||||
got = actual.get(obj.name)
|
||||
if got is None:
|
||||
out.append(
|
||||
f"{obj.name}：Gazebo 里找不到这个实体（scene.yaml 里写了，但没 spawn 成功？）"
|
||||
)
|
||||
continue
|
||||
d = max(abs(got[0] - obj.x), abs(got[1] - obj.y), abs(got[2] - obj.z))
|
||||
if d > tol_m:
|
||||
out.append(
|
||||
f"{obj.name}：位姿与 scene.yaml 不符（最大偏差 {d * 1000:.1f}mm）。\n"
|
||||
f" 场景要求 ({obj.x:.3f}, {obj.y:.3f}, {obj.z:.3f})，"
|
||||
f"实际 ({got[0]:.3f}, {got[1]:.3f}, {got[2]:.3f})。\n"
|
||||
f" 偏差大 = 没摆到位置；Z 偏大且持续 = 悬空掉下来了。"
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def features_from_workpiece(
|
||||
spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
|
||||
) -> Dict[str, float]:
|
||||
"""从工件模型的视觉里读出"该被检测到的特征尺寸"（米）。
|
||||
|
||||
读的是 <visual> 而不是 <collision>：检测看的是图像，图像来自视觉几何。
|
||||
（工件的碰撞体是方孔、视觉是圆环，两者尺寸不同 —— 这不是笔误：
|
||||
碰撞体只要能让规穿过去，视觉才决定相机看到什么。见模型文件头的说明。）
|
||||
|
||||
识别的办法是按 visual 的**名字**取：
|
||||
insert_*_ring → 嵌件亮环（外径 = 2×radius）→ 会被 HoughCircles 检测
|
||||
insert_*_bore → 孔本身（可能低于 hough 下限，用于交叉印证）
|
||||
center_bore → 中心大孔（应当超过 hough 上限而被滤掉）
|
||||
名字是本框架的约定，写在模型的注释里；改名字要同步改这里。
|
||||
"""
|
||||
import math
|
||||
|
||||
try:
|
||||
tree = ET.parse(model_sdf_path(spec, obj, root))
|
||||
except (OSError, ET.ParseError):
|
||||
return {}
|
||||
|
||||
out: Dict[str, float] = {}
|
||||
rings: List[float] = []
|
||||
for vis in tree.iter("visual"):
|
||||
name = vis.get("name") or ""
|
||||
cyl = vis.find("geometry/cylinder")
|
||||
if cyl is None:
|
||||
continue
|
||||
r_el = cyl.find("radius")
|
||||
if r_el is None or not r_el.text:
|
||||
continue
|
||||
try:
|
||||
r = float(r_el.text)
|
||||
except ValueError:
|
||||
continue
|
||||
if name.endswith("_ring"):
|
||||
out["insert_ring_radius"] = r
|
||||
rings.append(r)
|
||||
elif name == "center_bore":
|
||||
# ⚠️ 必须先判 center_bore 再判 _bore 后缀：名字 "center_bore" 也以
|
||||
# "_bore" 结尾，先判后缀会让中心大孔的半径覆盖掉小孔的。
|
||||
# （踩过：insert_bore_radius 报出 0.02，看起来"孔比环大"，很费解。）
|
||||
out["center_bore_radius"] = r
|
||||
elif name.endswith("_bore"):
|
||||
out["insert_bore_radius"] = r
|
||||
|
||||
# 两孔间距 = 两个环心之间的距离（从 visual 的 pose 里取）
|
||||
centers: List[Tuple[float, float]] = []
|
||||
for vis in tree.iter("visual"):
|
||||
if not (vis.get("name") or "").endswith("_ring"):
|
||||
continue
|
||||
pose_el = vis.find("pose")
|
||||
if pose_el is not None and pose_el.text:
|
||||
parts = pose_el.text.split()
|
||||
if len(parts) >= 3:
|
||||
try:
|
||||
centers.append((float(parts[0]), float(parts[1])))
|
||||
except ValueError:
|
||||
pass
|
||||
if len(centers) == 2:
|
||||
out["hole_spacing"] = math.hypot(
|
||||
centers[0][0] - centers[1][0], centers[0][1] - centers[1][1]
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def visual_ring_features(
|
||||
spec: SceneSpec, root: Optional[Path] = None
|
||||
) -> Dict[str, float]:
|
||||
"""把默认工件（operating.part_object）的检测特征整理成像素换算的输入。"""
|
||||
part = next((o for o in spec.objects if o.name == spec.operating.part_object), None)
|
||||
if part is None:
|
||||
return {}
|
||||
return features_from_workpiece(spec, part, root)
|
||||
|
||||
|
||||
def operating_pixel_report(
|
||||
spec: SceneSpec, fx_px: float, root: Optional[Path] = None
|
||||
) -> Dict[str, float]:
|
||||
"""在"作业条件"（operating 段声明的相机到工件距离）下，把工件的检测特征换算成像素。
|
||||
|
||||
这是把"仿真里到底看不看得见该看见的东西"变成**可打印、可断言**的数字：
|
||||
亮环半径 → 应当落在 hough 的 [min_radius, max_radius]
|
||||
两孔间距 → 应当落在 [hole_distance_min, hole_distance_max]
|
||||
中心大孔 → 应当**超出** max_radius（否则它会被误当成目标孔）
|
||||
"""
|
||||
feats = visual_ring_features(spec, root)
|
||||
if not feats:
|
||||
return {}
|
||||
return feature_pixels(fx_px, spec.operating.camera_to_part_m, feats)
|
||||
@@ -87,9 +87,15 @@ def _prepare(context, *args, **kwargs):
|
||||
world = LaunchConfiguration("world").perform(context).strip()
|
||||
load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower()
|
||||
with_sensors = LaunchConfiguration("with_sensors").perform(context).strip().lower()
|
||||
with_scene = LaunchConfiguration("with_scene").perform(context).strip().lower()
|
||||
scene_file = LaunchConfiguration("scene").perform(context).strip()
|
||||
headless = LaunchConfiguration("headless").perform(context).strip().lower()
|
||||
verbose = LaunchConfiguration("verbose").perform(context).strip().lower()
|
||||
|
||||
# 相机清单在下面的传感器分支里才读，但场景分支（成像几何自检）也要用它。
|
||||
# 先置空再赋值，避免"因为分支没进而在别处 NameError"这种低级但难查的错误。
|
||||
specs = []
|
||||
|
||||
try:
|
||||
root = repo_root()
|
||||
# 与 gazebo_sim_launcher 用同一套优先级，直接 `ros2 launch` 的人也不会
|
||||
@@ -194,7 +200,7 @@ def _prepare(context, *args, **kwargs):
|
||||
DEFAULT_CAMERAS_REL,
|
||||
CameraConfigError,
|
||||
bridge_args,
|
||||
check_fov_consistency,
|
||||
check_camera_consistency,
|
||||
load_cameras,
|
||||
rpy_to_quat,
|
||||
)
|
||||
@@ -204,12 +210,13 @@ def _prepare(context, *args, **kwargs):
|
||||
except CameraConfigError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 仿真相机清单有问题：\n{exc}\n") from exc
|
||||
|
||||
# 视场一致性：内参由 cameras.yaml 的 horizontal_fov 算，而**渲染**用的是
|
||||
# SDF 里的值。两者不一致时图像与内参对不上，三维点位会整体错 ——
|
||||
# 而单目预览完全看不出来（gz 自己发的 camera_info 本来就是错的，
|
||||
# 所以这份内参是"唯一真相"，更要保证它与渲染一致）。
|
||||
# 一致性：内参由 cameras.yaml 的 horizontal_fov 与 width/height 算，
|
||||
# 而**渲染**用的是 SDF 里的值。两者不一致时图像与内参对不上，
|
||||
# 三维点位会整体错 —— 而单目预览完全看不出来（gz 自己发的
|
||||
# camera_info 本来就是错的，所以这份内参是"唯一真相"，更要保证它与渲染一致）。
|
||||
# 图幅尤其要查：它决定 fx 的尺度，写错会让孔检测的像素门槛静默失效。
|
||||
# 这里只告警不拦截：真有偏差时，仍应能起仿真去现场比对。
|
||||
_fov_issues = check_fov_consistency(
|
||||
_cam_issues = check_camera_consistency(
|
||||
specs,
|
||||
world_file=Path(world_path),
|
||||
sensors_xacro=Path(root)
|
||||
@@ -219,7 +226,7 @@ def _prepare(context, *args, **kwargs):
|
||||
# LogWarn（Iron 才加），照抄新版本会直接 ImportError 让整个 launch
|
||||
# 加载失败 —— 一个"只是想说句话"的动作把仿真起不来，代价太大。
|
||||
# 前缀里带 ⚠ 保证它在满屏日志里仍然显眼。
|
||||
for _w in _fov_issues:
|
||||
for _w in _cam_issues:
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ {_w}"))
|
||||
|
||||
# 4a) 世界相机的 TF 兜底
|
||||
@@ -372,6 +379,92 @@ def _prepare(context, *args, **kwargs):
|
||||
)
|
||||
)
|
||||
|
||||
# --- 5b) spawn 场景物体（工件 / 通止规）---------------------------------
|
||||
# 场景物体**不进 world 文件**，而是在这里 spawn：
|
||||
# world 放"固定不动"的东西（地面、台子、支架相机），
|
||||
# 物料（工件、规）会按站点/工序换位置，写进 world 就等于每次换料都改 world，
|
||||
# 而 world 里还有物理参数与插件，动它的风险面太大（见 assembly.world 的注释）。
|
||||
#
|
||||
# ⚠️ 必须**等机器人 spawn 完**再 spawn 场景（这里再延后 2s）：
|
||||
# 两者都走 /world/<名>/create 服务，同时打会争同一个服务，
|
||||
# 重试窗口内失败的那个**不报错**（create 打印 OK 但实体没进），
|
||||
# 表现成"工件时有时无"，非常难查。
|
||||
if with_scene != "false":
|
||||
from arm_sim_gazebo.sim_cameras import intrinsics as _intrinsics
|
||||
from arm_sim_gazebo.sim_scene import (
|
||||
SceneError,
|
||||
check_pixel_gates,
|
||||
load_scene,
|
||||
operating_flange_z,
|
||||
operating_pixel_report,
|
||||
spawn_commands,
|
||||
validate_scene,
|
||||
)
|
||||
|
||||
try:
|
||||
scene = load_scene(
|
||||
Path(scene_file) if scene_file else None, root=root
|
||||
)
|
||||
except SceneError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 场景配置有问题：\n{exc}\n") from exc
|
||||
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] 场景: {scene.source}"))
|
||||
|
||||
# 摆放校验：悬空 / 穿模 / 出界 / 互相重叠 —— 全是"看起来正常、跑起来不对"
|
||||
# 的那一类。这里只告警不拦截：真有问题时仍需能起仿真去看现场。
|
||||
for _p in validate_scene(scene, root):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 场景摆放检查: {_p}"))
|
||||
|
||||
scene_nodes = []
|
||||
for args in spawn_commands(scene, root):
|
||||
name = args[args.index("-name") + 1]
|
||||
scene_nodes.append(
|
||||
Node(
|
||||
package="ros_gz_sim",
|
||||
executable="create",
|
||||
name=f"spawn_{name}",
|
||||
output="screen",
|
||||
arguments=["-world", wname] + args,
|
||||
)
|
||||
)
|
||||
if scene_nodes:
|
||||
actions.append(TimerAction(period=SPAWN_DELAY + 2.0, actions=scene_nodes))
|
||||
|
||||
# 成像几何自检：把"该被检测到的特征"在这台相机下换算成像素，
|
||||
# 与门槛比一遍。这是**唯一**能在上真机之前发现"孔间距不够像素门槛"
|
||||
# 的办法 —— 等真机上才发现要贵得多。
|
||||
cam = next((s for s in specs if s.ros_ns == scene.operating.camera), None) if with_sensors != "false" else None
|
||||
if cam is not None:
|
||||
_fx = _intrinsics(cam, cam.width, cam.height)[0][0]
|
||||
px = operating_pixel_report(scene, _fx, root)
|
||||
if px:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] 孔检测几何自检（相机 {}，到工件 {:.2f}m，fx={:.1f}px）：\n".format(
|
||||
scene.operating.camera, scene.operating.camera_to_part_m, _fx
|
||||
)
|
||||
+ "\n".join(f" {k} = {v:.1f}px" for k, v in sorted(px.items()))
|
||||
)
|
||||
)
|
||||
for _p in check_pixel_gates(px, scene.gates_px):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 孔检测几何: {_p}"))
|
||||
_fz = operating_flange_z(scene, root)
|
||||
if _fz is not None:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=f"[gazebo_sim] 二阶段在仿真里应悬停到法兰 z≈{_fz:.3f}m"
|
||||
f"（真机 site.yaml 写的是 0.23 —— 两者不同是**已知**的，"
|
||||
f"原因是两边「基座与台面的相对高度」不同；"
|
||||
f"决定成像的是相机到工件的距离，见 scene.yaml 的 operating 段）"
|
||||
)
|
||||
)
|
||||
else:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] 未做孔检测几何自检（缺相机清单，或 --no-sensors）"
|
||||
)
|
||||
)
|
||||
|
||||
# --- 6) 控制器 -----------------------------------------------------------
|
||||
# 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
|
||||
# rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
|
||||
@@ -408,7 +501,10 @@ def _prepare(context, *args, **kwargs):
|
||||
" ros2 topic echo /joint_states --once # 7 个 sim_joint\n"
|
||||
" ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查\n"
|
||||
" ros2 topic echo /tactile/left/status --once # 触觉三力在发\n"
|
||||
" ign model -m workpiece -p # 工件在台面上（核对摆放）\n"
|
||||
" ign model -m gauge -p # 通止规立在台面上\n"
|
||||
" python3 scripts/check_sim_cameras.py # ★ 相机的真正验收\n"
|
||||
" python3 scripts/check_sim_scene.py # ★ 场景摆放 + 孔检测几何验收\n"
|
||||
" 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
|
||||
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n"
|
||||
" ★ 别用 'ros2 topic hz' 或肉眼预览来验收相机 —— 那些只能证明'有图像'。\n"
|
||||
@@ -445,6 +541,16 @@ def generate_launch_description():
|
||||
default_value="true",
|
||||
description="是否挂 D405 与触觉。关掉跑得更快，用于纯运动学验证",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"with_scene",
|
||||
default_value="true",
|
||||
description="是否 spawn 场景物料（工件/通止规）。false=只用空台面",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"scene",
|
||||
default_value="",
|
||||
description="场景配置文件路径。留空=用 devices/sim/gazebo/config/scene.yaml",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"headless",
|
||||
default_value="true",
|
||||
|
||||
@@ -0,0 +1,407 @@
|
||||
"""scripts/check_sim_scene.py 的测试：只看**纯函数**，不启动 Gazebo。
|
||||
|
||||
这个脚本是仿真场景的"验收闸门"，它自己出错的代价和被测对象出错的代价一样大 ——
|
||||
一个恒返回空列表的检查函数会让所有人误以为场景没问题。所以这里逐条钉住：
|
||||
|
||||
* rpy→R 的复合顺序（Rz·Ry·Rx）。顺序错了的典型症状是"相机朝向反了/翻了"，
|
||||
而 Gazebo 不会报错，只会拍到别处。历史上 D435 就这么翻过一次。
|
||||
* 门槛核对必须**真的会报错**（对不上、缺键两种都要报）。
|
||||
* 门槛取交集（hough 区间 ∩ 工业校验区间），不能只取一边。
|
||||
* 相机投影的符号约定：俯视相机的视野里，世界 +X 应当出现在画面的**上方**。
|
||||
这一条钉住的是"用了 R 而不是 Rᵀ"这类转置错误 —— 它不会报错，只会把
|
||||
工件投影到镜面的位置，于是"可见性检查"永远通过。
|
||||
|
||||
脚本在 scripts/ 下，不是 ROS 包，所以用 importlib 按路径加载（与
|
||||
test_weights_layout.py 同一做法）。不依赖 ROS，也不需要构建。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
REPO = Path(__file__).resolve().parents[3]
|
||||
|
||||
|
||||
def _load():
|
||||
spec = importlib.util.spec_from_file_location(
|
||||
"check_sim_scene", REPO / "scripts/check_sim_scene.py"
|
||||
)
|
||||
mod = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(mod)
|
||||
return mod
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def chk():
|
||||
return _load()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 旋转与矩阵
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_rpy_zero_is_identity(chk):
|
||||
R = chk.rpy_to_R(0.0, 0.0, 0.0)
|
||||
for i in range(3):
|
||||
for j in range(3):
|
||||
assert R[i][j] == pytest.approx(1.0 if i == j else 0.0)
|
||||
|
||||
|
||||
def test_rpy_yaw_90_maps_x_to_y(chk):
|
||||
R = chk.rpy_to_R(0.0, 0.0, math.pi / 2)
|
||||
assert chk.matvec3(R, [1, 0, 0]) == pytest.approx([0, 1, 0], abs=1e-12)
|
||||
|
||||
|
||||
def test_rpy_pitch_90_maps_z_to_x(chk):
|
||||
"""俯仰 +90°：世界 Z 轴转到本体 X 轴（俯视相机的姿态基础）。"""
|
||||
R = chk.rpy_to_R(0.0, math.pi / 2, 0.0)
|
||||
assert chk.matvec3(R, [0, 0, 1]) == pytest.approx([1, 0, 0], abs=1e-12)
|
||||
|
||||
|
||||
def test_rpy_is_rz_ry_rx_not_other_order(chk):
|
||||
"""复合顺序必须是 Rz·Ry·Rx。
|
||||
|
||||
这是本仓库踩过的坑：URDF/SDF 的 rpy 一律是 Rz·Ry·Rx。
|
||||
如果实现成 Ry·Rz·Rx（或任何别的顺序），在单轴旋转时**完全看不出来**，
|
||||
只有多轴组合才暴露 —— 那时表现是"相机对着别处"，最难查。
|
||||
这里取一组三轴都不为 0 的角度，与显式乘积对比。
|
||||
"""
|
||||
r, p, y = 0.31, -0.72, 1.13
|
||||
|
||||
def Rz(a):
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
return [[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]
|
||||
|
||||
def Ry(a):
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
return [[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]]
|
||||
|
||||
def Rx(a):
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
return [[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]]
|
||||
|
||||
want = chk.matmul3(chk.matmul3(Rz(y), Ry(p)), Rx(r))
|
||||
got = chk.rpy_to_R(r, p, y)
|
||||
for i in range(3):
|
||||
assert got[i] == pytest.approx(want[i], abs=1e-12)
|
||||
|
||||
|
||||
def test_matmul_matches_manual_and_transpose_roundtrip(chk):
|
||||
a = chk.rpy_to_R(0.2, 0.3, 0.4)
|
||||
b = chk.rpy_to_R(-0.7, 0.1, 0.9)
|
||||
# (A·B)·v == A·(B·v)：矩阵乘法与逐次作用的等价性
|
||||
v = [0.5, -1.25, 3.0]
|
||||
assert chk.matvec3(chk.matmul3(a, b), v) == pytest.approx(
|
||||
chk.matvec3(a, chk.matvec3(b, v)), abs=1e-12
|
||||
)
|
||||
# 旋转矩阵正交：Aᵀ·A = I（投影用到的 R_ow 就是靠这个从 R_wo 来的）
|
||||
prod = chk.matmul3(chk.transpose3(a), a)
|
||||
for i in range(3):
|
||||
assert prod[i] == pytest.approx([1.0 if i == j else 0.0 for j in range(3)], abs=1e-12)
|
||||
|
||||
|
||||
def test_transpose3_swaps_indices(chk):
|
||||
m = [[1, 2, 3], [4, 5, 6], [7, 8, 9]]
|
||||
assert chk.transpose3(m) == [[1, 4, 7], [2, 5, 8], [3, 6, 9]]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 投影：用真实相机模型走一遍，钉住符号约定
|
||||
# ---------------------------------------------------------------------------
|
||||
def _project(chk, cam_pose, rpy, points, k):
|
||||
"""把世界点投到像素：与脚本里 check_d435_visibility 同一套算法。
|
||||
|
||||
单独抽出来是为了能独立测"符号约定"——如果只在 check_d435_visibility
|
||||
内部测，就必须先造出场景和模型文件，测不出单点投影的对错。
|
||||
"""
|
||||
fx, fy, cx, cy = k
|
||||
R_wb = chk.rpy_to_R(*rpy)
|
||||
R_bo = chk.rpy_to_R(-math.pi / 2, 0.0, -math.pi / 2)
|
||||
R_ow = chk.transpose3(chk.matmul3(R_wb, R_bo))
|
||||
out = []
|
||||
for p in points:
|
||||
d = [p[i] - cam_pose[i] for i in range(3)]
|
||||
o = chk.matvec3(R_ow, d)
|
||||
out.append((o[2], cx + fx * o[0] / o[2], cy + fy * o[1] / o[2]))
|
||||
return out
|
||||
|
||||
|
||||
def test_downward_camera_puts_point_directly_below_at_image_center(chk):
|
||||
"""正下方俯视：轴上的点必须落在画面正中。
|
||||
|
||||
这一条同时验证了 R_bo（本体→光学）两个 90° 旋转的符号：
|
||||
只要有一个符号错，轴上的点就会偏到画面外，而 Gazebo 里看起来"相机在工作"。
|
||||
"""
|
||||
k = (600.0, 600.0, 320.0, 240.0)
|
||||
# 相机在原点上方 1m 处，俯仰 +90° = 正下方看
|
||||
(z, u, v), = _project(chk, [0.0, 0.0, 1.0], (0.0, math.pi / 2, 0.0), [(0.0, 0.0, 0.0)], k)
|
||||
assert z == pytest.approx(1.0) # 光学 z = 深度，正值表示在相机前方
|
||||
assert u == pytest.approx(320.0)
|
||||
assert v == pytest.approx(240.0)
|
||||
|
||||
|
||||
def test_downward_camera_x_axis_appears_upward_in_image(chk):
|
||||
"""俯视相机的符号约定：世界 +X → 画面**上方**（v 变小）。
|
||||
|
||||
这是纯约定，没有"对错"，但**必须与 ros_gz_bridge + 真机坐标系**同一套。
|
||||
钉住它，是为了让任何旋转被转置/取反的改动立刻红掉 ——
|
||||
否则可见性检查会在镜像位置永远通过（工件实际在画面外也报"完整可见"）。
|
||||
"""
|
||||
k = (600.0, 600.0, 320.0, 240.0)
|
||||
# 相机在 (0,0,1)，朝下看；目标点在 +X 方向、同一高度
|
||||
(z, u, v), = _project(chk, [0.0, 0.0, 1.0], (0.0, math.pi / 2, 0.0), [(0.2, 0.0, 0.0)], k)
|
||||
assert z == pytest.approx(1.0)
|
||||
assert u == pytest.approx(320.0)
|
||||
assert v < 240.0 # 上方
|
||||
|
||||
# +Y 方向同理应落在画面左右某一侧（这里钉住是左侧）
|
||||
(_, u2, _), = _project(chk, [0.0, 0.0, 1.0], (0.0, math.pi / 2, 0.0), [(0.0, 0.2, 0.0)], k)
|
||||
assert u2 < 320.0
|
||||
|
||||
|
||||
def test_projection_scales_inversely_with_depth(chk):
|
||||
"""同样的横向偏移，深度加倍 → 像素偏移减半（针孔模型的基本性质）。"""
|
||||
k = (600.0, 600.0, 320.0, 240.0)
|
||||
near, = _project(chk, [0.0, 0.0, 0.5], (0.0, math.pi / 2, 0.0), [(0.1, 0.0, 0.0)], k)
|
||||
far, = _project(chk, [0.0, 0.0, 1.0], (0.0, math.pi / 2, 0.0), [(0.1, 0.0, 0.0)], k)
|
||||
d_near = 240.0 - near[2]
|
||||
d_far = 240.0 - far[2]
|
||||
assert d_near == pytest.approx(2 * d_far, rel=1e-9)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 门槛核对
|
||||
# ---------------------------------------------------------------------------
|
||||
def _write_tree(root: Path, pkg: dict, site: dict) -> Path:
|
||||
(root / "src/arm_application/config").mkdir(parents=True, exist_ok=True)
|
||||
(root / "config").mkdir(parents=True, exist_ok=True)
|
||||
import yaml
|
||||
|
||||
(root / "src/arm_application/config/hole_detect.yaml").write_text(
|
||||
yaml.safe_dump(pkg), encoding="utf-8"
|
||||
)
|
||||
(root / "config/site.yaml").write_text(yaml.safe_dump(site), encoding="utf-8")
|
||||
return root
|
||||
|
||||
|
||||
class _Scene:
|
||||
"""只带 gates_px 的最小替身（check_gate_drift 只读这一个字段）。"""
|
||||
|
||||
def __init__(self, gates):
|
||||
self.gates_px = gates
|
||||
|
||||
|
||||
def _fake_scene_with_part():
|
||||
"""check_hover_height 用得到的最小场景替身。
|
||||
|
||||
只保留它真正读的两个字段：operating.camera_to_part_m（拼错误信息用）。
|
||||
刻意不造真实模型文件 —— 单测要跑得飞快、且不依赖仓库里的 SDF。
|
||||
"""
|
||||
|
||||
class _Op:
|
||||
camera_to_part_m = 0.17
|
||||
camera_offset_below_flange_m = 0.05
|
||||
|
||||
class _S:
|
||||
operating = _Op()
|
||||
objects = [type("O", (), {"name": "part"})()]
|
||||
|
||||
return _S()
|
||||
|
||||
|
||||
def test_effective_gates_intersects_hough_and_validation_ranges(chk, tmp_path):
|
||||
"""半径门槛 = hough 区间 ∩ 工业校验区间。
|
||||
|
||||
只取一边会得出乐观结论：例如 hough 允许 3px 的圆、但校验环节要求 ≥6px，
|
||||
那么"能被找到的圆"其实用不上。真正的门槛是两者更严的那个。
|
||||
"""
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={
|
||||
"hough_min_radius": 3,
|
||||
"hough_max_radius": 50,
|
||||
"min_radius": 6,
|
||||
"max_radius": 30,
|
||||
"hole_distance_min": 150,
|
||||
"hole_distance_max": 700,
|
||||
},
|
||||
site={},
|
||||
)
|
||||
eff = chk.effective_gates(root)
|
||||
assert eff["insert_ring_radius"] == (6.0, 30.0)
|
||||
assert eff["hole_spacing"] == (150.0, 700.0)
|
||||
|
||||
|
||||
def test_effective_gates_site_overrides_package(chk, tmp_path):
|
||||
"""site.yaml 必须压过包内默认值（这是真机改参数唯一该改的地方）。"""
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={
|
||||
"hough_min_radius": 10,
|
||||
"hough_max_radius": 30,
|
||||
"min_radius": 10,
|
||||
"max_radius": 30,
|
||||
"hole_distance_min": 150,
|
||||
"hole_distance_max": 700,
|
||||
},
|
||||
site={"hole_detect": {"hough_min_radius": 6, "min_radius": 6, "hole_distance_min": 90}},
|
||||
)
|
||||
eff = chk.effective_gates(root)
|
||||
assert eff["insert_ring_radius"] == (6.0, 30.0)
|
||||
assert eff["hole_spacing"] == (90.0, 700.0)
|
||||
|
||||
|
||||
def test_effective_gates_survives_garbage_values(chk, tmp_path):
|
||||
"""配置里写了 null / 字符串等垃圾值时要退回默认，而不是抛异常崩掉验收。"""
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={"hough_min_radius": None, "min_radius": "abc", "hough_max_radius": 30},
|
||||
site={},
|
||||
)
|
||||
eff = chk.effective_gates(root)
|
||||
lo, hi = eff["insert_ring_radius"]
|
||||
assert lo == 10.0 and hi == 30.0
|
||||
|
||||
|
||||
def test_gate_drift_passes_when_equal(chk, tmp_path):
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={
|
||||
"hough_min_radius": 6,
|
||||
"hough_max_radius": 30,
|
||||
"min_radius": 6,
|
||||
"max_radius": 30,
|
||||
"hole_distance_min": 90,
|
||||
"hole_distance_max": 700,
|
||||
},
|
||||
site={},
|
||||
)
|
||||
scene = _Scene({"insert_ring_radius": (6.0, 30.0), "hole_spacing": (90.0, 700.0)})
|
||||
assert chk.check_gate_drift(scene, root) == []
|
||||
|
||||
|
||||
def test_gate_drift_flags_mismatch_with_both_numbers(chk, tmp_path):
|
||||
"""对不上必须报错，而且要把"写的值"和"真机值"都打出来 —— 只看一边没法修。"""
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={
|
||||
"hough_min_radius": 6,
|
||||
"hough_max_radius": 30,
|
||||
"min_radius": 6,
|
||||
"max_radius": 30,
|
||||
"hole_distance_min": 90,
|
||||
"hole_distance_max": 700,
|
||||
},
|
||||
site={},
|
||||
)
|
||||
scene = _Scene({"insert_ring_radius": (10.0, 30.0), "hole_spacing": (90.0, 700.0)})
|
||||
probs = chk.check_gate_drift(scene, root)
|
||||
assert len(probs) == 1
|
||||
msg = probs[0]
|
||||
assert "[10, 30]" in msg and "[6, 30]" in msg
|
||||
assert "scene.yaml" in msg # 必须指明去哪改
|
||||
|
||||
|
||||
def test_gate_drift_flags_missing_key(chk, tmp_path):
|
||||
"""scene.yaml 少了某个门槛键 = 那一项不再被校验 = 静默放行，必须报出来。"""
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={
|
||||
"hough_min_radius": 6,
|
||||
"hough_max_radius": 30,
|
||||
"min_radius": 6,
|
||||
"max_radius": 30,
|
||||
"hole_distance_min": 90,
|
||||
"hole_distance_max": 700,
|
||||
},
|
||||
site={},
|
||||
)
|
||||
scene = _Scene({"insert_ring_radius": (6.0, 30.0)}) # 缺 hole_spacing
|
||||
probs = chk.check_gate_drift(scene, root)
|
||||
assert len(probs) == 1
|
||||
assert "hole_spacing" in probs[0]
|
||||
|
||||
|
||||
def test_gate_drift_checks_both_bounds_not_just_one(chk, tmp_path):
|
||||
"""只改上限也要被抓到（容易只比下限的写法）。"""
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={
|
||||
"hough_min_radius": 6,
|
||||
"hough_max_radius": 30,
|
||||
"min_radius": 6,
|
||||
"max_radius": 30,
|
||||
"hole_distance_min": 90,
|
||||
"hole_distance_max": 700,
|
||||
},
|
||||
site={},
|
||||
)
|
||||
scene = _Scene({"insert_ring_radius": (6.0, 40.0), "hole_spacing": (90.0, 700.0)})
|
||||
assert chk.check_gate_drift(scene, root)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 工作空间
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_workspace_z_limits_reads_site(chk, tmp_path):
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={},
|
||||
site={"servo": {"workspace_xyz_min": [0.2, -0.5, 0.18], "workspace_xyz_max": [0.8, 0.5, 0.65]}},
|
||||
)
|
||||
assert chk.workspace_z_limits(root) == (0.18, 0.65)
|
||||
|
||||
|
||||
def test_workspace_z_limits_degrades_to_none(chk, tmp_path):
|
||||
"""site.yaml 结构不对时返回 None（调用方降级成"不核对"），不要崩。"""
|
||||
root = _write_tree(tmp_path, pkg={}, site={"servo": {}})
|
||||
assert chk.workspace_z_limits(root) is None
|
||||
root2 = _write_tree(tmp_path / "b", pkg={}, site={"servo": {"workspace_xyz_min": [0, 0]}})
|
||||
assert chk.workspace_z_limits(root2) is None
|
||||
|
||||
|
||||
def test_check_hover_height_flags_above_workspace(chk, tmp_path, monkeypatch):
|
||||
"""悬停高度超出工作空间上界必须报错，并指明两个可调的旋钮。
|
||||
|
||||
这个坑很实际：相机离工件越近像素越大（检测更好），但法兰越高（越够不着）。
|
||||
两个目标对立，所以报错信息里必须同时给出两边，否则人会只调一边来回撞。
|
||||
"""
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={},
|
||||
site={"servo": {"workspace_xyz_min": [0.0, 0.0, 0.0], "workspace_xyz_max": [1.0, 1.0, 0.20]}},
|
||||
)
|
||||
# check_hover_height 内部是"用哪个函数"由 arm_sim_gazebo.sim_scene 提供，
|
||||
# 所以替身要打在源模块上（打在 chk 上不起作用 —— 内部是函数内 import）。
|
||||
import arm_sim_gazebo.sim_scene as sim_scene
|
||||
|
||||
monkeypatch.setattr(sim_scene, "operating_flange_z", lambda spec, root=None: 0.632)
|
||||
probs = chk.check_hover_height(_fake_scene_with_part(), root)
|
||||
assert len(probs) == 1
|
||||
assert "0.632" in probs[0] and "0.2" in probs[0]
|
||||
assert "camera_to_part_m" in probs[0]
|
||||
|
||||
|
||||
def test_check_hover_height_silent_when_inside_workspace(chk, tmp_path, monkeypatch):
|
||||
root = _write_tree(
|
||||
tmp_path,
|
||||
pkg={},
|
||||
site={"servo": {"workspace_xyz_min": [0.0, 0.0, 0.0], "workspace_xyz_max": [1.0, 1.0, 0.70]}},
|
||||
)
|
||||
import arm_sim_gazebo.sim_scene as sim_scene
|
||||
|
||||
monkeypatch.setattr(sim_scene, "operating_flange_z", lambda spec, root=None: 0.632)
|
||||
assert chk.check_hover_height(_fake_scene_with_part(), root) == []
|
||||
|
||||
|
||||
def test_check_hover_height_reports_when_part_missing(chk, tmp_path):
|
||||
"""part_object 名字写错时要说"算不出来"，而不是静默跳过。"""
|
||||
|
||||
class _S:
|
||||
objects = []
|
||||
gates_px = {}
|
||||
|
||||
assert chk.check_hover_height(_S(), tmp_path)
|
||||
@@ -321,10 +321,10 @@ cameras:
|
||||
assert "horizontal_fov" in str(ei.value)
|
||||
|
||||
|
||||
# ----------------------------------------------------------- 视场一致性比对
|
||||
def test_fov_check_flags_world_camera_mismatch(tmp_path):
|
||||
# ------------------------------------------- 声明与实际的一致性比对（视场 + 图幅）
|
||||
def test_cam_check_flags_world_camera_mismatch(tmp_path):
|
||||
"""世界相机：按 <topic> 精确匹配，能抓到 cameras.yaml 与 world 不一致。"""
|
||||
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||
from arm_sim_gazebo.sim_cameras import check_camera_consistency
|
||||
|
||||
world = tmp_path / "assembly.world"
|
||||
world.write_text(
|
||||
@@ -332,7 +332,8 @@ def test_fov_check_flags_world_camera_mismatch(tmp_path):
|
||||
<sdf><world name="w">
|
||||
<model name="d435"><link name="link"><sensor name="s" type="rgbd_camera">
|
||||
<topic>/d435</topic>
|
||||
<camera><horizontal_fov>1.5184</horizontal_fov></camera>
|
||||
<camera><horizontal_fov>1.5184</horizontal_fov>
|
||||
<image><width>640</width><height>480</height></image></camera>
|
||||
</sensor></link></model>
|
||||
</world></sdf>
|
||||
""",
|
||||
@@ -346,31 +347,132 @@ def test_fov_check_flags_world_camera_mismatch(tmp_path):
|
||||
horizontal_fov=1.5184,
|
||||
world_model="d435",
|
||||
world_link="link",
|
||||
width=640,
|
||||
height=480,
|
||||
)
|
||||
assert check_fov_consistency([spec], world_file=world) == []
|
||||
assert check_camera_consistency([spec], world_file=world) == []
|
||||
|
||||
bad = CameraSpec(**{**spec.__dict__, "horizontal_fov": 1.2})
|
||||
issues = check_fov_consistency([bad], world_file=world)
|
||||
issues = check_camera_consistency([bad], world_file=world)
|
||||
assert len(issues) == 1 and "1.5184" in issues[0] and "1.2" in issues[0]
|
||||
|
||||
|
||||
def test_fov_check_flags_urdf_camera_when_value_absent(tmp_path):
|
||||
def test_cam_check_flags_world_camera_size_mismatch(tmp_path):
|
||||
"""图幅不一致必须报出来，且要解释清楚后果是"像素门槛静默失效"。
|
||||
|
||||
这条独立的理由是：视场错会让三维点位**整体偏移**（还能靠反投影发现），
|
||||
而图幅错只是**尺度**不对（几何依然自洽、预览依然正常），
|
||||
更难怀疑到 —— 正是本项目反复遇到的那类静默错误。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_cameras import check_camera_consistency
|
||||
|
||||
world = tmp_path / "assembly.world"
|
||||
world.write_text(
|
||||
"""
|
||||
<sdf><world name="w">
|
||||
<model name="d405"><link name="link"><sensor name="s" type="rgbd_camera">
|
||||
<topic>/d405</topic>
|
||||
<camera><horizontal_fov>1.5184</horizontal_fov>
|
||||
<image><width>640</width><height>480</height></image></camera>
|
||||
</sensor></link></model>
|
||||
</world></sdf>
|
||||
""",
|
||||
encoding="utf-8",
|
||||
)
|
||||
spec = CameraSpec(
|
||||
gz_ns="/d405",
|
||||
ros_ns="d405",
|
||||
link_frame="d405_link",
|
||||
optical_frame="d405_color_optical_frame",
|
||||
horizontal_fov=1.5184,
|
||||
world_model="d405",
|
||||
world_link="link",
|
||||
width=848, # 声明 848，而渲染是 640
|
||||
height=480,
|
||||
)
|
||||
issues = check_camera_consistency([spec], world_file=world)
|
||||
assert len(issues) == 1, issues
|
||||
assert "848" in issues[0] and "640x480" in issues[0]
|
||||
assert "hole_distance_min" in issues[0], "必须点明哪类门槛会失效"
|
||||
assert "0.755" in issues[0], "要给出倍数，便于判断影响量级"
|
||||
|
||||
|
||||
def test_cam_check_size_skipped_when_not_declared(tmp_path):
|
||||
"""清单不声明图幅时不报图幅问题 —— 没有声明就没有"不一致"。
|
||||
|
||||
避免噪声：一台相机若本来就不关心尺寸（回退 640x480），
|
||||
对它报"读不到尺寸"只会淹没真正的告警。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_cameras import check_camera_consistency
|
||||
|
||||
world = tmp_path / "assembly.world"
|
||||
world.write_text(
|
||||
"""
|
||||
<sdf><world name="w">
|
||||
<model name="m"><link name="link"><sensor name="s" type="rgbd_camera">
|
||||
<topic>/c</topic>
|
||||
<camera><horizontal_fov>1.5184</horizontal_fov></camera>
|
||||
</sensor></link></model>
|
||||
</world></sdf>
|
||||
""",
|
||||
encoding="utf-8",
|
||||
)
|
||||
spec = CameraSpec(
|
||||
gz_ns="/c",
|
||||
ros_ns="c",
|
||||
link_frame="l",
|
||||
optical_frame="o",
|
||||
horizontal_fov=1.5184,
|
||||
world_model="m",
|
||||
world_link="link",
|
||||
# width/height 故意留 0
|
||||
)
|
||||
assert check_camera_consistency([spec], world_file=world) == []
|
||||
|
||||
|
||||
def test_cam_check_flags_urdf_camera_when_value_absent(tmp_path):
|
||||
"""腕上相机：视场在 xacro 里，只能"文件里出现过才算过"，但要能抓到缺失。"""
|
||||
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||
from arm_sim_gazebo.sim_cameras import check_camera_consistency
|
||||
|
||||
xacro = tmp_path / "arm.sim.sensors.xacro"
|
||||
xacro.write_text("<horizontal_fov>1.5184</horizontal_fov>", encoding="utf-8")
|
||||
spec = _spec()
|
||||
assert check_fov_consistency([spec], sensors_xacro=xacro) == []
|
||||
xacro.write_text(
|
||||
"<horizontal_fov>1.5184</horizontal_fov>"
|
||||
'params="image_width:=848 image_height:=480"',
|
||||
encoding="utf-8",
|
||||
)
|
||||
spec = _spec(width=848, height=480)
|
||||
assert check_camera_consistency([spec], sensors_xacro=xacro) == []
|
||||
|
||||
xacro.write_text("<horizontal_fov>1.2</horizontal_fov>", encoding="utf-8")
|
||||
issues = check_fov_consistency([spec], sensors_xacro=xacro)
|
||||
xacro.write_text(
|
||||
"<horizontal_fov>1.2</horizontal_fov>"
|
||||
'params="image_width:=848 image_height:=480"',
|
||||
encoding="utf-8",
|
||||
)
|
||||
issues = check_camera_consistency([spec], sensors_xacro=xacro)
|
||||
assert len(issues) == 1 and "1.2" in issues[0]
|
||||
|
||||
|
||||
def test_fov_check_warns_when_world_camera_sensor_missing(tmp_path):
|
||||
def test_cam_check_flags_urdf_camera_size_default(tmp_path):
|
||||
"""腕上相机的图幅来自宏的参数默认值，改了它忘了改 cameras.yaml 要被抓到。
|
||||
|
||||
launch 不会覆盖 image_width/image_height，所以宏默认值就是渲染用的值。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_cameras import check_camera_consistency
|
||||
|
||||
xacro = tmp_path / "arm.sim.sensors.xacro"
|
||||
xacro.write_text(
|
||||
"<horizontal_fov>1.5184</horizontal_fov>"
|
||||
'params="image_width:=640 image_height:=480"',
|
||||
encoding="utf-8",
|
||||
)
|
||||
issues = check_camera_consistency([_spec(width=848, height=480)], sensors_xacro=xacro)
|
||||
assert len(issues) == 1, issues
|
||||
assert "640x480" in issues[0] and "848x480" in issues[0]
|
||||
|
||||
|
||||
def test_cam_check_warns_when_world_camera_sensor_missing(tmp_path):
|
||||
"""world 里找不到对应 <sensor> 时要告警，而不是静默放过。"""
|
||||
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||
from arm_sim_gazebo.sim_cameras import check_camera_consistency
|
||||
|
||||
world = tmp_path / "assembly.world"
|
||||
world.write_text("<sdf><world name='w'/></sdf>", encoding="utf-8")
|
||||
@@ -382,7 +484,7 @@ def test_fov_check_warns_when_world_camera_sensor_missing(tmp_path):
|
||||
horizontal_fov=1.5184,
|
||||
world_model="d435",
|
||||
)
|
||||
issues = check_fov_consistency([spec], world_file=world)
|
||||
issues = check_camera_consistency([spec], world_file=world)
|
||||
assert len(issues) == 1 and "找不到" in issues[0]
|
||||
|
||||
|
||||
@@ -395,11 +497,11 @@ def test_fov_check_against_real_repo_files():
|
||||
"""
|
||||
from pathlib import Path
|
||||
|
||||
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||
from arm_sim_gazebo.sim_cameras import check_camera_consistency
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
specs = load_cameras(root=root)
|
||||
issues = check_fov_consistency(
|
||||
issues = check_camera_consistency(
|
||||
specs,
|
||||
world_file=root / "devices/sim/gazebo/worlds/assembly.world",
|
||||
sensors_xacro=root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
|
||||
|
||||
@@ -0,0 +1,939 @@
|
||||
"""sim_scene.py 的测试：场景读取、摆放校验、成像几何、Gazebo 位姿解析。
|
||||
|
||||
为什么这个模块需要单独一套测试：它管的全是**静默失效**。
|
||||
摆放错了不会报错，只表现为"工件自己会动"；门槛抄错了不会报错，只表现为
|
||||
"仿真通过、真机不认"。所以这里逐条钉住判断逻辑，而不是只测"能不能读文件"。
|
||||
|
||||
三条重点（都是真实踩过或差点踩到的）：
|
||||
* `world_boxes` 必须用"中心 + 半宽"变换。对 min/max 取绝对值再对称化
|
||||
会把偏移量吃掉，盒子凭空变大 → 判出假重叠（规座四块"全都"和规相交）。
|
||||
* 嵌套（规立在规座的方孔里）**不是**重叠。所以必须逐碰撞体、三维判，
|
||||
而不是拿总包围盒在 XY 上相交。
|
||||
* `parse_spawned_poses` 曾经按 "x: N" 键值对解析，而 Fortress 实际输出是
|
||||
方括号两行（XYZ / RPY）。两者对不上 → 永远解析出空 → 调用方"降级成跳过"，
|
||||
位姿检查其实**从来没跑过**，而日志看起来一切正常。
|
||||
这里用真实输出样本钉住它。
|
||||
|
||||
全部用 tmp_path 自建迷你仓库，不依赖仓库里真实的模型文件，也不需要 ROS。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
from arm_sim_gazebo.sim_scene import (
|
||||
EPS,
|
||||
Bounds,
|
||||
SceneError,
|
||||
SceneObject,
|
||||
check_pixel_gates,
|
||||
collision_boxes,
|
||||
feature_pixels,
|
||||
features_from_workpiece,
|
||||
footprint,
|
||||
load_scene,
|
||||
model_height,
|
||||
model_sdf_path,
|
||||
object_pose_map,
|
||||
operating_flange_z,
|
||||
operating_pixel_report,
|
||||
overlapping_pairs,
|
||||
parse_spawned_poses,
|
||||
pose_mismatches,
|
||||
px_per_m,
|
||||
spawn_commands,
|
||||
validate_scene,
|
||||
visual_ring_features,
|
||||
world_boxes,
|
||||
_boxes_overlap,
|
||||
_local_extent,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 造迷你仓库
|
||||
# ---------------------------------------------------------------------------
|
||||
MODELS_REL = "devices/sim/gazebo/models"
|
||||
|
||||
|
||||
def _write_model(
|
||||
root,
|
||||
name: str,
|
||||
collisions: list[str] | None = None,
|
||||
visuals: list[str] | None = None,
|
||||
write_config: bool = True,
|
||||
static: bool = False,
|
||||
):
|
||||
"""写一个模型目录。collisions/visuals 直接给 XML 片段。
|
||||
|
||||
默认给出**底面贴在原点**的碰撞体（_box_on_floor）—— 这是本框架的建模约定，
|
||||
也是 validate_scene 的前提。要故意违反约定（测"原点不在底面"）时显式传入
|
||||
居中的 _box_collision。
|
||||
"""
|
||||
d = root / MODELS_REL / name
|
||||
d.mkdir(parents=True, exist_ok=True)
|
||||
coll = "\n".join(collisions if collisions is not None else [_box_on_floor("body")])
|
||||
vis = "\n".join(visuals or [])
|
||||
(d / "model.sdf").write_text(
|
||||
f"""<?xml version="1.0"?>
|
||||
<sdf version="1.6">
|
||||
<model name="{name}">
|
||||
<static>{str(static).lower()}</static>
|
||||
<link name="link">
|
||||
{coll}
|
||||
{vis}
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
""",
|
||||
encoding="utf-8",
|
||||
)
|
||||
if write_config:
|
||||
(d / "model.config").write_text(
|
||||
f"""<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>{name}</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.6">model.sdf</sdf>
|
||||
</model>
|
||||
""",
|
||||
encoding="utf-8",
|
||||
)
|
||||
return d
|
||||
|
||||
|
||||
def _box_collision(name: str, size=(0.06, 0.06, 0.02), pose=None) -> str:
|
||||
"""几何居中的碰撞体（pose 不给就是原点居中）。"""
|
||||
p = f"<pose>{pose}</pose>" if pose else ""
|
||||
return (
|
||||
f' <collision name="{name}">\n'
|
||||
f" {p}\n"
|
||||
f" <geometry><box><size>{size[0]} {size[1]} {size[2]}</size></box></geometry>\n"
|
||||
f" </collision>"
|
||||
)
|
||||
|
||||
|
||||
def _box_on_floor(name: str, size=(0.06, 0.06, 0.02), xy=None) -> str:
|
||||
"""底面贴在 z=0 的碰撞体 —— 符合本框架"原点在底面中心"的建模约定。
|
||||
|
||||
validate_scene 会检查"局部 z_min ≈ 0"，所以凡是希望场景校验通过、
|
||||
或想测摆放相关的逻辑，都要用这个而不是居中的 _box_collision。
|
||||
"""
|
||||
x, y = xy or (0.0, 0.0)
|
||||
return _box_collision(name, size=size, pose=f"{x} {y} {size[2] / 2.0} 0 0 0")
|
||||
|
||||
|
||||
def _ring_visual(name: str, radius: float, pose="0 0 0.001 0 0 0") -> str:
|
||||
return (
|
||||
f' <visual name="{name}">\n'
|
||||
f" <pose>{pose}</pose>\n"
|
||||
f" <geometry><cylinder><radius>{radius}</radius><length>0.001</length></cylinder></geometry>\n"
|
||||
f" </visual>"
|
||||
)
|
||||
|
||||
|
||||
def _write_scene(
|
||||
root,
|
||||
objects: list[dict],
|
||||
table_top_z: float = 0.40,
|
||||
table_bounds=None,
|
||||
operating: dict | None = None,
|
||||
gates_px: dict | None = None,
|
||||
):
|
||||
cfg = root / "devices/sim/gazebo/config"
|
||||
cfg.mkdir(parents=True, exist_ok=True)
|
||||
doc = {
|
||||
"models_dir": MODELS_REL,
|
||||
"table_top_z": table_top_z,
|
||||
"table_bounds": table_bounds or {"x": [0.15, 0.95], "y": [-0.60, 0.60]},
|
||||
"objects": objects,
|
||||
}
|
||||
if operating is not None:
|
||||
doc["operating"] = operating
|
||||
if gates_px is not None:
|
||||
doc["gates_px"] = gates_px
|
||||
p = cfg / "scene.yaml"
|
||||
p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
return p
|
||||
|
||||
|
||||
def _obj(name="workpiece", model=None, x=0.5, y=0.0, z=0.40, yaw=0.0) -> dict:
|
||||
return {"name": name, "model": model or name, "pose": {"x": x, "y": y, "z": z, "yaw": yaw}}
|
||||
|
||||
|
||||
class _Spec:
|
||||
"""手搓 SceneSpec 的小工具：几何类测试不需要真的写文件。"""
|
||||
|
||||
def __init__(self, model: str, objects=None, table_top_z=0.40, gates=None, operating=None):
|
||||
from arm_sim_gazebo.sim_scene import Operating
|
||||
|
||||
self.models_dir = MODELS_REL
|
||||
self.table_top_z = table_top_z
|
||||
self.table_bounds = Bounds(0.15, 0.95, -0.60, 0.60)
|
||||
self.operating = operating or Operating("d405", 0.20, 0.05, "workpiece")
|
||||
self.objects = objects or [SceneObject("workpiece", model, 0.5, 0.0, 0.40)]
|
||||
self.gates_px = gates or {}
|
||||
self.source = None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 读配置
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_load_scene_missing_file_says_where_and_how(tmp_path):
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
msg = str(e.value)
|
||||
assert "找不到场景配置" in msg
|
||||
# "框架自带、不是站点数据"这个区别必须写明，否则人会以为缺了是正常的
|
||||
assert "框架自带" in msg
|
||||
assert "--scene" in msg
|
||||
|
||||
|
||||
def test_load_scene_missing_required_keys_shows_minimal_example(tmp_path):
|
||||
cfg = tmp_path / "devices/sim/gazebo/config"
|
||||
cfg.mkdir(parents=True)
|
||||
(cfg / "scene.yaml").write_text("table_top_z: 0.4\n", encoding="utf-8")
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
msg = str(e.value)
|
||||
assert "models_dir" in msg and "objects" in msg
|
||||
assert "models_dir:" in msg # 最小例子
|
||||
|
||||
|
||||
def test_load_scene_top_level_not_mapping_raises(tmp_path):
|
||||
cfg = tmp_path / "devices/sim/gazebo/config"
|
||||
cfg.mkdir(parents=True)
|
||||
(cfg / "scene.yaml").write_text("- 这是个列表\n", encoding="utf-8")
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "顶层不是映射" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_bad_yaml_raises(tmp_path):
|
||||
cfg = tmp_path / "devices/sim/gazebo/config"
|
||||
cfg.mkdir(parents=True)
|
||||
(cfg / "scene.yaml").write_text("a: [1, 2\n", encoding="utf-8")
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "解析失败" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_bad_table_bounds_raises(tmp_path):
|
||||
cfg = tmp_path / "devices/sim/gazebo/config"
|
||||
cfg.mkdir(parents=True)
|
||||
(cfg / "scene.yaml").write_text(
|
||||
"models_dir: m\ntable_top_z: 0.4\ntable_bounds: {x: [1]}\nobjects: []\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "table_bounds" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_objects_not_list_raises(tmp_path):
|
||||
cfg = tmp_path / "devices/sim/gazebo/config"
|
||||
cfg.mkdir(parents=True)
|
||||
(cfg / "scene.yaml").write_text(
|
||||
"models_dir: m\ntable_top_z: 0.4\ntable_bounds: {x: [0, 1], y: [0, 1]}\nobjects: 3\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "objects 不是列表" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_object_without_name_or_model_raises(tmp_path):
|
||||
_write_scene(tmp_path, [{"name": "a", "pose": {"x": 1, "y": 1, "z": 0.4}}])
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
msg = str(e.value)
|
||||
assert "缺 name 或 model" in msg
|
||||
# 两个字段的角色要说清（实体名 vs 目录名），否则补的时候容易搞混
|
||||
assert "实体名" in msg and "目录名" in msg
|
||||
|
||||
|
||||
def test_load_scene_pose_not_number_raises(tmp_path):
|
||||
_write_scene(tmp_path, [{"name": "a", "model": "a", "pose": {"x": "左边", "y": 0, "z": 0.4}}])
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "pose 不是数字" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_operating_defaults(tmp_path):
|
||||
_write_scene(tmp_path, [], operating={})
|
||||
spec = load_scene(root=tmp_path)
|
||||
assert spec.operating.camera == "d405"
|
||||
assert spec.operating.part_object == "workpiece"
|
||||
assert spec.operating.camera_to_part_m == 0.20
|
||||
|
||||
|
||||
def test_load_scene_operating_strips_slashes(tmp_path):
|
||||
_write_scene(tmp_path, [], operating={"camera": "/d405/"})
|
||||
assert load_scene(root=tmp_path).operating.camera == "d405"
|
||||
|
||||
|
||||
def test_load_scene_parses_objects_and_gates(tmp_path):
|
||||
_write_scene(
|
||||
tmp_path,
|
||||
[_obj("workpiece", x=0.52, yaw=0.3)],
|
||||
gates_px={"insert_ring_radius": [6, 30]},
|
||||
)
|
||||
spec = load_scene(root=tmp_path)
|
||||
assert len(spec.objects) == 1
|
||||
o = spec.objects[0]
|
||||
assert (o.name, o.model, o.x, o.z, o.yaw) == ("workpiece", "workpiece", 0.52, 0.40, 0.3)
|
||||
assert spec.gates_px["insert_ring_radius"] == (6.0, 30.0)
|
||||
|
||||
|
||||
def test_load_scene_gates_reversed_raises(tmp_path):
|
||||
"""下限比上限大 = 任何值都不合格。这种"写反了"必须当场拦住。"""
|
||||
_write_scene(tmp_path, [], gates_px={"insert_ring_radius": [30, 6]})
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
msg = str(e.value)
|
||||
assert "下限比上限大" in msg and "写反" in msg
|
||||
|
||||
|
||||
def test_load_scene_gates_bad_shape_raises(tmp_path):
|
||||
_write_scene(tmp_path, [], gates_px={"hole_spacing": 90})
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "[下限, 上限]" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_gates_not_mapping_raises(tmp_path):
|
||||
_write_scene(tmp_path, [], gates_px=[1, 2])
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "gates_px 不是映射" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_records_source_path(tmp_path):
|
||||
p = _write_scene(tmp_path, [])
|
||||
assert load_scene(root=tmp_path).source == p
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Bounds
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_bounds_contains_with_tolerance():
|
||||
outer = Bounds(0.0, 1.0, 0.0, 1.0)
|
||||
assert outer.contains(Bounds(0.1, 0.9, 0.1, 0.9))
|
||||
# 尾差（sqrt/解析产生）不能报错：1e-6 以内算相等
|
||||
assert outer.contains(Bounds(-EPS / 2, 1.0 + EPS / 2, 0.0, 1.0))
|
||||
assert not outer.contains(Bounds(-0.01, 1.0, 0.0, 1.0))
|
||||
|
||||
|
||||
def test_bounds_touching_is_not_overlap():
|
||||
"""恰好贴合不算重叠 —— 这条是"规坐在规座顶面上"能通过检查的前提。"""
|
||||
a = Bounds(0.0, 1.0, 0.0, 1.0)
|
||||
b = Bounds(1.0, 2.0, 0.0, 1.0)
|
||||
assert not a.overlaps(b)
|
||||
assert a.overlaps(Bounds(0.5, 1.5, 0.0, 1.0))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 从 SDF 读几何
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_collision_boxes_reads_box_cylinder_sphere_with_pose(tmp_path):
|
||||
_write_model(
|
||||
tmp_path,
|
||||
"m",
|
||||
collisions=[
|
||||
_box_collision("box1", size=(0.10, 0.20, 0.04), pose="0.05 0 0.02 0 0 0"),
|
||||
' <collision name="cyl"><geometry><cylinder><radius>0.01</radius>'
|
||||
"<length>0.06</length></cylinder></geometry></collision>",
|
||||
' <collision name="sph"><geometry><sphere><radius>0.005</radius>'
|
||||
"</sphere></geometry></collision>",
|
||||
],
|
||||
)
|
||||
boxes = collision_boxes(tmp_path / MODELS_REL / "m" / "model.sdf")
|
||||
assert len(boxes) == 3
|
||||
assert boxes[0] == pytest.approx((0.0, 0.10, -0.10, 0.10, 0.0, 0.04))
|
||||
assert boxes[1] == pytest.approx((-0.01, 0.01, -0.01, 0.01, -0.03, 0.03))
|
||||
assert boxes[2] == pytest.approx((-0.005, 0.005, -0.005, 0.005, -0.005, 0.005))
|
||||
|
||||
|
||||
def test_collision_boxes_bad_xml_returns_empty(tmp_path):
|
||||
p = tmp_path / "bad.sdf"
|
||||
p.write_text("<sdf><model>没闭合", encoding="utf-8")
|
||||
assert collision_boxes(p) == []
|
||||
|
||||
|
||||
def test_collision_boxes_missing_file_returns_empty(tmp_path):
|
||||
assert collision_boxes(tmp_path / "没有这个文件.sdf") == []
|
||||
|
||||
|
||||
def test_collision_without_geometry_is_skipped(tmp_path):
|
||||
"""有 <collision> 但没有 <geometry> 时不能当成"边长 0 的盒子"，那会变成假碰撞。"""
|
||||
_write_model(
|
||||
tmp_path,
|
||||
"m",
|
||||
collisions=[' <collision name="empty"></collision>', _box_collision("real")],
|
||||
)
|
||||
assert len(collision_boxes(tmp_path / MODELS_REL / "m" / "model.sdf")) == 1
|
||||
|
||||
|
||||
def test_local_extent_none_without_collisions(tmp_path):
|
||||
_write_model(tmp_path, "m", collisions=[])
|
||||
assert _local_extent(tmp_path / MODELS_REL / "m" / "model.sdf") is None
|
||||
|
||||
|
||||
def test_model_height_is_top_of_collision_extent(tmp_path):
|
||||
_write_model(tmp_path, "part", collisions=[_box_on_floor("body", size=(0.09, 0.06, 0.012))])
|
||||
spec = _Spec("part")
|
||||
assert model_height(spec, spec.objects[0], tmp_path) == pytest.approx(0.012)
|
||||
|
||||
|
||||
def test_model_sdf_path_is_model_dir_plus_model_sdf(tmp_path):
|
||||
spec = _Spec("part")
|
||||
p = model_sdf_path(spec, spec.objects[0], tmp_path)
|
||||
assert p == tmp_path / MODELS_REL / "part" / "model.sdf"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# world_boxes：变换必须"中心 + 半宽"
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_world_boxes_offset_element_is_not_inflated(tmp_path):
|
||||
"""★ 回归：偏移的碰撞体不能被"对称化"放大。
|
||||
|
||||
这是实际踩过的假重叠：规座的一侧方块在 x=+16.75mm、半宽 13.25mm，
|
||||
正确范围是 x∈[3.5, 30]mm。若实现成 abs(min)/abs(max) 再对称化，
|
||||
就会变成 x∈[-30, +30]mm —— 于是"规座的四个方块全都和规的轴相交"，
|
||||
报出一个根本不存在的问题，逼着人把这个检查关掉。
|
||||
"""
|
||||
_write_model(
|
||||
tmp_path,
|
||||
"stand",
|
||||
collisions=[_box_collision("right", size=(0.0265, 0.06, 0.032), pose="0.01675 0 0.016 0 0 0")],
|
||||
)
|
||||
spec = _Spec("stand", [SceneObject("stand", "stand", 0.30, 0.28, 0.40)])
|
||||
(x0, x1, y0, y1, z0, z1) = world_boxes(spec, spec.objects[0], tmp_path)[0]
|
||||
assert (x0, x1) == pytest.approx((0.30 + 0.0035, 0.30 + 0.0300))
|
||||
assert (y0, y1) == pytest.approx((0.28 - 0.03, 0.28 + 0.03))
|
||||
assert (z0, z1) == pytest.approx((0.40, 0.432))
|
||||
|
||||
|
||||
def test_world_boxes_yaw_90_swaps_extents(tmp_path):
|
||||
_write_model(tmp_path, "p", collisions=[_box_collision("b", size=(0.10, 0.04, 0.02))])
|
||||
spec = _Spec("p", [SceneObject("p", "p", 0.5, 0.0, 0.40, yaw=math.pi / 2)])
|
||||
(x0, x1, y0, y1, _, _) = world_boxes(spec, spec.objects[0], tmp_path)[0]
|
||||
assert (x1 - x0) == pytest.approx(0.04)
|
||||
assert (y1 - y0) == pytest.approx(0.10)
|
||||
|
||||
|
||||
def test_world_boxes_yaw_45_uses_projection(tmp_path):
|
||||
"""斜摆：外接盒半宽 = hx·|cos| + hy·|sin|。"""
|
||||
_write_model(tmp_path, "p", collisions=[_box_collision("b", size=(0.10, 0.04, 0.02))])
|
||||
spec = _Spec("p", [SceneObject("p", "p", 0.0, 0.0, 0.40, yaw=math.pi / 4)])
|
||||
(x0, x1, _, _, _, _) = world_boxes(spec, spec.objects[0], tmp_path)[0]
|
||||
want = 0.05 * math.cos(math.pi / 4) + 0.02 * math.sin(math.pi / 4)
|
||||
assert (x1 - x0) / 2 == pytest.approx(want)
|
||||
|
||||
|
||||
def test_world_boxes_rotates_element_center_around_origin(tmp_path):
|
||||
"""偏移的碰撞体，其中心也要跟着绕模型原点转。"""
|
||||
_write_model(tmp_path, "p", collisions=[_box_collision("b", size=(0.02, 0.02, 0.02), pose="0.1 0 0 0 0 0")])
|
||||
spec = _Spec("p", [SceneObject("p", "p", 0.0, 0.0, 0.40, yaw=math.pi / 2)])
|
||||
(x0, x1, y0, y1, _, _) = world_boxes(spec, spec.objects[0], tmp_path)[0]
|
||||
# 局部 (0.1, 0) 绕原点转 90° → 世界 (0, 0.1)
|
||||
assert (x0 + x1) / 2 == pytest.approx(0.0)
|
||||
assert (y0 + y1) / 2 == pytest.approx(0.1)
|
||||
|
||||
|
||||
def test_world_boxes_no_collision_returns_empty(tmp_path):
|
||||
_write_model(tmp_path, "p", collisions=[])
|
||||
spec = _Spec("p")
|
||||
assert world_boxes(spec, spec.objects[0], tmp_path) == []
|
||||
|
||||
|
||||
def test_footprint_is_union_of_world_boxes(tmp_path):
|
||||
_write_model(
|
||||
tmp_path,
|
||||
"p",
|
||||
collisions=[
|
||||
_box_collision("a", size=(0.02, 0.02, 0.02), pose="-0.05 0 0.01 0 0 0"),
|
||||
_box_collision("b", size=(0.02, 0.02, 0.02), pose="0.05 0 0.01 0 0 0"),
|
||||
],
|
||||
)
|
||||
spec = _Spec("p", [SceneObject("p", "p", 0.5, 0.0, 0.40)])
|
||||
fp = footprint(spec, spec.objects[0], tmp_path)
|
||||
assert fp == pytest.approx((0.44, 0.56, -0.01, 0.01))
|
||||
|
||||
|
||||
def test_footprint_none_without_collisions(tmp_path):
|
||||
_write_model(tmp_path, "p", collisions=[])
|
||||
spec = _Spec("p")
|
||||
assert footprint(spec, spec.objects[0], tmp_path) is None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 相交判定
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_boxes_overlap_true_when_penetrating():
|
||||
a = (0.0, 0.1, 0.0, 0.1, 0.0, 0.1)
|
||||
assert _boxes_overlap(a, (0.05, 0.15, 0.05, 0.15, 0.05, 0.15))
|
||||
|
||||
|
||||
def test_boxes_overlap_false_when_touching():
|
||||
a = (0.0, 0.1, 0.0, 0.1, 0.0, 0.1)
|
||||
# 面贴合：x 方向恰好接到 a 的右面
|
||||
assert not _boxes_overlap(a, (0.1, 0.2, 0.0, 0.1, 0.0, 0.1))
|
||||
|
||||
|
||||
def test_boxes_overlap_false_when_disjoint_in_one_axis_only():
|
||||
"""三轴都要相交才算。只在一个轴上分开就是分离的（这是三维判的意义）。"""
|
||||
a = (0.0, 0.1, 0.0, 0.1, 0.0, 0.1)
|
||||
assert not _boxes_overlap(a, (0.05, 0.15, 0.05, 0.15, 0.2, 0.3))
|
||||
|
||||
|
||||
def test_boxes_overlap_false_when_disjoint():
|
||||
a = (0.0, 0.1, 0.0, 0.1, 0.0, 0.1)
|
||||
assert not _boxes_overlap(a, (0.2, 0.3, 0.2, 0.3, 0.2, 0.3))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 重叠检测：嵌套必须放行
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_overlapping_pairs_allows_nested_parts(tmp_path):
|
||||
"""★ 正常嵌套不算重叠：规立在规座的方孔里。
|
||||
|
||||
规座 = 4 块围出一个孔；规 = 一根细轴插在孔中（四周有缝隙）。
|
||||
若实现成"拿总包围盒在 XY 上相交"，这里必然误报 ——
|
||||
而误报的后果是人把检查关掉，之后真重叠也抓不到了。
|
||||
"""
|
||||
_write_model(
|
||||
tmp_path,
|
||||
"stand",
|
||||
collisions=[
|
||||
_box_collision("west", size=(0.021, 0.06, 0.032), pose="-0.0195 0 0.016 0 0 0"),
|
||||
_box_collision("east", size=(0.021, 0.06, 0.032), pose=" 0.0195 0 0.016 0 0 0"),
|
||||
_box_collision("south", size=(0.06, 0.021, 0.032), pose="0 -0.0195 0.016 0 0 0"),
|
||||
_box_collision("north", size=(0.06, 0.021, 0.032), pose="0 0.0195 0.016 0 0 0"),
|
||||
],
|
||||
)
|
||||
_write_model(tmp_path, "pin", collisions=[_box_collision("shaft", size=(0.007, 0.007, 0.05))])
|
||||
objects = [
|
||||
SceneObject("stand", "stand", 0.30, 0.28, 0.40),
|
||||
# 轴插在孔里，四周各有 2.5mm 缝
|
||||
SceneObject("pin", "pin", 0.30, 0.28, 0.402),
|
||||
]
|
||||
spec = _Spec("stand", objects)
|
||||
assert overlapping_pairs(spec, tmp_path) == []
|
||||
|
||||
|
||||
def test_overlapping_pairs_detects_true_penetration(tmp_path):
|
||||
_write_model(tmp_path, "a", collisions=[_box_collision("b", size=(0.06, 0.06, 0.02))])
|
||||
_write_model(tmp_path, "b", collisions=[_box_collision("b", size=(0.06, 0.06, 0.02))])
|
||||
objects = [
|
||||
SceneObject("a", "a", 0.50, 0.00, 0.40),
|
||||
SceneObject("b", "b", 0.51, 0.00, 0.40), # 中心只差 10mm，必然嵌入
|
||||
]
|
||||
spec = _Spec("a", objects)
|
||||
pairs = overlapping_pairs(spec, tmp_path)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0][0] == "a" and pairs[0][1] == "b"
|
||||
assert "互相嵌入" in pairs[0][2]
|
||||
|
||||
|
||||
def test_overlapping_pairs_empty_scene(tmp_path):
|
||||
spec = _Spec("a", [])
|
||||
assert overlapping_pairs(spec, tmp_path) == []
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# validate_scene
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_validate_scene_ok(tmp_path):
|
||||
_write_model(tmp_path, "workpiece", collisions=[_box_on_floor("b", size=(0.09, 0.06, 0.012))])
|
||||
_write_scene(tmp_path, [_obj("workpiece", x=0.52, z=0.40)])
|
||||
assert validate_scene(load_scene(root=tmp_path), tmp_path) == []
|
||||
|
||||
|
||||
def test_validate_scene_missing_models_dir(tmp_path):
|
||||
_write_scene(tmp_path, [])
|
||||
(tmp_path / MODELS_REL).mkdir(parents=True, exist_ok=True)
|
||||
# models_dir 指向不存在的地方
|
||||
cfg = tmp_path / "devices/sim/gazebo/config/scene.yaml"
|
||||
d = yaml.safe_load(cfg.read_text(encoding="utf-8"))
|
||||
d["models_dir"] = "devices/sim/gazebo/没有这个目录"
|
||||
cfg.write_text(yaml.safe_dump(d, allow_unicode=True), encoding="utf-8")
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert len(probs) == 1
|
||||
assert "模型目录不存在" in probs[0] and "仓库根" in probs[0]
|
||||
|
||||
|
||||
def test_validate_scene_missing_model_sdf_explains_layout(tmp_path):
|
||||
(tmp_path / MODELS_REL).mkdir(parents=True)
|
||||
_write_scene(tmp_path, [_obj("workpiece")])
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert any("模型文件不存在" in p and "<模型名>/model.sdf" in p for p in probs)
|
||||
|
||||
|
||||
def test_validate_scene_missing_model_config_warns_silently_found(tmp_path):
|
||||
"""缺 model.config 时 Gazebo **不报错**（model:// 静默找不到），必须由我们拦。"""
|
||||
_write_model(tmp_path, "workpiece", write_config=False)
|
||||
_write_scene(tmp_path, [_obj("workpiece")])
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert len(probs) == 1
|
||||
assert "model.config" in probs[0]
|
||||
assert "静默" in probs[0]
|
||||
|
||||
|
||||
def test_validate_scene_no_collision_is_a_problem(tmp_path):
|
||||
_write_model(tmp_path, "workpiece", collisions=[])
|
||||
_write_scene(tmp_path, [_obj("workpiece")])
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert len(probs) == 1
|
||||
assert "解析不出任何 <collision>" in probs[0]
|
||||
assert "物理上不存在" in probs[0]
|
||||
|
||||
|
||||
def test_validate_scene_origin_not_at_bottom(tmp_path):
|
||||
"""原点在质心（z 从 -h/2 开始）时一律报错 —— 否则整体会悬空/沉下去。"""
|
||||
_write_model(
|
||||
tmp_path,
|
||||
"workpiece",
|
||||
collisions=[_box_collision("b", size=(0.09, 0.06, 0.012), pose="0 0 -0.006 0 0 0")],
|
||||
)
|
||||
_write_scene(tmp_path, [_obj("workpiece")])
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert len(probs) == 1
|
||||
assert "原点不在底面" in probs[0]
|
||||
|
||||
|
||||
def test_validate_scene_below_table(tmp_path):
|
||||
_write_model(tmp_path, "workpiece")
|
||||
_write_scene(tmp_path, [_obj("workpiece", z=0.30)])
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert len(probs) == 1
|
||||
assert "穿进台子" in probs[0] and "弹起来" in probs[0]
|
||||
|
||||
|
||||
def test_validate_scene_floating_is_allowed(tmp_path):
|
||||
"""悬空（z 高于台面）**不报错** —— 这是有意的设计决定，钉住它。
|
||||
|
||||
理由：`z` 表达的是"摆在哪"，而"落座"是物理引擎的事。
|
||||
模型底面贴原点时，z=0.45 只是"从比台面高 5cm 的地方松手"，会自己掉下来坐稳。
|
||||
若不报错变成"必须等于台面"，那么稍微抬高一点放置都会被拦，检查会被人关掉。
|
||||
真正报错的是**低于**台面（穿模），因为那会让求解器把它顶出来。
|
||||
"""
|
||||
_write_model(tmp_path, "workpiece", collisions=[_box_on_floor("b", size=(0.09, 0.06, 0.012))])
|
||||
_write_scene(tmp_path, [_obj("workpiece", z=0.45)])
|
||||
assert validate_scene(load_scene(root=tmp_path), tmp_path) == []
|
||||
|
||||
|
||||
def test_validate_scene_out_of_bounds(tmp_path):
|
||||
_write_model(tmp_path, "workpiece", collisions=[_box_on_floor("b", size=(0.09, 0.06, 0.012))])
|
||||
_write_scene(tmp_path, [_obj("workpiece", x=0.94)]) # 右缘 0.985 > 0.95
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert len(probs) == 1
|
||||
assert "超出台面" in probs[0]
|
||||
|
||||
|
||||
def test_validate_scene_duplicate_names(tmp_path):
|
||||
_write_model(tmp_path, "workpiece")
|
||||
_write_scene(
|
||||
tmp_path,
|
||||
[_obj("workpiece", x=0.40), _obj("workpiece", model="workpiece", x=0.70)],
|
||||
)
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert any("物体名重复" in p for p in probs)
|
||||
|
||||
|
||||
def test_validate_scene_collects_multiple_problems_at_once(tmp_path):
|
||||
"""一次报全，而不是修一个跑一次 —— 现场调摆放通常是连着挪几个。"""
|
||||
_write_model(tmp_path, "a", collisions=[_box_collision("b", size=(0.06, 0.06, 0.02))])
|
||||
_write_model(tmp_path, "b", collisions=[_box_collision("b", size=(0.06, 0.06, 0.02))])
|
||||
_write_scene(tmp_path, [_obj("a", x=0.94, z=0.30), _obj("b", x=0.95)])
|
||||
probs = validate_scene(load_scene(root=tmp_path), tmp_path)
|
||||
assert len(probs) >= 2
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# spawn 命令
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_spawn_commands_uses_file_name_pose_and_yaw(tmp_path):
|
||||
_write_model(tmp_path, "workpiece")
|
||||
_write_scene(tmp_path, [_obj("workpiece", x=0.52, y=0.0, z=0.40, yaw=0.25)])
|
||||
spec = load_scene(root=tmp_path)
|
||||
cmds = spawn_commands(spec, tmp_path)
|
||||
assert len(cmds) == 1
|
||||
c = cmds[0]
|
||||
assert c[0] == "-file" and c[1].endswith("/workpiece/model.sdf")
|
||||
assert c[2] == "-name" and c[3] == "workpiece"
|
||||
assert c[4] == "-x" and float(c[5]) == pytest.approx(0.52)
|
||||
assert c[6] == "-y" and float(c[7]) == pytest.approx(0.0)
|
||||
assert c[8] == "-z" and float(c[9]) == pytest.approx(0.40)
|
||||
# 偏航用大写的 -Y（ros_gz_sim 的约定）
|
||||
assert c[10] == "-Y" and float(c[11]) == pytest.approx(0.25)
|
||||
|
||||
|
||||
def test_spawn_commands_order_follows_scene_file(tmp_path):
|
||||
"""顺序有意义：规座必须先于规 spawn，否则规会先掉下去再被"套住"。"""
|
||||
_write_model(tmp_path, "gauge_stand")
|
||||
_write_model(tmp_path, "gauge")
|
||||
_write_scene(
|
||||
tmp_path,
|
||||
[_obj("gauge_stand", x=0.30, y=0.28), _obj("gauge", x=0.30, y=0.28, z=0.402)],
|
||||
)
|
||||
names = [c[3] for c in spawn_commands(load_scene(root=tmp_path), tmp_path)]
|
||||
assert names == ["gauge_stand", "gauge"]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 像素几何
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_px_per_m_is_fx_over_distance():
|
||||
assert px_per_m(447.0, 0.17) == pytest.approx(447.0 / 0.17)
|
||||
|
||||
|
||||
def test_px_per_m_rejects_nonpositive_distance():
|
||||
"""距离为 0 会除零。宁可抛错也不要返回 inf —— inf 会一路传成"像素无穷大"。"""
|
||||
with pytest.raises(ValueError):
|
||||
px_per_m(400.0, 0.0)
|
||||
with pytest.raises(ValueError):
|
||||
px_per_m(400.0, -0.1)
|
||||
|
||||
|
||||
def test_feature_pixels_scales_all_features():
|
||||
out = feature_pixels(400.0, 0.20, {"a": 0.01, "b": 0.02})
|
||||
assert out["a"] == pytest.approx(0.01 * 2000)
|
||||
assert out["b"] == pytest.approx(0.02 * 2000)
|
||||
|
||||
|
||||
def test_check_pixel_gates_passes_in_range():
|
||||
assert check_pixel_gates({"r": 18.0}, {"r": (6.0, 30.0)}) == []
|
||||
|
||||
|
||||
def test_check_pixel_gates_flags_out_of_range_with_causes():
|
||||
"""超范围时必须给出三种成因与对应旋钮，否则人只能猜。"""
|
||||
probs = check_pixel_gates({"r": 4.0}, {"r": (6.0, 30.0)})
|
||||
assert len(probs) == 1
|
||||
assert "4.0" in probs[0] and "[6.0, 30.0]" in probs[0]
|
||||
assert "camera_to_part_m" in probs[0] and "cameras.yaml" in probs[0]
|
||||
|
||||
|
||||
def test_check_pixel_gates_skips_features_without_measurement():
|
||||
"""算不出来的特征（模型里有你没量的东西）不该被当成不合格。"""
|
||||
assert check_pixel_gates({}, {"r": (6.0, 30.0)}) == []
|
||||
|
||||
|
||||
def test_check_pixel_gates_boundaries_are_inclusive():
|
||||
assert check_pixel_gates({"r": 6.0}, {"r": (6.0, 30.0)}) == []
|
||||
assert check_pixel_gates({"r": 30.0}, {"r": (6.0, 30.0)}) == []
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 工件的检测特征
|
||||
# ---------------------------------------------------------------------------
|
||||
def _workpiece_visuals() -> list[str]:
|
||||
return [
|
||||
_ring_visual("insert_k1_ring", 0.007, pose="0.029 0 0.003 0 0 0"),
|
||||
_ring_visual("insert_k2_ring", 0.007, pose="-0.029 0 0.003 0 0 0"),
|
||||
_ring_visual("insert_k1_bore", 0.0035, pose="0.029 0 0.003 0 0 0"),
|
||||
_ring_visual("insert_k2_bore", 0.0035, pose="-0.029 0 0.003 0 0 0"),
|
||||
_ring_visual("center_bore", 0.02, pose="0 0 0.003 0 0 0"),
|
||||
]
|
||||
|
||||
|
||||
def test_features_from_workpiece_reads_conventions(tmp_path):
|
||||
_write_model(tmp_path, "workpiece", visuals=_workpiece_visuals())
|
||||
spec = _Spec("workpiece")
|
||||
f = features_from_workpiece(spec, spec.objects[0], tmp_path)
|
||||
assert f["insert_ring_radius"] == pytest.approx(0.007)
|
||||
assert f["insert_bore_radius"] == pytest.approx(0.0035)
|
||||
assert f["hole_spacing"] == pytest.approx(0.058)
|
||||
|
||||
|
||||
def test_center_bore_is_not_eaten_by_bore_suffix(tmp_path):
|
||||
"""★ 回归：`center_bore` 也以 `_bore` 结尾。
|
||||
|
||||
若先判后缀，中心大孔（半径 20mm）会覆盖掉小孔（3.5mm），
|
||||
报出"孔半径比亮环还大"这种自相矛盾的数字，非常费解。
|
||||
必须先判全名。
|
||||
"""
|
||||
_write_model(tmp_path, "workpiece", visuals=_workpiece_visuals())
|
||||
spec = _Spec("workpiece")
|
||||
f = features_from_workpiece(spec, spec.objects[0], tmp_path)
|
||||
assert f["center_bore_radius"] == pytest.approx(0.02)
|
||||
assert f["insert_bore_radius"] == pytest.approx(0.0035)
|
||||
|
||||
|
||||
def test_features_from_workpiece_empty_without_cylinders(tmp_path):
|
||||
_write_model(tmp_path, "workpiece", visuals=[' <visual name="insert_k1_ring">'
|
||||
"<geometry><box><size>1 1 1</size></box></geometry>"
|
||||
"</visual>"])
|
||||
spec = _Spec("workpiece")
|
||||
assert features_from_workpiece(spec, spec.objects[0], tmp_path) == {}
|
||||
|
||||
|
||||
def test_features_from_workpiece_hole_spacing_needs_two_rings(tmp_path):
|
||||
"""只有一个环时不该编出一个间距来（宁可没有，也不要错的数）。"""
|
||||
_write_model(tmp_path, "workpiece", visuals=[_ring_visual("insert_k1_ring", 0.007)])
|
||||
spec = _Spec("workpiece")
|
||||
assert "hole_spacing" not in features_from_workpiece(spec, spec.objects[0], tmp_path)
|
||||
|
||||
|
||||
def test_features_from_workpiece_bad_file_returns_empty(tmp_path):
|
||||
spec = _Spec("不存在")
|
||||
assert features_from_workpiece(spec, spec.objects[0], tmp_path) == {}
|
||||
|
||||
|
||||
def test_visual_ring_features_empty_when_part_object_missing(tmp_path):
|
||||
from arm_sim_gazebo.sim_scene import Operating
|
||||
|
||||
_write_model(tmp_path, "workpiece", visuals=_workpiece_visuals())
|
||||
spec = _Spec("workpiece")
|
||||
spec.operating = Operating("d405", 0.17, 0.05, "别的东西")
|
||||
assert visual_ring_features(spec, tmp_path) == {}
|
||||
|
||||
|
||||
def test_operating_pixel_report_matches_hand_calc(tmp_path):
|
||||
"""亮环半径按"相机到工件的距离"换算：r_px = r_m · fx / d。"""
|
||||
_write_model(tmp_path, "workpiece", visuals=_workpiece_visuals())
|
||||
spec = _Spec("workpiece")
|
||||
spec.operating = type(spec.operating)("d405", 0.17, 0.05, "workpiece")
|
||||
fx = 446.8
|
||||
px = operating_pixel_report(spec, fx, tmp_path)
|
||||
assert px["insert_ring_radius"] == pytest.approx(0.007 * fx / 0.17, rel=1e-6)
|
||||
assert px["hole_spacing"] == pytest.approx(0.058 * fx / 0.17, rel=1e-6)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 悬停高度
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_operating_flange_z_formula(tmp_path):
|
||||
"""法兰高度 = 工件上表面 + 相机到工件距离 + 相机伸出量。"""
|
||||
_write_model(tmp_path, "workpiece", collisions=[_box_on_floor("b", size=(0.09, 0.06, 0.02))])
|
||||
_write_scene(
|
||||
tmp_path,
|
||||
[_obj("workpiece", z=0.40)],
|
||||
table_top_z=0.40,
|
||||
operating={"camera_to_part_m": 0.17, "camera_offset_below_flange_m": 0.05},
|
||||
)
|
||||
spec = load_scene(root=tmp_path)
|
||||
assert operating_flange_z(spec, tmp_path) == pytest.approx(0.40 + 0.02 + 0.17 + 0.05)
|
||||
|
||||
|
||||
def test_operating_flange_z_none_without_part(tmp_path):
|
||||
_write_scene(tmp_path, [], operating={"part_object": "workpiece"})
|
||||
assert operating_flange_z(load_scene(root=tmp_path), tmp_path) is None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Gazebo 位姿解析
|
||||
# ---------------------------------------------------------------------------
|
||||
FORTRESS_POSE = """
|
||||
Requesting state for world [assembly]...
|
||||
|
||||
Model: [105]
|
||||
- Name: workpiece
|
||||
- Pose [ XYZ (m) ] [ RPY (rad) ]:
|
||||
[0.520000 -0.000000 0.400000]
|
||||
[0.000000 -0.000000 0.000000]
|
||||
"""
|
||||
|
||||
FORTRESS_LIST = """
|
||||
Requesting state for world [assembly]...
|
||||
|
||||
Available models:
|
||||
- ground_plane
|
||||
- work_table
|
||||
- d435
|
||||
- rizon_sim
|
||||
- gauge_stand
|
||||
- workpiece
|
||||
- gauge
|
||||
"""
|
||||
|
||||
|
||||
def test_parse_fortress_model_pose_output():
|
||||
"""★ 回归：Fortress 的实际输出是方括号两行（XYZ / RPY），不是键值对。
|
||||
|
||||
这条测试用的是**真实抓下来的输出样本**，因为真实格式与当初的假设不同，
|
||||
而"解析不出来就返回空字典"的降级设计让这个 bug 一直没被发现：
|
||||
位姿检查从来没跑过，日志只说"跳过"。
|
||||
"""
|
||||
out = parse_spawned_poses(FORTRESS_POSE)
|
||||
assert out == {"workpiece": (0.52, -0.0, 0.4)}
|
||||
|
||||
|
||||
def test_parse_multiple_models_in_one_dump():
|
||||
text = (
|
||||
"Model: [1]\n - Name: a\n - Pose [ XYZ (m) ] [ RPY (rad) ]:\n"
|
||||
" [1.0 2.0 3.0]\n [0 0 0]\n"
|
||||
"Model: [2]\n - Name: b\n - Pose [ XYZ (m) ] [ RPY (rad) ]:\n"
|
||||
" [-0.5 0.25 0.4]\n [0 0 1.5]\n"
|
||||
)
|
||||
assert parse_spawned_poses(text) == {"a": (1.0, 2.0, 3.0), "b": (-0.5, 0.25, 0.4)}
|
||||
|
||||
|
||||
def test_parse_model_list_has_names_but_no_poses():
|
||||
"""--list 只有名字。不能把它当成"位姿是 0" —— 那会报出满屏假偏差。"""
|
||||
assert parse_spawned_poses(FORTRESS_LIST) == {}
|
||||
|
||||
|
||||
def test_parse_legacy_keyvalue_format_still_supported():
|
||||
text = "Name: workpiece\nx: 0.52\ny: 0.01\nz: 0.40\n"
|
||||
assert parse_spawned_poses(text) == {"workpiece": (0.52, 0.01, 0.40)}
|
||||
|
||||
|
||||
def test_parse_garbage_returns_empty_not_crash():
|
||||
assert parse_spawned_poses("完全不是这个格式\n啥也没有") == {}
|
||||
|
||||
|
||||
def test_parse_pose_with_scientific_notation():
|
||||
text = (
|
||||
" - Name: tiny\n - Pose [ XYZ (m) ] [ RPY (rad) ]:\n"
|
||||
" [1.5e-3 -2.5E-4 4.0e-1]\n [0 0 0]\n"
|
||||
)
|
||||
assert parse_spawned_poses(text) == {"tiny": (1.5e-3, -2.5e-4, 0.4)}
|
||||
|
||||
|
||||
def test_parse_ignores_rpy_line():
|
||||
"""RPY 那一行也是方括号数值，绝不能被当成 XYZ 读进去。"""
|
||||
out = parse_spawned_poses(
|
||||
" - Name: a\n - Pose [ XYZ (m) ] [ RPY (rad) ]:\n"
|
||||
" [1.0 2.0 3.0]\n [9.0 9.0 9.0]\n"
|
||||
)
|
||||
assert out["a"] == (1.0, 2.0, 3.0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 位姿核对
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_pose_mismatches_reports_missing_entity():
|
||||
spec = _Spec("workpiece")
|
||||
probs = pose_mismatches(spec, {})
|
||||
assert len(probs) == 1
|
||||
assert "找不到这个实体" in probs[0]
|
||||
|
||||
|
||||
def test_pose_mismatches_within_tolerance_is_silent():
|
||||
"""落座时的毫米级微动不能报 —— 卡到 1e-6 只会得到永远为真的告警。"""
|
||||
spec = _Spec("workpiece")
|
||||
assert pose_mismatches(spec, {"workpiece": (0.503, 0.001, 0.401)}) == []
|
||||
|
||||
|
||||
def test_pose_mismatches_reports_large_offset_with_numbers():
|
||||
spec = _Spec("workpiece")
|
||||
probs = pose_mismatches(spec, {"workpiece": (0.20, 0.0, 0.40)})
|
||||
assert len(probs) == 1
|
||||
assert "300.0mm" in probs[0]
|
||||
assert "0.500" in probs[0] and "0.200" in probs[0]
|
||||
|
||||
|
||||
def test_pose_mismatches_hints_falling_objects():
|
||||
"""Z 差得特别大时给出"是不是掉下去了"的提示。"""
|
||||
spec = _Spec("workpiece")
|
||||
probs = pose_mismatches(spec, {"workpiece": (0.5, 0.0, -0.02)})
|
||||
assert "悬空掉下来" in probs[0]
|
||||
|
||||
|
||||
def test_object_pose_map_includes_yaw():
|
||||
spec = _Spec("workpiece")
|
||||
assert object_pose_map(spec) == {"workpiece": (0.5, 0.0, 0.40, 0.0)}
|
||||
Reference in New Issue
Block a user