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
@@ -53,6 +53,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
|
||||
| 双爪 ROS | `scripts/run_dual_gripper.sh` |
|
||||
| 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` |
|
||||
| Orisys 触觉 | `scripts/run_tactile.sh [--cli]` |
|
||||
| Gazebo 仿真 | `scripts/run_sim_gz.sh [--gui]`（停：`scripts/stop_sim_gz.sh`） |
|
||||
| 看配置 | `scripts/show_site_config.sh` |
|
||||
|
||||
89 与 91 **各有** 手眼、相机序列号、锁高、named poses，禁止混用。
|
||||
@@ -70,12 +71,13 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
|
||||
| [docs/GRIPPER.md](docs/GRIPPER.md) | PGEA + RGI |
|
||||
| [docs/HOLE_DETECT.md](docs/HOLE_DETECT.md) | 入孔检测 |
|
||||
| [docs/TACTILE.md](docs/TACTILE.md) | Orisys 视触觉 |
|
||||
| [devices/sim/gazebo/README.md](devices/sim/gazebo/README.md) | Gazebo 仿真手册（使用方法 / 建模要求 / 导入格式 / 配置方式 / 避坑清单） |
|
||||
| [docs/INTERFACES.md](docs/INTERFACES.md) | ROS / 脚本接口 |
|
||||
| [docs/TRAJECTORIES.md](docs/TRAJECTORIES.md) | 轨迹回放 |
|
||||
|
||||
## 上传 git 时
|
||||
|
||||
- **要提交**：`src/` `scripts/` `docs/` `config/site.example.yaml` `models/README.md` `models/**/*.labels.yaml` `handeye/`
|
||||
- **要提交**：`src/` `scripts/` `docs/` `devices/` `config/site.example.yaml` `models/README.md` `models/**/*.labels.yaml` `handeye/`
|
||||
- **不要提交**：`config/site.yaml`（含本机 IP/序列号）、`handeye/tmp/`（临时产物）、`build/` `install/` `log/`、YOLO `.pt/.onnx/.engine`、`thirdparty/` `vendor/`
|
||||
- 规则已写在 `.gitignore`
|
||||
|
||||
|
||||
@@ -111,11 +111,15 @@ paths:
|
||||
# bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640
|
||||
# 4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640）
|
||||
# 5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control]
|
||||
# 6) 换完确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines
|
||||
# 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入）
|
||||
yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx"
|
||||
# 备选模型：直接换成 models/yolo/ 下的其他 .onnx / .pt 绝对路径即可。
|
||||
# 换完建议确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines
|
||||
# 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用上面的工件权重。
|
||||
# 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
|
||||
# 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
|
||||
# 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
|
||||
# 原先指向 hole_insert/best.pt —— 那个文件不存在（悬空引用）。这里改成实际存在的
|
||||
# detect_best_1.onnx，这样 backend: yolo 至少是能跑的。查悬空引用：
|
||||
# bash ~/ros2/scripts/weights.sh doctor
|
||||
hole_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||||
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
|
||||
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
|
||||
@@ -339,6 +343,7 @@ gripper:
|
||||
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
|
||||
hole_detect:
|
||||
backend: "two_stage" # two_stage | yolo | hough
|
||||
# backend=yolo 时用。原为 hole_insert/best.pt（文件不存在），已改为实际存在的模型。
|
||||
weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||||
# 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔）
|
||||
workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||||
@@ -491,7 +496,7 @@ sim:
|
||||
# 决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。
|
||||
# 写了个不同的值会告警。要真改，三处必须一起改。
|
||||
#
|
||||
# 另外注意：`run_sim.sh` 走的是 mock 驱动（假数据 + RViz），**不是**物理仿真；
|
||||
# 另外注意：`run_sim.sh` 走的仍是 mock 驱动（假数据 + RViz），不是物理仿真；
|
||||
# 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。
|
||||
platform: "gazebo"
|
||||
world: "devices/sim/gazebo/worlds/assembly.world"
|
||||
|
||||
+426
-110
@@ -1,148 +1,464 @@
|
||||
# devices/sim/gazebo/ —— Gazebo 仿真平台
|
||||
# devices/sim/gazebo/ —— Gazebo 仿真平台（中文手册）
|
||||
|
||||
> 这份文档记录的是**已经跑通的事实**和**实测踩到的坑**。仿真里绝大多数错误都是
|
||||
> 静默的（不报错、能出图、能查到 TF，结果却是错的），所以下面每条坑都写了
|
||||
> **怎么验**。改仿真之前先把"坑"那一节看完。
|
||||
> **这份文档是仿真的"使用说明书 + 建模规范 + 避坑清单"。**
|
||||
>
|
||||
> 读法建议：
|
||||
> - 只想**跑起来** → 看 [第 1 节 使用方法](#1-使用方法)。
|
||||
> - 要**加一个工件/夹具/机器人** → 看 [第 2 节 建模要求](#2-建模要求) 与 [第 3 节 导入格式](#3-导入格式)。
|
||||
> - 要**改参数** → 看 [第 4 节 配置方式](#4-配置方式)（有一张"改哪里才真的生效"的表，**先看它**）。
|
||||
> - **改仿真之前必须看完** [第 5 节 实测踩到的坑](#5-实测踩到的坑) —— 仿真里绝大多数错误都是**静默**的：
|
||||
> 不报错、能出图、能查到 TF，结果却是错的。每条坑都写了"怎么发现"。
|
||||
>
|
||||
> 本文档只记录**已经跑通的事实**。没接线的东西会明确标 ✘，不要凭印象改成 ✔。
|
||||
|
||||
---
|
||||
|
||||
## 先读这段：哪个"仿真"是真的
|
||||
## 0. 先读这段：哪个"仿真"是真的
|
||||
|
||||
仓库里有两个都叫"仿真"的东西，很容易混：
|
||||
|
||||
| 入口 | 底层 | 有物理吗 | 用途 |
|
||||
|------|------|----------|------|
|
||||
| `scripts/run_sim.sh` | `arm_bringup` 的 **mock 驱动**（`mock_arm_driver.py`） | ✘ 假数据 + RViz | 验证话题/流程/UI，不耗资源 |
|
||||
| `scripts/run_sim_gz.sh` | **Gazebo Fortress**（真物理、真接触） | ✔ | 验证接触/力控/几何 |
|
||||
| `scripts/run_sim_gz.sh` | **Gazebo Fortress**（真物理、真接触） | ✔ | 验证接触/力控/几何、装配状态机 |
|
||||
|
||||
**要验证装配的接触与力判据，只能用后者。** 前者永远不会产生接触力。
|
||||
|
||||
判断当前在跑哪个：`ros2 topic echo /clock --once` —— 有 `/clock` 且时间在走，是 Gazebo。
|
||||
|
||||
---
|
||||
|
||||
## 第一部分：现状（截至 P5c-4，2026-09-23）
|
||||
## 1. 使用方法
|
||||
|
||||
### 已就绪
|
||||
### 1.1 装环境（只做一次）
|
||||
|
||||
```bash
|
||||
bash scripts/install_sim_env.sh # 建独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
|
||||
bash scripts/build_sim.sh # 编仿真用的包（--merge-install）
|
||||
```
|
||||
|
||||
**环境是独立的一份**，与真机环境 `ros_flexiv` 隔离。理由与踩坑见
|
||||
[坑 3](#坑-3conda-环境里-gz-插件路径缺-conda_prefixlib) 和 `scripts/install_sim_env.sh` 顶部的长注释。
|
||||
|
||||
### 1.2 启动
|
||||
|
||||
```bash
|
||||
bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个）
|
||||
bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI
|
||||
bash scripts/run_sim_gz.sh --check # 只自检描述，不启动任何东西
|
||||
bash scripts/run_sim_gz.sh --world /abs/other.world # 换 world
|
||||
bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源，只验运动学
|
||||
```
|
||||
|
||||
首次启动约 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。启动日志末尾会打印一份
|
||||
**验收命令清单**，照它做一遍就能确认仿真真的活了。
|
||||
|
||||
### 1.3 验收（★ 必做，别只看"有没有图像"）
|
||||
|
||||
```bash
|
||||
ros2 topic echo /clock --once # 仿真时间在走
|
||||
ros2 control list_controllers # 两个控制器 active
|
||||
ros2 topic echo /joint_states --once # 7 个 sim_joint
|
||||
ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查
|
||||
ros2 topic echo /tactile/left/status --once # 触觉三力在发
|
||||
ign model -m workpiece -p # 工件在台面上
|
||||
python3 scripts/check_sim_cameras.py # ★ 相机的真正验收
|
||||
python3 scripts/check_sim_scene.py # ★ 场景摆放 + 孔检测几何验收
|
||||
```
|
||||
|
||||
机械臂应当**在重力下保持站住**。若它瘫下去或消失，说明描述有问题 ——
|
||||
见 [坑 1](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)。
|
||||
|
||||
**两条 `check_*.py` 才是验收标准，"有图像"证明不了什么。**
|
||||
它们做的判断分别是：
|
||||
|
||||
| 工具 | 它凭什么判"通过" | 它防的是哪一类错 |
|
||||
|------|------------------|------------------|
|
||||
| `check_sim_cameras.py` | 把深度**反投影**到 world，要求命中已知平面（地面 z=0、台面 z=0.40） | 深度单位差 1000 倍、`frame_id` 盖成本体系、内参是 gz 的错值、相机位姿朝向反了 —— 全都**能出图** |
|
||||
| `check_sim_scene.py` | 摆放自洽 + 像素门槛与真机一致 + 孔检测特征的像素值达标 + 手外相机**完整**看得见工件 | 物体悬空/穿模/嵌入、场景里的门槛与真机漂移、成像尺度不够、粗定位框不全 |
|
||||
|
||||
### 1.4 停止
|
||||
|
||||
```bash
|
||||
bash scripts/stop_sim_gz.sh # 稳妥地停掉整套仿真
|
||||
```
|
||||
|
||||
**不要用 `pkill -f gz`**：它会把调用它的那个 shell 一起杀掉（`-f` 匹配命令行，
|
||||
而 `pkill` 自己的命令行里就含 `gz`），也会误杀同名的其他进程。`stop_sim_gz.sh`
|
||||
按进程树精确识别，排除了自己和自己的祖先进程。
|
||||
|
||||
---
|
||||
|
||||
## 2. 建模要求
|
||||
|
||||
### 2.1 三条通用规矩（违反哪一条都会"看起来正常"）
|
||||
|
||||
| 规矩 | 为什么 | 违反了会怎样 |
|
||||
|------|--------|--------------|
|
||||
| **单位必须是米** | Gazebo 没有单位系统，SDF 里的数字就是米 | 用毫米建的 mesh 会变成 1000 倍大的怪物，或者小到看不见 |
|
||||
| **模型原点放在底面中心** | 放置时 `pose.z` 就等于"放在台面上的高度" | 原点在质心时物体一半埋进台面，或者悬空；`sim_scene` 会校验并报错 |
|
||||
| **`<model name>` 必须与推理用的类别名一致** | 仿真里算出的坐标要与 YOLO 结果对齐 | 视觉算出的类别名与场景里的名字对不上，坐标静默错位 |
|
||||
|
||||
`sim_scene.validate_scene()` 会在启动时把上面第 1、2 条自动查一遍
|
||||
（模型原点、是否穿台面/悬空/出界/互相嵌入），有问题的物体**不会**被 spawn 出去。
|
||||
|
||||
### 2.2 质量与惯性（`<inertial>`）
|
||||
|
||||
**`<inertial>` 不是可选的。** 缺了它 Gazebo 会给一个默认值，物体表现得像另一个东西；
|
||||
更糟的是惯性为 0 或 NaN 时物体直接消失（见 [坑 1](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)）。
|
||||
|
||||
- 质量与真实值同量级即可，不必精确。装配里要紧的是**质量比**：
|
||||
工件太重会让夹爪"压进去"的读数异常大（见 `config/tactile.yaml` 的复测记录）。
|
||||
- `inertia` 用简化公式算（薄板、圆柱的公式够用），只要量级对。
|
||||
- 参考 `models/workpiece/model.sdf`（铝板：`mass=0.08`）与 `models/gauge/model.sdf`（钢规：`mass=0.02`）。
|
||||
|
||||
### 2.3 `<collision>` 与 `<visual>` 的分工
|
||||
|
||||
| | 作用 | 建模要求 |
|
||||
|---|------|----------|
|
||||
| `<collision>` | 决定物理：穿不穿、卡不卡、接触力多大 | 用**简单几何体**（box/cylinder），不要用 mesh —— mesh 碰撞在接触力计算上又慢又不稳 |
|
||||
| `<visual>` | 决定"相机看见什么" | 可以加入**只为人眼/算法准备**的细节（亮环、色块），它们不参与物理 |
|
||||
|
||||
**这两个可以不一样，而且这个框架有意让它们不一样**：
|
||||
|
||||
`models/workpiece/model.sdf` 的 collision 是"一块带三个方孔的平板"，
|
||||
而 visual 额外加了两个**亮环**（φ14 外 / φ7 内）来模拟真机上的压入衬套 ——
|
||||
因为真机的 HoughCircles 就是靠这两个亮环找孔的。若只按 collision 建视觉，
|
||||
仿真里"孔检测"会因为看不见环而永远失败，而这不是算法问题。
|
||||
|
||||
> ⚠️ 由此带来一条约定：**visual 的名字有意义**，`sim_scene.features_from_workpiece`
|
||||
> 按名字前缀（`insert_*_ring` / `center_bore`）去读出特征尺寸，用来算像素值。
|
||||
> 改 visual 的名字必须同步改那个函数（有单测守着）。
|
||||
|
||||
### 2.4 接触参数（`<surface>`）：只在必要时改，且要写清理由
|
||||
|
||||
默认接触参数（`kp` 很大 = 刚性）对"位置控制 + 刚性夹持"是灾难：
|
||||
夹爪把工件"压进去"的力会到几百牛，工件被弹飞。
|
||||
|
||||
`models/gauge/model.sdf` 因此对规**单独放软**了接触刚度（`kp`/`kd` 比台面软得多），
|
||||
注释里写明了原因。**这是有意为之，不是随手抄的参数** —— 照抄前先想清楚你的物体
|
||||
是不是也要被夹爪夹。
|
||||
|
||||
### 2.5 落地判据：怎么知道模型建对了
|
||||
|
||||
```bash
|
||||
ign sdf -p models/<名>/model.sdf # 先过语法
|
||||
python3 scripts/check_sim_scene.py # 再过摆放与成像
|
||||
```
|
||||
|
||||
两条都过了，才认为"模型建好了"。**不要以"Gazebo 里能看到它"为判据。**
|
||||
|
||||
---
|
||||
|
||||
## 3. 导入格式
|
||||
|
||||
### 3.1 物料放置总表
|
||||
|
||||
| 类型 | 放哪 | 格式 | 命名要求 | 谁读它 |
|
||||
|------|------|------|----------|--------|
|
||||
| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world` | `site.yaml` → `sim.world` |
|
||||
| 机器人描述 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） | `gazebo_sim_launcher` |
|
||||
| 场景物体 | `models/<模型名>/` | `<模型名>.sdf` + `<模型名>.config` | 目录名 = `<model name>` = 推理类别名 | `config/scene.yaml` → 启动时 spawn |
|
||||
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米** | 被 SDF 的 `<mesh><uri>` 引用 |
|
||||
| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用的 `camera_name` | 桥接 + 适配器 + 静态 TF |
|
||||
| 场景布局 | `config/scene.yaml` | YAML | — | 启动时 spawn + 像素门槛校验 |
|
||||
| 触觉标定 | `config/tactile.yaml` | YAML | 话题名必须与 URDF 里的 `<topic>` 一致 | `sim_tactile_adapter` |
|
||||
| 控制器 | `config/controllers.yaml` | YAML | 控制器名与真机一致 | `ros2_control` |
|
||||
|
||||
> `sensor_plugins/` 目前是**空的**（只有 `.gitkeep`）。触觉走的是 Gazebo 内置的
|
||||
> `force_torque` + `contact` 传感器 + 本框架的适配器，不需要自己写 C++ 插件。
|
||||
> 那个目录留给"以后真要写插件"的场景。
|
||||
|
||||
### 3.2 一个模型目录长什么样
|
||||
|
||||
```
|
||||
models/workpiece/
|
||||
├── model.sdf # 几何 + 惯性 + 接触参数
|
||||
└── model.config # 元数据：<name>、版本、指向 model.sdf、描述
|
||||
```
|
||||
|
||||
`model.config` 里的 `<name>` **必须等于目录名**，也必须等于 YOLO 的类别名。
|
||||
`models/workpiece/model.config` 里写了这条约定，照它抄。
|
||||
|
||||
### 3.3 加一个场景物体：改 `scene.yaml` 一处即可
|
||||
|
||||
物体**不在 world 文件里**，而在 `config/scene.yaml` 的 `objects` 列表里，
|
||||
启动时由 `ros_gz_sim create` 动态 spawn：
|
||||
|
||||
```yaml
|
||||
objects:
|
||||
- name: workpiece # Gazebo 里的实体名（也是 ign model -m 的名字）
|
||||
model: workpiece # models/ 下的目录名
|
||||
pose: {x: 0.52, y: 0.0, z: 0.40, yaw: 0.0} # z = 台面顶面高度（原点在底面）
|
||||
note: 为什么摆这儿 —— 请写清，否则半年后没人敢动
|
||||
```
|
||||
|
||||
这么设计的原因：world 文件是"环境"（地面、台面、重力、固定相机），
|
||||
场景是"这一批活要做什么"（工件在哪、规座在哪）。混在一起会导致
|
||||
"为了挪一下工件而去改 world 文件"，而 world 文件一旦改错（比如丢了
|
||||
UserCommands 系统）会让**所有**模型都 spawn 不进去（见 [坑 2](#坑-2world-里声明了任何-plugin-都会挤掉默认系统)）。
|
||||
|
||||
### 3.4 URDF → Gazebo：中间隔着 `sdformat`，会丢东西
|
||||
|
||||
`flexiv_description` 的 xacro 展开后交给 `sdformat` 转成 SDF。**这个转换是有损的**：
|
||||
|
||||
| 会被丢/被改的 | 后果 | 本框架的应对 |
|
||||
|---------------|------|--------------|
|
||||
| `<mimic>` 标签 | 夹爪的从动指节不再跟随 → 夹爪"驱动了但不动" | `urdf_normalize` 把 mimic 关系**记下来**，改成把全部 7 个夹爪关节都接入 `ros2_control`，由控制器按记录的关系驱动 |
|
||||
| 缺 `<inertial>` 的 link | 惯性 NaN → 物体消失 | 启动时补默认惯性 |
|
||||
| 零质量的 link | 惯性叠加出 NaN | 同上，一律归一化 |
|
||||
| 世界 link（无质量的根 link） | **整棵子树被静默丢掉** | D435 因此必须放在 world 文件里，不能挂在 URDF 的 world link 上 |
|
||||
|
||||
这些都不是"理论上可能"，是实测踩过的。归一化逻辑在
|
||||
`src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py`（37 项单测）。
|
||||
单独排查：`gazebo_sim_launcher --check`。
|
||||
|
||||
---
|
||||
|
||||
## 4. 配置方式
|
||||
|
||||
### 4.1 ★ 改哪里才真的生效（先看这张表）
|
||||
|
||||
| 想改什么 | 改哪里 | 真的生效吗 |
|
||||
|----------|--------|------------|
|
||||
| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 优先级：`--world` > `site.yaml` > 内置默认 |
|
||||
| 场景里有哪些物体、摆在哪 | `config/scene.yaml` | ✔ 启动时 spawn，并做摆放/像素校验 |
|
||||
| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致告警 |
|
||||
| 相机图幅（影响像素尺度） | `config/cameras.yaml` 的 `width`/`height` **且** SDF 的 `image_width/height` | ✔ 启动时自动比对（这是最容易漏的一处，见 4.3） |
|
||||
| 触觉标度/轴向/基线 | `config/tactile.yaml` | ✔ 适配器每次启动都读 |
|
||||
| 装配的悬高/速度 | **复用真机那套**（`servo.*` / `hole_detect.stage2.*`） | ✔ 有意共用 |
|
||||
| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 的 `<gravity>` |
|
||||
| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。改要动三处：`urdf/arm.sim.urdf.xacro` 的 `arm_prefix`、`config/controllers.yaml` 的 `sim_joint1..7`、本项 |
|
||||
|
||||
> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何"仿真专用参数"都是
|
||||
> 未来"仿真里调好的值搬到真机不生效"的隐患。只允许**物理引擎自身的**参数
|
||||
> （重力、步长）放在 `sim.*` 下。
|
||||
>
|
||||
> ✘ 的两个键由 `arm_sim_gazebo.sim_site.unwired_warnings()` 守着：写的值与实际状态
|
||||
> 不一致时启动会**告警**。**不要让它们重回静默。**
|
||||
|
||||
### 4.2 配置文件之间的分工
|
||||
|
||||
```
|
||||
config/site.yaml ← 站点真相：真机有哪些设备、序列号、阈值（仿真与真机共用）
|
||||
│
|
||||
├── devices/sim/gazebo/config/cameras.yaml ← "Gazebo 里有什么相机、桥到哪个 ROS 名字"
|
||||
├── devices/sim/gazebo/config/scene.yaml ← "这一批活：工件/规座摆在哪"（含像素门槛）
|
||||
├── devices/sim/gazebo/config/tactile.yaml ← 仿真的力标定口径与轴向映射
|
||||
└── devices/sim/gazebo/config/controllers.yaml ← 控制器（名字与真机一致）
|
||||
```
|
||||
|
||||
**一条原则：能派的就别抄。** 相机位姿从 world 文件里**解析**（不在 `cameras.yaml` 重复写）、
|
||||
内参从 `horizontal_fov` 与图幅**算出**（不手抄 fx/fy）—— 抄一份就多一处能漂移的地方。
|
||||
|
||||
唯一的例外是 `scene.yaml` 里的 `gates_px`：像素门槛本来只在真机的
|
||||
`hole_detect.yaml`/`site.yaml` 里，但仿真要在**启动时**就判"能不能看见"，
|
||||
所以抄了一份。为了不让它漂移，`check_sim_scene.py` 会读那两个文件做**交叉核对**，
|
||||
不一致直接判失败。**抄一份可以，但必须有人守着。**
|
||||
|
||||
### 4.3 改相机图幅会牵动三处（最常见的漏改）
|
||||
|
||||
```bash
|
||||
# 想改 D405 的分辨率，三处必须同时改：
|
||||
devices/sim/gazebo/config/cameras.yaml # width / height（内参由此算出）
|
||||
devices/sim/gazebo/urdf/arm.sim.sensors.xacro # image_width / image_height（真正渲染的）
|
||||
```
|
||||
|
||||
第三处是**下游**：`hole_detect.yaml` 的像素门槛（`hough_min_radius`、`hole_distance_min`）
|
||||
只在特定像素尺度下才对。仿真用了 640x480（fx=337）而不是真机的 848x480（fx=447）时，
|
||||
58mm 的两孔间距成像只剩 85px，低于 `hole_distance_min=90` ——
|
||||
**孔检测在仿真里根本不可能通过**，而报出来的现象会像是"算法不行"。
|
||||
|
||||
启动时会比对 `cameras.yaml` 与 SDF 的视场**和**图幅，不一致会告警并说明影响。
|
||||
|
||||
---
|
||||
|
||||
## 5. 实测踩到的坑
|
||||
|
||||
这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，但结果是错的。
|
||||
所以每条都写了"怎么发现"。
|
||||
|
||||
### 坑 1：官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉
|
||||
|
||||
7 轴本体缺 `<inertial>` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。
|
||||
**只有 Gazebo 会暴露，RViz/MoveIt 完全正常。**
|
||||
|
||||
→ 启动时归一化（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check`。
|
||||
|
||||
### 坑 2：world 里声明了任何 `<plugin>` 都会挤掉默认系统
|
||||
|
||||
world 里只要有一个 `<plugin>`，Gazebo 就**不再**自动加载默认的
|
||||
Physics / UserCommands / SceneBroadcaster。
|
||||
|
||||
少了 UserCommands 的后果最阴：`/world/<名>/create` 服务不存在 →
|
||||
模型根本没进 Gazebo，而 `create` **仍然打印** `OK creation of entity`，
|
||||
控制器永远起不来。
|
||||
|
||||
→ 这三个系统必须在 world 里显式声明（`assembly.world` 里已有）。启动时会检查并告警。
|
||||
|
||||
### 坑 3：conda 环境里 gz 插件路径缺 `$CONDA_PREFIX/lib`
|
||||
|
||||
`ros_gz_sim` 的 launch 只拼 `LD_LIBRARY_PATH`，而 conda 激活不往那里加。
|
||||
症状是 `gz_ros2_control` 报 `couldn't find shared library`，而库其实就在那儿（`ldd` 全齐）。
|
||||
|
||||
→ 必须在 `ros2 launch` **之前**设 `GZ_SIM_SYSTEM_PLUGIN_PATH`；资源路径同理（否则 mesh 全加载不出来）。两件事都由 `scripts/sim_env.sh` 负责 —— **不要绕过它直接 `ros2 launch`**。
|
||||
|
||||
### 坑 4：gz 发的 `camera_info` 是错的
|
||||
|
||||
恒为 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分辨率主点），
|
||||
而**渲染**用的是配置的 87° 视场与 640x480。
|
||||
|
||||
照抄它做三维反投影，点会偏**半米量级**；而单目预览完全正常。
|
||||
|
||||
→ 用已知地面平面最小二乘反解内参，得 `fy=337.2 / cy=239.5`，与
|
||||
`640/(2·tan(87°/2))=337.2`、`480/2=240` 吻合（残差中位 0.2mm）。
|
||||
已改为由 `sim_cameras.intrinsics()` 按 `cameras.yaml` 的视场**重算并覆盖**。
|
||||
|
||||
代价：`cameras.yaml` 的 `horizontal_fov` 必须与 SDF 真正渲染用的值一致，
|
||||
启动时自动比对告警（`sim_cameras.check_camera_consistency`）。
|
||||
|
||||
### 坑 5：相机位姿的 `rpy` 复合顺序
|
||||
|
||||
URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；而 `rpy` 的复合顺序是 `Rz·Ry·Rx`。
|
||||
"先俯 50° 再偏航 180°"会把视线翻到**背面**。
|
||||
|
||||
D435 就这么翻过：背对台面、只看得见地面。什么都不报，**而且几何自洽**
|
||||
（点确实落在真实地面上），只查数值发现不了。
|
||||
|
||||
→ 用 `scripts/check_sim_cameras.py` 检查"该看见的平面有没有看见"。已修正 + 加回归测试。
|
||||
|
||||
### 坑 6：接触传感器只发"接触"，不发"离开"
|
||||
|
||||
Gazebo 的 contact 是**事件式**的：接触时发消息，离开时**一条都不发**。
|
||||
|
||||
若写成"收到消息=接触 / 没收到=不管"，那么离开工件后 `contact` 会永远停在 `True`。
|
||||
|
||||
→ 判据必须是"最近 `contact_timeout_s` 内收到过消息"（`config/tactile.yaml`）。
|
||||
`config/tactile.yaml` 里对这点有专门说明。
|
||||
|
||||
### 坑 7：`contact-system` 发的 `Contacts` 消息里**没有力**
|
||||
|
||||
`ros_gz_interfaces/msg/Contact` 的 `wrenches` 字段在 Fortress 的默认
|
||||
contact-system 下**永远是空的**（实测 `ign topic -e -t /tactile/left/raw -n 1`，
|
||||
只有几何名与接触点，没有任何力）。
|
||||
|
||||
**这是这套链路里最耗时的一个坑**：消息结构里有 `wrenches` 字段，看起来"应该有"。
|
||||
|
||||
→ 三向力改由 `<sensor type="force_torque">` 提供（挂在指尖关节上，读该关节传递的约束力）。
|
||||
接触布尔仍用 contact 传感器（它只有这个用途）。
|
||||
|
||||
> 附带一个教训：**光看消息定义判断不了运行时会填什么**，必须 `ign topic -e` 实测。
|
||||
|
||||
### 坑 8：`forcetorque-system` 也必须显式声明
|
||||
|
||||
与坑 2 同源。只声明了 `contact-system` 时，`force_torque` 传感器**话题根本不出现**，
|
||||
而 URDF 里写得好好的。
|
||||
|
||||
→ `assembly.world` 里显式声明 `libignition-gazebo6-forcetorque-system.so`。
|
||||
|
||||
### 坑 9：`sdformat` 用**占位值**填它不认识/缺失的东西
|
||||
|
||||
这是坑 1 的"隐藏变体"，也是**验收检查本身**最容易失效的地方：
|
||||
|
||||
| 你写的 | `sdformat` 的输出 |
|
||||
|--------|-------------------|
|
||||
| 有 `<contact>` 但没 `<collision>` | `__default__` |
|
||||
| 有 `<contact>` 但没 `<topic>` | `__default_topic__` |
|
||||
| `<topic>` 写在 `<sensor>` 里而不是 `<contact>` 里 | 话题跑到默认作用域，订阅方永远收不到 |
|
||||
|
||||
于是"检查这个字段是不是空"的代码**永远通过** —— 因为它不是空的，它是个占位字符串。
|
||||
|
||||
→ `robot_description.check_tactile_collisions()` 现在显式判这两个占位值
|
||||
（`__default__` / `__default_topic__`），并检查 `<topic>` 的位置对不对。
|
||||
有负向单测守着（`test_robot_description.py`）。
|
||||
|
||||
> 通用教训：**校验代码自己会静默失效**。凡是"检查 X 是否为空"的地方，
|
||||
> 都要问一句"它会不会被填成一个假值"。
|
||||
|
||||
### 坑 10：`<mimic>` 标签被 `sdformat` 丢掉
|
||||
|
||||
夹爪的从动指节在 URDF 里靠 `<mimic>` 定义跟随关系，而 `sdformat` 转换时**把它丢了**。
|
||||
|
||||
症状是"夹爪控制器 active，但手指不动" —— 控制器在驱动一个不存在的关节。
|
||||
|
||||
→ `urdf_normalize` 在转换前把 mimic 关系**记录成一张表**（`mimic_relations()`），
|
||||
去掉标签，并把全部 7 个夹爪关节都接入 `ros2_control`，由适配器按关系驱动。
|
||||
|
||||
> 顺带：从动关节原本 `effort="0"`，归一化时一并改成可驱动的值，
|
||||
> 否则它在控制器里是个"只能看不能动"的关节。
|
||||
|
||||
### 坑 11：`gz model -m <名> -p` 的输出格式没有稳定契约
|
||||
|
||||
不同版本的文本格式不同。验收脚本因此**宽松解析**，解析不出来时降级成
|
||||
"跳过位姿核对"，而不是报一个假错。
|
||||
|
||||
> 这是有意的取舍：**宁可漏报，不可误报**。误报会让人不再相信工具。
|
||||
|
||||
---
|
||||
|
||||
## 6. 现状与进度
|
||||
|
||||
### 已就绪（端到端验证过）
|
||||
|
||||
| 部分 | 位置 | 说明 |
|
||||
|------|------|------|
|
||||
| 运行环境 | `scripts/install_sim_env.sh` / `sim_env.sh` | 独立 conda 环境 `ros_gz_sim`（Gazebo Fortress 6.16 + ROS 2 Humble），与真机环境 `ros_flexiv` 隔离 |
|
||||
| 世界 | `worlds/assembly.world` | 地面 + 工作台 + D435 + 通止规标记位 |
|
||||
| 机器人描述 | `urdf/*.xacro` | 复用官方 `flexiv_description`，启动时**归一化**后喂 Gazebo |
|
||||
| 控制器 | `config/controllers.yaml` | 控制器名与真机一致（`rizon_arm_controller` + `joint_state_broadcaster`） |
|
||||
| 启动器 | `src/arm_sim_gazebo` 的 `gazebo_sim_launcher` | 起 Gazebo → 发归一化 URDF → spawn → 拉起控制器 |
|
||||
| 相机 | `config/cameras.yaml` | 两台相机的**唯一真相**：话题、坐标系、视场 |
|
||||
| 运行环境 | `scripts/install_sim_env.sh` / `sim_env.sh` | 独立 conda 环境 `ros_gz_sim`（Gazebo Fortress 6.16 + ROS 2 Humble） |
|
||||
| 世界 | `worlds/assembly.world` | 地面 + 工作台（顶面 z=0.40）+ D435（世界相机） |
|
||||
| 机器人描述 | `urdf/*.xacro` | 复用官方 `flexiv_description`，启动时**归一化** |
|
||||
| 控制器 | `config/controllers.yaml` | 控制器名与真机一致 |
|
||||
| 启动器 | `src/arm_sim_gazebo` 的 `gazebo_sim_launcher` | 起 Gazebo → 发归一化 URDF → spawn → 拉控制器 → spawn 场景 |
|
||||
| 相机 | `config/cameras.yaml` + `sim_camera_adapter` | D405/D435 桥接到 ROS，内参/单位/frame 都已校正 |
|
||||
| 触觉 | `config/tactile.yaml` + `sim_tactile_adapter` | `force_torque` 三向力 + `contact` 布尔 → `arm_msgs/TactileStatus` |
|
||||
| 场景物体 | `models/` + `config/scene.yaml` | 工件（带亮环的孔板）、通止规、规座，可校验摆放 |
|
||||
| 夹爪 | 7 关节接入 `ros2_control` | 实测开合 27→127mm |
|
||||
|
||||
### 还没做
|
||||
|
||||
| 部分 | 缺什么 | 影响 |
|
||||
|------|--------|------|
|
||||
| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 |
|
||||
| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） |
|
||||
|
||||
> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 +
|
||||
> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据，
|
||||
> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到
|
||||
> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。
|
||||
> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。
|
||||
| 视触觉图像 | 仿真的深度图/力矢量图只能是**占位**（空/NaN） | `TactileStatus` 的图字段在仿真里不可信；三力是可信的 |
|
||||
| 夹具/料框 | 目前只有工件、规、规座 | 上料流程（若有）没法验 |
|
||||
| 夹爪服务适配器 | `~/open|~/close|~/move` 服务（mm ↔ 弧度换算） | 仿真里目前只能直接发关节指令开合，无法走真机那套服务接口 |
|
||||
|
||||
---
|
||||
|
||||
## 第二部分：日常怎么用
|
||||
## 7. 有意保留的规格差异（不是 bug，但会让人困惑）
|
||||
|
||||
### 启动
|
||||
|
||||
```bash
|
||||
bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个）
|
||||
bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI
|
||||
bash scripts/run_sim_gz.sh --check # 只自检描述，不启动任何东西
|
||||
bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源，只验运动学
|
||||
```
|
||||
|
||||
首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。
|
||||
|
||||
### 验收（★ 必做，别只看"有没有图像"）
|
||||
|
||||
```bash
|
||||
ros2 topic echo /clock --once # 仿真时间在走
|
||||
ros2 control list_controllers # 两个控制器 active
|
||||
ros2 topic echo /joint_states --once # 7 个 sim_joint
|
||||
python3 scripts/check_sim_cameras.py # ★ 相机的真正验收
|
||||
```
|
||||
|
||||
**`check_sim_cameras.py` 才是相机的验收标准。** 它把深度反投影到 world，
|
||||
检查落点是否命中已知平面（地面 z=0、台面 z=0.40）。"有图像"证明不了什么 ——
|
||||
详见下面的坑 4 与坑 5。
|
||||
|
||||
### 放置格式与命名要求
|
||||
|
||||
| 类型 | 放哪 | 格式 | 命名要求 |
|
||||
|------|------|------|----------|
|
||||
| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` |
|
||||
| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） |
|
||||
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
|
||||
| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
|
||||
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米**。用毫米建模的必须先缩放到米 |
|
||||
| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用启动时的 `camera_name` |
|
||||
|
||||
### 改配置：改哪里才会真正生效
|
||||
|
||||
| 想改什么 | 改哪里 | 真的生效吗 |
|
||||
|----------|--------|------------|
|
||||
| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 已接线（优先级：`--world` > `site.yaml` > 内置默认） |
|
||||
| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致会告警 |
|
||||
| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 里的 `<gravity>` |
|
||||
| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。前缀由 `urdf/arm.sim.urdf.xacro` 的 `arm_prefix` 决定，且 `config/controllers.yaml` 硬编码了 `sim_joint1..7` —— 三处要一起改 |
|
||||
| 仿真里的悬高/速度 | **复用真机那套参数**（`servo.*` / `hole_detect.stage2.*`） | ✔ |
|
||||
|
||||
> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何"仿真专用参数"都是
|
||||
> 未来"仿真调好的值搬到真机不生效"的隐患。只允许物理引擎自身的参数（重力、步长）
|
||||
> 放在 `sim.*` 下。
|
||||
>
|
||||
> ✘ 标记的两个键由 `arm_sim_gazebo.sim_site.unwired_warnings()` 守着：只要你写的值
|
||||
> 和实际状态不一致，启动时就会告警。**不要让它们重回静默。**
|
||||
| 项 | 真机 | 仿真 | 后果 |
|
||||
|----|------|------|------|
|
||||
| 镜头畸变 | 有（`d` 非 0） | **无**（`d` 全 0、`R` 单位阵） | "仿真里用畸变矫正调出来的参数"搬到真机仍需重调 |
|
||||
| D435 图幅 | 1280x720 | 640x480 | 像素尺度不同，`hole_detect` 的像素门槛搬过去要重算 |
|
||||
| 触觉单位 | Orisys 形变任意单位（非牛顿） | 牛顿 × `force_scale` | 见 `config/tactile.yaml` 头部的"单位口径"；选错**不报错**，只是阈值不触发或一碰就炸 |
|
||||
| 触觉图像 | 有真实深度图/力矢量图 | 占位 | 下游不用改代码，但图内容不可信 |
|
||||
|
||||
---
|
||||
|
||||
## 第三部分：实测踩到的坑（都属于"静默失效"）
|
||||
|
||||
这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，
|
||||
但结果是错的。所以每条都配了"怎么发现"。
|
||||
|
||||
| # | 坑 | 症状 | 怎么发现 / 怎么修 |
|
||||
|---|----|------|-------------------|
|
||||
| 1 | 官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉：7 轴本体缺 `<inertial>` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN | 只有 Gazebo 拒绝，RViz/MoveIt 完全正常 | 启动时**归一化**（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check` |
|
||||
| 2 | world 里只要声明了**任何一个** `<plugin>`，就会挤掉默认的 Physics / UserCommands / SceneBroadcaster | 少了 UserCommands → `/world/<名>/create` 服务不存在 → 模型根本没进 Gazebo，而 `create` 仍打印 `OK creation of entity`；控制器永远起不来 | 启动时会检查并告警。修法：在 world 里显式声明这三个系统 |
|
||||
| 3 | gz 的插件搜索路径不含 `$CONDA_PREFIX/lib`（`ros_gz_sim` 的 launch 只拼 `LD_LIBRARY_PATH`，而 conda 激活不往那里加） | `gz_ros2_control` 报 `couldn't find shared library`，而库其实就在那儿（`ldd` 全齐） | 必须在 `ros2 launch` **之前**设 `GZ_SIM_SYSTEM_PLUGIN_PATH`；资源路径同理（否则 mesh 全加载不出来）。都由 `scripts/sim_env.sh` 负责 |
|
||||
| 4 | **gz 发的 `camera_info` 是错的**：恒为 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分辨率主点），而**渲染**用的是配置的 87° 视场 | 照抄它做三维反投影，点会偏**半米量级**；单目预览完全正常 | 用已知地面平面做最小二乘反解内参，得到 `fy=337.2 / cy=239.5`，与 `640/(2·tan(87°/2))=337.2`、`480/2=240` 吻合（残差中位 0.2mm）。已改为由 `sim_cameras.intrinsics()` 按 `cameras.yaml` 的视场**重算并覆盖** |
|
||||
| 5 | **相机位姿的 `rpy` 顺序**：URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；而 `rpy` 复合顺序是 `Rz·Ry·Rx`，"先俯 50° 再偏航 180°"会把视线翻到**背面** | D435 背对台面、只看得见地面。什么都不报，**而且几何自洽**（点确实落在真实地面上），只查数值发现不了 | 用 `scripts/check_sim_cameras.py` 检查"该看见的平面有没有看见"。已修正 + 加回归测试 |
|
||||
|
||||
关于坑 4：代价是 `cameras.yaml` 的 `horizontal_fov` 必须与 SDF 真正渲染用的值一致。
|
||||
启动时会自动比对并告警（`sim_cameras.check_fov_consistency`）。
|
||||
|
||||
关于相机还有两条**规格差异**，是有意保留的（不是 bug，但会让人困惑）：
|
||||
|
||||
- **畸变**：gz 不模拟镜头畸变（`d` 全 0，`R` 单位阵）。所以"仿真里用畸变矫正
|
||||
调出来的参数"搬到真机仍需重调。
|
||||
- **分辨率**：真机 D405 是 848x480、D435 是 1280x720，仿真建模用 640x480。
|
||||
对"算法能不能跑通"没影响（内参从 `cameras.yaml` 自洽推导），
|
||||
但"仿真里调好的像素阈值搬到真机"会因像素尺度不同而需要重调。
|
||||
|
||||
---
|
||||
|
||||
## 相关文件一览
|
||||
## 8. 文件一览
|
||||
|
||||
```
|
||||
devices/sim/gazebo/
|
||||
├── manifest.yaml # 设备清单：进度、配置声明、放置要求
|
||||
├── manifest.yaml # 设备清单：进度、配置声明、放置要求（与本文档同源）
|
||||
├── README.md # 本文档
|
||||
├── config/
|
||||
│ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场）
|
||||
│ └── controllers.yaml # 控制器配置（名字与真机一致）
|
||||
├── urdf/ # 仿真用机器人描述（含 ros2_control 与传感器）
|
||||
├── worlds/assembly.world # 场景
|
||||
├── models/ # ✘ 待做：工件/通止规/夹具
|
||||
└── sensor_plugins/ # ✘ 待做：Orisys 触觉插件
|
||||
│ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场/图幅）
|
||||
│ ├── controllers.yaml # 控制器配置（名字与真机一致）
|
||||
│ ├── scene.yaml # ★ 场景布局（工件/规/规座）+ 像素门槛
|
||||
│ └── tactile.yaml # ★ 触觉标定：单位口径、轴向映射（实测）、基线策略
|
||||
├── urdf/
|
||||
│ ├── arm.sim.urdf.xacro # 顶层：拼装 + 前缀
|
||||
│ ├── arm.sim.gazebo.xacro # Gazebo 扩展（接触/摩擦参数）
|
||||
│ ├── arm.sim.ros2_control.xacro# 控制器接口（7 轴 + 7 个夹爪关节）
|
||||
│ └── arm.sim.sensors.xacro # D405 + 触觉传感器（contact + force_torque）
|
||||
├── worlds/assembly.world # 环境：地面 + 台面 + D435 + 显式声明的系统插件
|
||||
├── models/
|
||||
│ ├── workpiece/ # 工件：带三个孔的铝板 + 两个检测用亮环
|
||||
│ ├── gauge/ # 通止规：通端/套环/握柄/止端，接触参数刻意放软
|
||||
│ └── gauge_stand/ # 规座：static，把规立住
|
||||
└── sensor_plugins/ # （空）触觉走内置传感器 + 适配器，不需要自定义插件
|
||||
```
|
||||
|
||||
配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器）。
|
||||
配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/check_sim_cameras.py`（相机验收）。
|
||||
配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器、触觉适配器、场景逻辑）
|
||||
配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/stop_sim_gz.sh`（停止）、
|
||||
`scripts/check_sim_cameras.py`（相机验收）、`scripts/check_sim_scene.py`（场景验收）
|
||||
单测：`src/arm_sim_gazebo/test/`（`test_urdf_normalize` / `test_sim_cameras` /
|
||||
`test_sim_site` / `test_sim_tactile` / `test_robot_description` / `test_check_sim_scene`）
|
||||
|
||||
@@ -36,7 +36,7 @@
|
||||
# 因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。
|
||||
#
|
||||
# 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。
|
||||
# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_fov_consistency）：
|
||||
# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_camera_consistency）：
|
||||
# 腕上相机 → devices/sim/gazebo/urdf/arm.sim.sensors.xacro 的 horizontal_fov
|
||||
# 世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 <sensor> 的 horizontal_fov
|
||||
# =============================================================================
|
||||
@@ -59,7 +59,13 @@ cameras:
|
||||
# 仿真的传感器也按这个建，这样两边的像素尺度接近。
|
||||
# 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。
|
||||
horizontal_fov: 1.5184
|
||||
width: 640
|
||||
# 图像尺寸必须与真机一致：真机 D405 跑 848x480@30（site.yaml 的 camera.width/height）。
|
||||
# 这**不是**显示偏好 —— 阶段二的孔检测在原始分辨率图上跑 HoughCircles，
|
||||
# hole_detect.yaml 的像素门槛（hough_min_radius / hole_distance_min）只在这个
|
||||
# 尺度下才对。曾经用 640x480（fx 337 而非真机 447），58mm 的两孔间距
|
||||
# 成像只剩 85px，低于 hole_distance_min=90 → 孔检测在仿真里根本不可能通过。
|
||||
# 启动时会比对这里与 xacro 宏默认值，不一致会告警（sim_cameras）。
|
||||
width: 848
|
||||
height: 480
|
||||
note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供
|
||||
|
||||
|
||||
@@ -0,0 +1,146 @@
|
||||
# =============================================================================
|
||||
# devices/sim/gazebo/config/scene.yaml
|
||||
#
|
||||
# 仿真场景：**谁摆在台面上的哪里**。
|
||||
#
|
||||
# 为什么单独一份文件，而不是把位姿写进模型或 world：
|
||||
# 模型描述"这个东西长什么样"，场景描述"它现在在哪"。
|
||||
# 分开之后，同一个模型可以在场景里出现多次、可以在不改模型的情况下换位置，
|
||||
# 也才能在真机现场对照着调整（"现场工件是摆在偏左还是偏右？"）。
|
||||
# world 文件里只放**固定不动**的东西（地面、台子、眼在手外的相机）。
|
||||
#
|
||||
# ⚠️ 位姿是**绝对**坐标（世界系、米），不是相对台面的偏移。
|
||||
# 仿真世界没有"台面坐标系"这种东西，写相对量只会多一层换算、多一个出错点。
|
||||
# 下面的 z 值都是"物体的原点"。本框架约定：模型的局部原点 = 它**底面的中心**
|
||||
# （工件的板底、通止规的规尖），所以"放在台面上"就是 z = 台面高度。
|
||||
# 这条约定让场景文件读起来最直观，代价是建模时必须把原点设在底部 ——
|
||||
# 用 SolidWorks/Blender 导出网格前先把原点挪到底面中心。
|
||||
# =============================================================================
|
||||
|
||||
# 场景模型所在目录（相对仓库根）。每个子目录一个模型：<名字>/model.sdf + model.config
|
||||
models_dir: devices/sim/gazebo/models
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 台面几何 —— 用于校验"模型确实摆在台面上、没悬空也没穿模"
|
||||
# ---------------------------------------------------------------------------
|
||||
# 这些值必须与 worlds/assembly.world 里的 work_table 一致。
|
||||
# 不一致时脚本会报错（scripts/check_sim_scene.py），而不是让你在现场猜。
|
||||
# 台面：pose(0.55,0,0) + link top 在 z=0.375 + box 厚 0.05 → 顶面 0.40
|
||||
# box 尺寸 0.8(x) × 1.2(y)
|
||||
table_top_z: 0.40
|
||||
table_bounds:
|
||||
x: [0.15, 0.95]
|
||||
y: [-0.60, 0.60]
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 作业条件 —— 校验"仿真里的成像几何与真机一致"
|
||||
# ---------------------------------------------------------------------------
|
||||
# 二阶段的孔检测是**像素门槛**（hough_min_radius / hole_distance_min …），
|
||||
# 它们只在"相机到工件表面距离"合适时才有意义。所以这个距离必须在仿真里
|
||||
# 显式复现，不能靠"看起来差不多"。
|
||||
#
|
||||
# ⚠️ 真机与仿真的**悬停高度不一样**，别照抄：
|
||||
# 真机 site.yaml：robots[].servo.hole_hover_z_m = 0.23（法兰相对基座）
|
||||
# 仿真需要：约 0.56（法兰相对世界，算下来见下）
|
||||
# 原因是两者的"基座与台面的相对关系"不同：真机是机器人装在台面同一高度上
|
||||
# （法兰比台面高 23cm 就等于离工件 23cm），而仿真里机械臂站在地面、
|
||||
# 台面在 0.40m 高。**相机到工件的距离**才是决定成像的东西，绝对高度不是。
|
||||
# 这个差异目前是**已知且未解决**的（要让两套参数完全通用，得把仿真的台面
|
||||
# 降到与真机一致，那会连带改 D435 的位姿和相机验收基线）——
|
||||
# 详见 README 的"仿真 ≠ 真机"一节，那里写了决策点与影响面。
|
||||
operating:
|
||||
# 用哪台相机做孔检测（决定用哪组内参算像素尺度）
|
||||
camera: d405
|
||||
# 哪个物体是"工件" —— 相机到工件的距离以**它的上表面**为基准。
|
||||
# 这里写的是物体的 name（不是模型名）。
|
||||
part_object: workpiece
|
||||
# 相机光心到工件**上表面**的距离 [m]。
|
||||
# 这个值不是量出来的，是**两条约束夹出来的**：
|
||||
#
|
||||
# (a) 像素门槛（来自 hole_detect.yaml，见下面 gates_px）
|
||||
# 亮环 φ14（半径 7mm）要落在 10~30px → 距离 ∈ [0.104, 0.313]
|
||||
# 两孔间距 58mm 要 ≥ 90px → 距离 ≤ 0.288
|
||||
# 取中段 ⇒ 约 0.17~0.25 都行。
|
||||
#
|
||||
# (b) 机械臂够得着（site.yaml 的 servo.workspace_xyz_max 是 z=0.65）
|
||||
# 仿真里法兰高度 = 工件上表面(0.412) + 本值 + 相机伸出量(0.05)
|
||||
# 本值 0.17 → 法兰 0.632，离上界还有 18mm 余量。
|
||||
# 本值 0.22 → 法兰 0.682，**超出工作空间**，二阶段会直接拒绝执行。
|
||||
#
|
||||
# 两条约束交出来的取值，恰好也解释了真机的数：
|
||||
# 真机 hole_hover_z_m=0.23（法兰相对基座）、相机比法兰靠下 0.05、
|
||||
# 台面与基座同一水平面、板上表面 0.012 ⇒ 相机到工件 = 0.23-0.05-0.012
|
||||
# = 0.168 ≈ 0.17。所以 0.17 不是凑出来的，而是与真机的悬停高度自洽的。
|
||||
camera_to_part_m: 0.17
|
||||
# 相机相对法兰沿工具轴的伸出量 [m]。arm.sim.sensors.xacro 里 d405_z=0.05。
|
||||
# 竖直朝下装配时工具轴朝下，所以相机比法兰更靠近工件这么多。
|
||||
camera_offset_below_flange_m: 0.05
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 孔检测的像素门槛 —— 用于在**启动时**就算出"仿真里这些特征能否被检测到"
|
||||
# ---------------------------------------------------------------------------
|
||||
# ⚠️ 这里的值必须与真机的检测配置一致：
|
||||
# src/arm_application/config/hole_detect.yaml
|
||||
# config/site.yaml 里 hole_detect.* 对它的覆盖（site.yaml 优先）
|
||||
# 抄一份的原因：那些门槛是"像素"量，只有在仿真侧算出像素值才能提前发现
|
||||
# "孔间距不够/环太小"，不必等上真机。代价是两份数字可能漂移 ——
|
||||
# 所以 scripts/check_sim_scene.py 会读那两个文件做**交叉核对**，
|
||||
# 不一致直接报错。改门槛时改真机那份，然后同步到这里（脚本会提醒）。
|
||||
#
|
||||
# 键名必须与模型里 visual 的名字约定对应（见 sim_scene.features_from_workpiece）：
|
||||
# insert_ring_radius 嵌件亮环半径（模型里 insert_*_ring 的 radius）
|
||||
# hole_spacing 两孔中心距（两个 insert_*_ring 的 pose 距离）
|
||||
#
|
||||
# 半径区间取 [6, 30]：这是 config/site.yaml **实际生效**的值
|
||||
# （site.yaml 覆盖了包内 hole_detect.yaml 的 10/30；生效规则是 site 优先，
|
||||
# 见 hole_detector.config_from_site）。脚本会读那两个文件做交叉核对，
|
||||
# 不一致直接失败 —— 所以这里永远等于真机在用的门槛，不需要人工同步。
|
||||
gates_px:
|
||||
insert_ring_radius: [6, 30]
|
||||
hole_spacing: [90, 700]
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 场景物体
|
||||
# ---------------------------------------------------------------------------
|
||||
# name = 进 Gazebo 后的实体名（也是 `ign model -m <name>` 用的名字）。
|
||||
# model = models_dir 下的子目录名 / model.config 里的 <name>。
|
||||
# yaw = 绕世界 Z 的旋转 [rad]。工件的 yaw 决定两个孔的连线方向，
|
||||
# 而两孔连线方向会直接影响 stage2 的 k1/k2 判定（k1_label: left/right）
|
||||
# —— 换 yaw 后要重新确认哪边是"画面里的左孔"。
|
||||
objects:
|
||||
- name: workpiece
|
||||
model: workpiece
|
||||
# 摆在工作台长度方向的中间偏左（靠近机械臂一侧）：
|
||||
# 太靠外（x 大）→ 手臂要伸直，接近工作空间边缘，柔顺下压时余量小
|
||||
# 太靠里（x 小）→ 会被手臂本体挡住 D435 的视线（粗定位就瞎了）
|
||||
# x=0.52 是两者折中；校验脚本会把工件四角反投影到 D435 画面里确认没被裁掉。
|
||||
pose: {x: 0.52, y: 0.00, z: 0.40, yaw: 0.0}
|
||||
note: >
|
||||
工件。z=0.40 = 台面顶面，板底贴台面。
|
||||
工件**上表面**在 0.40+0.012=0.412，二阶段的对孔基准面就是它。
|
||||
|
||||
- name: gauge_stand
|
||||
model: gauge_stand
|
||||
# 与规**同心**：规座就在规的下面，规插在它的方孔里。
|
||||
# 座高 32mm，所以规的 z = 台面 + 32 - 30（通端长 30）= 0.402，
|
||||
# 也就是"台肩正好坐在座顶面上、规尖离台面 2mm"。
|
||||
# 这个 30mm 不是随手写的：它来自 gauge/model.sdf 里通端的长度，
|
||||
# 改模型时必须一起改（改错了的表现是规穿进台面或悬在座里没坐实）。
|
||||
pose: {x: 0.30, y: 0.28, z: 0.40, yaw: 0.0}
|
||||
note: >
|
||||
规座（工装，静态）。**必须在规之前 spawn**（objects 里的顺序就是 spawn 顺序）：
|
||||
否则规先落到台面上、规座再出现在它身体里 —— 两个刚体互相嵌入，
|
||||
求解器会把规弹飞，而现象看起来像"规自己会跳"。
|
||||
|
||||
- name: gauge
|
||||
model: gauge
|
||||
# 立在规座里、工件旁边偏机械臂一侧，方便夹爪从上往下夹住夹持段。
|
||||
# y=0.28 是避开工件与 D435 视线的一侧（工件在 y=0 附近）。
|
||||
# z 见 gauge_stand 的说明：0.402 = 座顶(0.432) - 通端长(0.030)。
|
||||
pose: {x: 0.30, y: 0.28, z: 0.402, yaw: 0.0}
|
||||
note: >
|
||||
通止规。规尖在 z=0.402（原点在规尖），夹持段在 0.435~0.480。
|
||||
本阶段它只是"插在规座里立着"；把它夹起来是 P6 的活（P6 会用
|
||||
GripperExecute 指令夹爪开到 ~35mm、伸到夹持段、再闭合到 ~29mm）。
|
||||
真机上规是**人工放进夹爪**的（P6 里有提示交互），仿真里先立在座上，
|
||||
便于确认夹爪能不能够到、开合范围够不够。
|
||||
@@ -3,7 +3,7 @@
|
||||
#
|
||||
# Gazebo 仿真平台清单。
|
||||
#
|
||||
# 进度（截至 P5c-4，如实记录，不要凭印象改）：
|
||||
# 进度（截至 P5c-6，如实记录，不要凭印象改）：
|
||||
# ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
|
||||
# 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh
|
||||
# ✔ 世界文件 worlds/assembly.world（sdformat 校验通过）
|
||||
@@ -14,8 +14,12 @@
|
||||
# 起 Gazebo → 发归一化 URDF → spawn → 拉起两个控制器（active）
|
||||
# 入口脚本 scripts/run_sim_gz.sh（此前缺失，被 install_sim_env.sh
|
||||
# 与 build_sim.sh 的提示语引用，照做会报文件不存在）
|
||||
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做）
|
||||
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做）
|
||||
# ✔ 场景模型 models/ 已有工件（带检测亮环的孔板）、通止规、规座
|
||||
# 布局在 config/scene.yaml，启动时动态 spawn 并做摆放/几何校验
|
||||
# ✔ 触觉 **不需要自定义插件**：用 Gazebo 内置的 force_torque（三向力）
|
||||
# + contact（接触布尔）传感器，由 sim_tactile_adapter 合成
|
||||
# arm_msgs/TactileStatus。标定与轴向映射在 config/tactile.yaml
|
||||
# （轴向是实测的，不是猜的）。视触觉图像只能是占位。
|
||||
# ✔ 相机出图 D405/D435 已注册并桥接到 ROS，且**几何正确性已验证**
|
||||
# 桥接+适配：config/cameras.yaml → sim_cameras（纯逻辑：
|
||||
# 话题映射 / 深度换算 / 内参，单测覆盖）
|
||||
@@ -24,7 +28,9 @@
|
||||
# 实测：d435 台面 11.9% / 地面 75.3%；d405 台面 37.8% / 地面 54.0%
|
||||
# ✔ 配置接线 sim.world 现在真的从 config/site.yaml 读（此前硬编码，改了无效）
|
||||
#
|
||||
# 所以：D405/D435 可用，触觉还要等 P5c-5，装配状态机（P6）依赖它。
|
||||
# 所以：相机、触觉三力、场景物体都可用，装配状态机（P6）可以在这里验几何与接触。
|
||||
# 仍然做不到的：视触觉的**图像**（深度图/力矢量图只有占位），以及走真机那套
|
||||
# 夹爪服务接口（仿真里目前直接发关节指令）。
|
||||
#
|
||||
# 实测踩到的坑（都属于"静默失效"，后续改仿真务必先看这几条）：
|
||||
# 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
|
||||
@@ -49,6 +55,22 @@
|
||||
# d435 就踩了这个坑：相机背对台面、只看得见地面，却什么都不报，
|
||||
# 而且几何自洽（点确实落在真实地面上），只查数值发现不了。
|
||||
# 已修正并加回归测试（test_robot_description 的 d435 位姿用例）。
|
||||
# 6. 接触传感器的消息是**事件式**的：只在接触时发，离开时不发。写成
|
||||
# "收到=接触/没收到=不管"会让 contact 永远停在 True。判据必须是
|
||||
# "最近 contact_timeout_s 内收到过"。
|
||||
# 7. **contact-system 发的 Contacts 消息里没有力**（wrenches 恒空，实测确认）。
|
||||
# 消息结构里有这个字段，看起来"应该有"—— 这是最耗时的一个坑。三向力改由
|
||||
# force_torque 传感器提供；且它所需的 forcetorque-system 也必须像坑 2 那样
|
||||
# 在 world 里显式声明，否则话题根本不出现。
|
||||
# 8. **sdformat 会用占位值填缺失的东西**：<contact> 缺 <collision> → "__default__"，
|
||||
# 缺 <topic> → "__default_topic__"。于是"检查字段是否为空"的校验代码永远通过。
|
||||
# check_tactile_collisions 已显式判这两个占位值（有负向单测）。
|
||||
# 通用教训：校验代码自己会静默失效。
|
||||
# 9. **<mimic> 标签会被 sdformat 丢掉**：夹爪从动指节不再跟随（控制器 active
|
||||
# 但手指不动）。解法是在归一化时记录 mimic 关系、去掉标签、把全部 7 个夹爪
|
||||
# 关节都接入 ros2_control（从动关节原先 effort=0，也一并修掉）。
|
||||
# 10. `ign model -m <名> -p` 的文本输出没有稳定契约，验收脚本按"宽松解析 +
|
||||
# 解析不出来就跳过"处理 —— 宁可漏报，不可误报。
|
||||
# =============================================================================
|
||||
|
||||
id: gazebo
|
||||
@@ -63,11 +85,14 @@ description: >
|
||||
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
|
||||
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
|
||||
D405/D435 已桥接到 ROS，且几何正确性有自动验收（scripts/check_sim_cameras.py）。
|
||||
场景模型（工件/通止规）与 Orisys 触觉插件尚未实现；
|
||||
场景物体（工件/通止规/规座）在 devices/sim/gazebo/models/，布局在 config/scene.yaml，
|
||||
摆放与成像几何有自动验收（scripts/check_sim_scene.py）。
|
||||
触觉用 Gazebo 内置的 force_torque + contact 传感器，由 sim_tactile_adapter
|
||||
合成 arm_msgs/TactileStatus；轴向映射是实测的（config/tactile.yaml）。
|
||||
视触觉的**图像**（深度图/力矢量图）在仿真里只能是占位，不可信。
|
||||
sim.world 已真正从 config/site.yaml 读取（physics_enable / joint_prefix 尚未接线，
|
||||
写了不一致的值会告警而非静默忽略，见文件头 config_schema）。
|
||||
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
|
||||
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
|
||||
手册见 devices/sim/gazebo/README.md（含建模要求、导入格式、配置方式、避坑清单）。
|
||||
|
||||
# --- 物料放置要求 ----------------------------------------------------------
|
||||
assets:
|
||||
@@ -81,11 +106,18 @@ assets:
|
||||
- path: models
|
||||
required: false
|
||||
what: 工件 / 夹具 / 通止规模型
|
||||
format: 每个模型一个子目录，内含 <模型名>.sdf 与 meshes/
|
||||
format: >
|
||||
每个模型一个子目录，内含 model.sdf 与 model.config（两个文件名都是固定的，
|
||||
Gazebo 按这个约定查找）。原点必须在**底面中心**，这样 scene.yaml 里的
|
||||
pose.z 就等于"放在台面上的高度"。碰撞体用 box/cylinder 等简单几何，
|
||||
视觉可以额外加只服务于算法的细节（如工件上的检测亮环）。
|
||||
naming: >
|
||||
模型名必须与推理用的类别名一致（如 workpiece），否则仿真里算出的坐标
|
||||
与推理结果对不上。网格单位必须是米，用毫米建模的必须先缩放。
|
||||
where_used: Gazebo 场景加载
|
||||
目录名 = SDF 里的 <model name> = model.config 里的 <name> = 推理用的类别名
|
||||
（如 workpiece），四者必须一致，否则仿真里算出的坐标与推理结果对不上。
|
||||
网格单位必须是米，用毫米建模的必须先缩放。
|
||||
where_used: >
|
||||
由 config/scene.yaml 的 objects[].model 引用，启动时动态 spawn；
|
||||
摆放合法性与成像几何由 scripts/check_sim_scene.py 验收。
|
||||
|
||||
- path: urdf
|
||||
required: false
|
||||
@@ -96,10 +128,13 @@ assets:
|
||||
|
||||
- path: sensor_plugins
|
||||
required: false
|
||||
what: 传感器插件（含 Orisys 触觉自定义插件）
|
||||
what: 传感器插件（自定义 C++ 插件用；当前**为空**）
|
||||
format: 插件源码 + CMakeLists.txt
|
||||
naming: 插件类名以 Orisys 开头
|
||||
where_used: 仿真传感器数据发布
|
||||
where_used: >
|
||||
目前没有使用者：触觉走 Gazebo 内置的 force_torque + contact 传感器，
|
||||
由 sim_tactile_adapter（Python）合成 TactileStatus，无需自研插件。
|
||||
保留此目录用于"以后真需要写插件"的场景（如自研视触觉弹性体成像）。
|
||||
|
||||
# --- 适配器 ----------------------------------------------------------------
|
||||
driver:
|
||||
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Gazebo 模型清单文件。命名规则见同目录 workpiece 的 model.config。
|
||||
|
||||
这个模型的 <name> 是 gauge，与目录名一致。
|
||||
注意它**不**对应 YOLO 的类别名 —— 通止规是被夹爪夹着的，不靠视觉去找它
|
||||
（真机上是人工放到夹爪里的，仿真里由 config/scene.yaml 摆好）。
|
||||
所以"名字必须等于类别名"这条约束对工件成立，对规不成立。
|
||||
-->
|
||||
<model>
|
||||
<name>gauge</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.9">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>ros2 通止规装配项目</name>
|
||||
<email></email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
螺纹通止规：通端 φ6.0×30mm（朝下，要能穿过 φ7 孔）、
|
||||
夹持段 φ30×45mm（夹爪夹这里）、止端 φ8.0×8mm（朝上，应通不过 φ7 孔）。
|
||||
总长 86mm，钢件，约 0.265kg。
|
||||
|
||||
螺纹不建模（旋入靠状态机的 screw_pitch_mm 做运动学同步，不是螺纹几何），
|
||||
接触刚度故意给软（kp=5e4）以匹配位置控制夹爪 —— 两处原因都写在 model.sdf 文件头。
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,193 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
=============================================================================
|
||||
devices/sim/gazebo/models/gauge/model.sdf
|
||||
通止规（螺纹通止规）。被夹爪夹住、往工件孔里旋入的那个"规"。
|
||||
|
||||
位姿由 config/scene.yaml 决定，不在本文件里写死（见 workpiece 的同类说明）。
|
||||
|
||||
============================================================================
|
||||
结构（局部坐标：原点在最下端的规尖，+Z 朝上，与真机"竖直朝下装配"一致）
|
||||
============================================================================
|
||||
|
||||
┌──────┐ ← 止端 NO-GO φ8.0 × 8mm z 0.078~0.086
|
||||
│ │ 通不过 φ7 的孔（过盈 0.5mm 半径）→ 判断"孔大了"
|
||||
├──────┤
|
||||
│ │
|
||||
│ │ ← 夹持段 GROUND φ30 × 45mm z 0.033~0.078
|
||||
│ │ 夹爪夹这里。φ30 是**按夹爪的实际开合范围选的**：
|
||||
│ │ 实测两指指尖间距 fw=0 时约 27~29mm、fw=0.10 时约 128mm
|
||||
│ │ （见 README 的实测表）。取 30mm 让夹爪在接近闭合处
|
||||
│ │ 夹住 —— 既能夹稳，又不会一上来就顶到行程末端。
|
||||
├──────┤ ← 台肩 COLLAR φ16 × 3mm z 0.030~0.033
|
||||
│ │ 进入孔前会先碰到工件表面，是"到底了"的硬限位
|
||||
└──────┘
|
||||
││││ ← 通端 GO φ6.0 × 30mm z 0~0.030
|
||||
││││ 要通过 φ7 的光孔（单边间隙 0.5mm）→ 判断"孔小了"
|
||||
▼▼▼▼
|
||||
|
||||
通端/止端的方向是有讲究的：**朝下的是通端**。因为装配时规是竖直朝下插入的，
|
||||
真正进入孔里的必须是通端；止端朝上、只在人工复核（把规拔出来调头再试）时用。
|
||||
若两者装反，仿真里会表现成"规怎么都插不进孔"，而看模型外观完全正常。
|
||||
|
||||
============================================================================
|
||||
⚠️ 螺纹没有建模 —— 这是有意的，不是说简化
|
||||
============================================================================
|
||||
SDF 里没有螺纹几何，本框架也**不需要**它：
|
||||
S08 的旋入是状态机按 site.yaml 的 screw_pitch_mm 同步下发"绕世界 Z 旋转
|
||||
+ 按螺距下降"（见 stage3_search_node 的 insert 段）。物理上就是一根光轴
|
||||
在光孔里转，螺距是**运动学参数**，不是几何参数。
|
||||
所以仿真能验证的是"旋入动作/时序/力判据的联动对不对"，
|
||||
**不能**验证螺纹啮合本身（会不会卡牙、螺纹精度够不够）。
|
||||
|
||||
============================================================================
|
||||
⚠️ 单位：米。坐标原点在规尖，所以场景里 z = 台面高度 就是"规立在台面上"。
|
||||
=============================================================================
|
||||
-->
|
||||
<sdf version="1.9">
|
||||
<model name="gauge">
|
||||
|
||||
<static>false</static>
|
||||
<self_collide>false</self_collide>
|
||||
|
||||
<link name="body">
|
||||
|
||||
<!-- =====================================================================
|
||||
惯量
|
||||
=====================================================================
|
||||
钢件（ρ≈7850）。按各段体积算：
|
||||
通端 π·0.003²·0.030 = 8.5e-7
|
||||
台肩 π·0.008²·0.003 = 6.0e-7
|
||||
夹持段 π·0.015²·0.045 = 3.18e-5
|
||||
止端 π·0.004²·0.008 = 4.0e-7
|
||||
合计 ≈ 3.38e-5 m³ → 质量 ≈ 0.265kg
|
||||
惯量按"等效圆柱"(r=15mm, h=86mm)算，够用 —— 接触仿真里
|
||||
规的姿态由接触约束主导，惯量的精确值不影响结论。
|
||||
===================================================================== -->
|
||||
<inertial>
|
||||
<mass>0.265</mass>
|
||||
<inertia>
|
||||
<ixx>1.784e-04</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>1.784e-04</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>2.980e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
<!-- =====================================================================
|
||||
通端 GO —— φ6.0 × 30mm（朝下）
|
||||
=====================================================================
|
||||
⚠️ 接触刚度（kp/kd）故意给得**软**，这是本模型最关键的一处调参：
|
||||
夹爪是**位置控制**的（config/controllers.yaml 只有 position 接口），
|
||||
夹持力不是"设"出来的，而是由位置误差乘接触刚度决定的。
|
||||
用默认硬接触（world 里台面是 kp=1e7）时，夹爪指令闭合的几毫米
|
||||
会变成几百牛的挤压力 —— 实测夹一块薄板时指尖力传感器读到过
|
||||
700N 量级。那种力量下规会被弹飞，整条装配流程没法看。
|
||||
把规这一侧的 kp 降到 5e4（比台面低 200 倍）：夹持 0.5mm 的
|
||||
指令过盈 ≈ 25N，既夹得住（不掉），又不会弹飞。
|
||||
想让夹持更紧/更松，首选的旋钮是**指令过盈量**，而不是继续改 kp。
|
||||
===================================================================== -->
|
||||
<collision name="go_stud">
|
||||
<pose>0 0 0.015 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.003</radius><length>0.030</length></cylinder></geometry>
|
||||
<surface>
|
||||
<friction><ode><mu>0.25</mu><mu2>0.25</mu2></ode></friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>5e4</kp>
|
||||
<kd>5e2</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.0005</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<!-- 台肩 -->
|
||||
<collision name="collar">
|
||||
<pose>0 0 0.0315 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.008</radius><length>0.003</length></cylinder></geometry>
|
||||
<surface>
|
||||
<friction><ode><mu>0.25</mu><mu2>0.25</mu2></ode></friction>
|
||||
<contact>
|
||||
<ode><kp>5e4</kp><kd>5e2</kd><max_vel>0.01</max_vel><min_depth>0.0005</min_depth></ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<!-- 夹持段：夹爪夹这里，同样用软接触。
|
||||
摩擦给 0.4：真机夹爪指垫是橡胶/聚氨酯，比金属-金属的 0.25 高。
|
||||
给太低（<0.2）规会在指间打滑自转，而现象看起来像"夹爪没夹紧"。 -->
|
||||
<collision name="grip">
|
||||
<pose>0 0 0.0555 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.015</radius><length>0.045</length></cylinder></geometry>
|
||||
<surface>
|
||||
<friction><ode><mu>0.4</mu><mu2>0.4</mu2></ode></friction>
|
||||
<contact>
|
||||
<ode><kp>5e4</kp><kd>5e2</kd><max_vel>0.01</max_vel><min_depth>0.0005</min_depth></ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<!-- 止端 NO-GO -->
|
||||
<collision name="nogo_stud">
|
||||
<pose>0 0 0.082 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.004</radius><length>0.008</length></cylinder></geometry>
|
||||
<surface>
|
||||
<friction><ode><mu>0.25</mu><mu2>0.25</mu2></ode></friction>
|
||||
<contact>
|
||||
<ode><kp>5e4</kp><kd>5e2</kd><max_vel>0.01</max_vel><min_depth>0.0005</min_depth></ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<!-- =====================================================================
|
||||
视觉
|
||||
===================================================================== -->
|
||||
<!-- 通端：深色（发黑处理的量规就是黑的），与工件的灰形成对比，
|
||||
便于在 D405 画面里看清"规尖是不是对准了孔"。 -->
|
||||
<visual name="go_stud_vis">
|
||||
<pose>0 0 0.015 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.003</radius><length>0.030</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.08 0.08 0.09 1</ambient>
|
||||
<diffuse>0.12 0.12 0.13 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="collar_vis">
|
||||
<pose>0 0 0.0315 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.008</radius><length>0.003</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.7 0.7 0.68 1</ambient>
|
||||
<diffuse>0.8 0.8 0.78 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- 夹持段：亮色 + 滚花的感觉只用颜色区分（不建真实滚花网格，
|
||||
它对物理与视觉判据都没有影响，只会让模型变重）。 -->
|
||||
<visual name="grip_vis">
|
||||
<pose>0 0 0.0555 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.015</radius><length>0.045</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.75 0.75 0.72 1</ambient>
|
||||
<diffuse>0.85 0.85 0.82 1</diffuse>
|
||||
<specular>0.3 0.3 0.3 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- 止端：做成红色，一眼能区分通端/止端。
|
||||
这不是装饰 —— 现场最容易犯的错就是把规装反（通端朝上），
|
||||
红色让"哪头朝下"在预览里瞬间可判。 -->
|
||||
<visual name="nogo_stud_vis">
|
||||
<pose>0 0 0.082 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.004</radius><length>0.008</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.6 0.05 0.05 1</ambient>
|
||||
<diffuse>0.8 0.08 0.08 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Gazebo 模型清单文件。命名规则见同目录 workpiece 的 model.config。
|
||||
-->
|
||||
<model>
|
||||
<name>gauge_stand</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.9">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>ros2 通止规装配项目</name>
|
||||
<email></email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
通止规座：60×60×32mm 方块、中心 7×7 贯通方孔，静态（工装，不该被推走）。
|
||||
|
||||
存在的唯一理由：让通止规能立住。规的支撑面只有 φ6，高 86mm，
|
||||
直接立在台面上会在 0.2 秒内倒掉（实测 RPY 变成躺平）—— 详见 model.sdf 文件头。
|
||||
规座与规的关系是"插在孔里"，属于正常嵌套，不是碰撞重叠。
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,107 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
=============================================================================
|
||||
devices/sim/gazebo/models/gauge_stand/model.sdf
|
||||
通止规座（规架）。作用只有一个：让通止规能**立住**。
|
||||
|
||||
============================================================================
|
||||
为什么必须有这个东西（这是实测出来的，不是设计出来的）
|
||||
============================================================================
|
||||
第一版没有规座，直接把规"立"在台面上，结果它 0.2 秒内就倒了：
|
||||
ign model -m gauge -p → RPY = (-1.09, -0.37, 0.64) ← 已经躺平
|
||||
|
||||
原因很直接：规的支撑面是 φ6 的规尖，而它本身高 86mm、
|
||||
重心在夹持段（约 55mm 高处）。这种细高比（高/底面直径 ≈ 14）
|
||||
在真实世界里也立不住 —— 真机现场是把规插在规座里、或者直接放进夹爪。
|
||||
|
||||
于是有两种"让规待着"的办法，这里选了第一种：
|
||||
方案 A（本模型）：加一个规座，规插在方孔里立着
|
||||
✔ 与现场一致（规座是真实存在的工装）
|
||||
✔ 夹爪能真的从上方去夹 —— P6 的"夹取"这一步才有东西可夹
|
||||
方案 B：让规一开始就"浮"在空中（<static>true</static>）
|
||||
✘ 视觉假、也不可夹取（static 物体夹不动，力会直接顶到夹爪上）
|
||||
|
||||
============================================================================
|
||||
几何
|
||||
============================================================================
|
||||
▣ 60×60×32 的方块，中心一个 7×7 的**贯通方孔**
|
||||
规的通端（φ6）插进方孔，台肩（φ16）坐在方块顶面上 → 立住
|
||||
方孔贯通是为了让 30mm 长的通端有地方去：座高 32mm、
|
||||
规尖落在座底面之上 2mm（离台面 2mm，避免与台面接触）
|
||||
|
||||
孔为什么是方的：
|
||||
SDF 没有布尔运算，挖不出圆孔。这里用 4 个 box 围出方孔，
|
||||
与工件用同一套办法（见 workpiece/model.sdf 的说明）。
|
||||
φ6 的规过 7×7 的方孔：对边间隙 0.5mm，足够。
|
||||
|
||||
坐标原点：座底面中心。所以场景里 z = 台面高度 就把它放在台面上。
|
||||
<static>true</static>：规座是工装，不该被机械臂推走。
|
||||
static 物体不需要 <inertial>（SDF 允许省略）。
|
||||
=============================================================================
|
||||
-->
|
||||
<sdf version="1.9">
|
||||
<model name="gauge_stand">
|
||||
|
||||
<static>true</static>
|
||||
<self_collide>false</self_collide>
|
||||
|
||||
<link name="base">
|
||||
|
||||
<!-- 四块围出中间的 7×7 贯通方孔（局部坐标，底面中心为原点） -->
|
||||
<collision name="stand_south">
|
||||
<pose>0 -0.01675 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.060 0.0265 0.032</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<collision name="stand_north">
|
||||
<pose>0 0.01675 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.060 0.0265 0.032</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<collision name="stand_west">
|
||||
<pose>-0.01675 0 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.0265 0.007 0.032</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<collision name="stand_east">
|
||||
<pose>0.01675 0 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.0265 0.007 0.032</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
|
||||
<visual name="stand_south_vis">
|
||||
<pose>0 -0.01675 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.060 0.0265 0.032</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.25 0.28 0.32 1</ambient>
|
||||
<diffuse>0.33 0.36 0.40 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="stand_north_vis">
|
||||
<pose>0 0.01675 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.060 0.0265 0.032</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.25 0.28 0.32 1</ambient>
|
||||
<diffuse>0.33 0.36 0.40 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="stand_west_vis">
|
||||
<pose>-0.01675 0 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.0265 0.007 0.032</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.25 0.28 0.32 1</ambient>
|
||||
<diffuse>0.33 0.36 0.40 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="stand_east_vis">
|
||||
<pose>0.01675 0 0.016 0 0 0</pose>
|
||||
<geometry><box><size>0.0265 0.007 0.032</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.25 0.28 0.32 1</ambient>
|
||||
<diffuse>0.33 0.36 0.40 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Gazebo 模型清单文件。作用是让这个目录能被当成 model:// 资源引用，
|
||||
并给 GUI / `ign model --list` 提供可读信息。
|
||||
|
||||
⚠️ 命名规则（两条都是框架约定，改之前先看 README 的"建模要求"）：
|
||||
1. <name> 必须与**目录名一致**（此处 workpiece），否则 model:// 引用会
|
||||
找不到 —— 而 Gazebo 只会静默跳过，不报错。
|
||||
2. <name> 必须与推理用的**类别名一致**（YOLO 的 labels 里那个名字）。
|
||||
否则仿真里算出的工件坐标和检测结果对不上：明明检测到工件，
|
||||
坐标却是另一个东西的。这条最容易在"新建一个模型试试"时被破坏。
|
||||
-->
|
||||
<model>
|
||||
<name>workpiece</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.9">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>ros2 通止规装配项目</name>
|
||||
<email></email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
通止规装配的工件：140×100×12mm 板，中心 φ40 光孔（会被检测的 max_radius 滤掉），
|
||||
两侧各一个 φ7 光孔、孔心间距 58mm、孔口带 φ14 亮环（二阶段 HoughCircles 检测的目标）。
|
||||
|
||||
详细说明（尺寸怎么反推出来的、哪些是近似）见 model.sdf 文件头与
|
||||
devices/sim/gazebo/README.md 的"仿真 ≠ 真机"一节。
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,325 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
=============================================================================
|
||||
devices/sim/gazebo/models/workpiece/model.sdf
|
||||
工件（被装配的对象）。
|
||||
|
||||
由 config/scene.yaml 决定摆在台面上的哪个位置，启动时由 gazebo_sim.launch.py
|
||||
用 ros_gz_sim create -file 生成。**不要**把位姿写死在这里 ——
|
||||
位姿属于"场景"，不属于"模型"，分开才能同一个模型摆多处。
|
||||
|
||||
============================================================================
|
||||
这个模型在还原真机的什么
|
||||
============================================================================
|
||||
尺寸与孔位不是随便定的，是按真机检测参数（src/arm_application/config/
|
||||
hole_detect.yaml）反推出来的，每条都对得上：
|
||||
|
||||
两个螺纹孔间距 58mm
|
||||
依据 insert_bolt_radius_mm: 29.0（注释写明"两目标孔物理间距~58mm"）。
|
||||
这两个孔就是二阶段要对准的 k1 / k2。
|
||||
|
||||
孔口是"环"而不是"洞"（视觉上）
|
||||
检测用的是 HoughCircles 找**亮环**，且 hough_min_radius=10 /
|
||||
hough_max_radius=30。换算成物理尺寸（真机 D405：848 宽、87° 视场，
|
||||
fx≈447，相机到工件约 0.23m）：
|
||||
radius_px = r_m × 447 / 0.23 ≈ r_m × 1943
|
||||
10px ≈ 5.1mm 30px ≈ 15.4mm
|
||||
也就是说**能被检测到的圆，半径必须在 5.1~15.4mm 之间**。
|
||||
本模型把每个螺纹孔的亮环外径做成 φ14（半径 7mm → 约 13.6px，落在
|
||||
区间中段），孔本身 φ7（半径 3.5mm → 约 6.8px，**低于下限会被滤掉**）。
|
||||
这正是真机上的情形：检测到的是嵌件的环，不是螺纹孔本身。
|
||||
|
||||
中间有个大孔，且必须被滤掉
|
||||
hole_detect 的 max_radius=30（"滤掉中间大孔"）与 site.yaml 的
|
||||
"过小会配到中心孔" 都说明真件中心有一个大孔。这里做成 φ40
|
||||
（半径 20mm → 约 38.9px > 30）→ 会被 max_radius 正确滤除。
|
||||
它的存在是有意义的：中心的**光孔**正是通止规的"通端要通过"的区域，
|
||||
也是二阶段粗居中时 bbox 的中心特征。
|
||||
|
||||
============================================================================
|
||||
⚠️ 两个必须知道的近似（都写在 README 的"仿真≠真机"一节）
|
||||
============================================================================
|
||||
1) SDF **没有布尔运算**，无法在板上"挖"一个圆孔。所以：
|
||||
碰撞体 = 用 10 个 box 拼出板子，留下**方孔**
|
||||
中心 40×40mm、两侧 7×7mm
|
||||
视觉 = 同一批 box（保证视觉与碰撞完全一致，不会骗人）
|
||||
+ 每个孔上方贴一个亮圆盘 / 暗圆柱，做出"环 + 暗孔"的样子
|
||||
相机从上往下看到的仍然是**圆环**（贴片是圆的），所以二阶段的圆检测
|
||||
在仿真里能看到该看到的东西；而通止规要穿过的是那个方孔 ——
|
||||
φ6 的规过 7×7 的方孔：对边间隙 0.5mm，足够。
|
||||
方孔在 GUI 里看得出来，这是有意的取舍，不是 bug。
|
||||
|
||||
2) 真机是 **M6 螺纹孔**，仿真里按**光孔**建模（SDF 没有螺纹）。
|
||||
孔的直径（φ7）比 M6 的小径（≈4.9mm）大，是为了让接触求解器稳定：
|
||||
world 的 contact_surface_layer 是 1mm，间隙小于 1mm 时求解器会认为
|
||||
一直贴着。**因此仿真里的插入力/接触力只能定性参考，不能用来标定真机的
|
||||
力阈值** —— 真机阈值必须在真机上标。
|
||||
|
||||
============================================================================
|
||||
单位：米。SDF 一律用米，用毫米建模的必须先缩放（见 README 的建模要求）。
|
||||
坐标原点：板子**底面中心**。所以场景里把 z 设成台面高度，板子就正好落在台面上。
|
||||
=============================================================================
|
||||
-->
|
||||
<sdf version="1.9">
|
||||
<model name="workpiece">
|
||||
|
||||
<!-- 静态试跑时可把下面这行改成 true 钉住工件（调试检测/对孔时很有用：
|
||||
否则一次误碰就把它推走了，而你会以为是检测出问题）。
|
||||
正常装配流程必须 false —— 工件被推动本身就是要观察的物理现象。 -->
|
||||
<static>false</static>
|
||||
<self_collide>false</self_collide>
|
||||
|
||||
<link name="body">
|
||||
|
||||
<!-- =====================================================================
|
||||
惯量
|
||||
=====================================================================
|
||||
铝合金（ρ≈2700）实心等效：无孔板体积 0.14×0.10×0.012 = 1.68e-4 m³
|
||||
→ 质量 0.454kg。三个孔挖掉约 1.7e-5 m³（≈10%），但这里**不扣**，
|
||||
偏保守（略重 = 略稳，不会因为惯量偏小而在接触里抖）。
|
||||
惯量按整块长方体的公式算：I = m/12 × (另两边平方和)
|
||||
========================================================================= -->
|
||||
<inertial>
|
||||
<mass>0.454</mass>
|
||||
<inertia>
|
||||
<ixx>3.804e-04</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>7.404e-04</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1.110e-03</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
<!-- =====================================================================
|
||||
碰撞体：10 个 box 拼出的"带方孔的板"
|
||||
=====================================================================
|
||||
切分方式（局部坐标，原点在底面中心，x 沿 140mm 方向）：
|
||||
y 方向切 5 条带，等宽处由孔决定：
|
||||
A [-0.050,-0.020] 只避开中心孔
|
||||
B [-0.020,-0.0035] 避开中心孔
|
||||
C [-0.0035,0.0035] 同时避开中心孔与两侧小孔
|
||||
D [ 0.0035,0.020] 同 B
|
||||
E [ 0.020, 0.050] 整条实心
|
||||
x 方向按各带里有哪些孔再切。
|
||||
留下的孔（都是方孔）：
|
||||
中心 x∈[-0.020,0.020] y∈[-0.020,0.020] 40×40mm
|
||||
左 x∈[-0.0325,-0.0255] y∈[-0.0035,0.0035] 7×7mm @ x=-29mm
|
||||
右 x∈[ 0.0255, 0.0325] 同上 7×7mm @ x=+29mm
|
||||
两侧小孔中心距恰好 58mm（0.058），与 insert_bolt_radius_mm=29 对齐。
|
||||
===================================================================== -->
|
||||
|
||||
<!-- A：最下一条带，整条实心 -->
|
||||
<collision name="plate_a">
|
||||
<pose>0 -0.035 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.140 0.030 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
|
||||
<!-- B：中心孔左侧 -->
|
||||
<collision name="plate_b_left">
|
||||
<pose>-0.045 -0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<!-- B：中心孔右侧 -->
|
||||
<collision name="plate_b_right">
|
||||
<pose>0.045 -0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
|
||||
<!-- C：穿过三个孔的那条带，被切成 4 段（见上面 x 切分） -->
|
||||
<collision name="plate_c_far_left">
|
||||
<pose>-0.05125 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0375 0.007 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<!-- 中心孔与小孔之间那道 5.5mm 的窄壁 —— 真件上就是这里最薄，
|
||||
也是通止规对孔时最容易别到的地方，保留它是有意义的。 -->
|
||||
<collision name="plate_c_web_left">
|
||||
<pose>-0.02275 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0055 0.007 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<collision name="plate_c_web_right">
|
||||
<pose>0.02275 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0055 0.007 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<collision name="plate_c_far_right">
|
||||
<pose>0.05125 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0375 0.007 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
|
||||
<!-- D：中心孔左/右 -->
|
||||
<collision name="plate_d_left">
|
||||
<pose>-0.045 0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
<collision name="plate_d_right">
|
||||
<pose>0.045 0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
|
||||
<!-- E：最上一条带，整条实心 -->
|
||||
<collision name="plate_e">
|
||||
<pose>0 0.035 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.140 0.030 0.012</size></box></geometry>
|
||||
<surface><friction><ode><mu>0.6</mu><mu2>0.6</mu2></ode></friction></surface>
|
||||
</collision>
|
||||
|
||||
<!-- =====================================================================
|
||||
视觉：与碰撞体**同一批 box**（不另画一个"漂亮但不一样"的板子）
|
||||
=====================================================================
|
||||
材质：深灰哑光。选哑光是因为真机的金属件在 D405 的环形光下不会
|
||||
镜面反光到过曝 —— 过曝会把亮环淹掉，检测直接失效。
|
||||
===================================================================== -->
|
||||
<visual name="plate_a_vis">
|
||||
<pose>0 -0.035 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.140 0.030 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_b_left_vis">
|
||||
<pose>-0.045 -0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_b_right_vis">
|
||||
<pose>0.045 -0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_c_far_left_vis">
|
||||
<pose>-0.05125 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0375 0.007 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_c_web_left_vis">
|
||||
<pose>-0.02275 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0055 0.007 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_c_web_right_vis">
|
||||
<pose>0.02275 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0055 0.007 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_c_far_right_vis">
|
||||
<pose>0.05125 0 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.0375 0.007 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_d_left_vis">
|
||||
<pose>-0.045 0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_d_right_vis">
|
||||
<pose>0.045 0.01175 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.050 0.0165 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="plate_e_vis">
|
||||
<pose>0 0.035 0.006 0 0 0</pose>
|
||||
<geometry><box><size>0.140 0.030 0.012</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.35 0.36 0.38 1</ambient>
|
||||
<diffuse>0.42 0.43 0.45 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- =====================================================================
|
||||
两个螺纹孔的"嵌件环"（视觉）—— 二阶段要检测的就是这个环
|
||||
=====================================================================
|
||||
做法：一个**亮**圆盘（φ14，环的外径）贴在孔里 2mm 深处，
|
||||
再叠一个**暗**圆柱（φ7，孔本身）比它高 1mm。
|
||||
上往下看：暗圆柱把亮盘中心挡掉 → 剩下的就是一圈亮环（φ7→φ14）。
|
||||
这不是"画个圆假装有孔"，而是用两个实体拼出环的**可见几何**，
|
||||
所以遮挡关系是对的、深度也是对的（孔比面低 1mm）。
|
||||
|
||||
为什么亮盘用 φ14：见文件头 —— 要让环外径落在
|
||||
hough 的 10~30px 有效区间，而 13.6px 正好在区间中段。
|
||||
===================================================================== -->
|
||||
<visual name="insert_left_ring">
|
||||
<pose>-0.029 0 0.010 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.007</radius><length>0.001</length></cylinder></geometry>
|
||||
<material>
|
||||
<!-- 亮：嵌件是机加工面，比哑光的板面亮得多 —— 检测靠的就是这个对比 -->
|
||||
<ambient>0.85 0.85 0.82 1</ambient>
|
||||
<diffuse>0.95 0.95 0.90 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="insert_left_bore">
|
||||
<pose>-0.029 0 0.0075 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.0035</radius><length>0.007</length></cylinder></geometry>
|
||||
<material>
|
||||
<!-- 暗：孔内是阴影 + 无光，与亮环形成对比，HoughCircles 才能锁定 -->
|
||||
<ambient>0.02 0.02 0.02 1</ambient>
|
||||
<diffuse>0.03 0.03 0.03 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<visual name="insert_right_ring">
|
||||
<pose>0.029 0 0.010 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.007</radius><length>0.001</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.85 0.85 0.82 1</ambient>
|
||||
<diffuse>0.95 0.95 0.90 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="insert_right_bore">
|
||||
<pose>0.029 0 0.0075 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.0035</radius><length>0.007</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.02 0.02 0.02 1</ambient>
|
||||
<diffuse>0.03 0.03 0.03 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- 中心大孔：只做一个暗的深孔。半径 20mm → 约 38.9px，
|
||||
超过 hough_max_radius=30，会被检测正确滤掉（真件也是这个行为）。 -->
|
||||
<visual name="center_bore">
|
||||
<pose>0 0 0.007 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.020</radius><length>0.010</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.02 0.02 0.02 1</ambient>
|
||||
<diffuse>0.03 0.03 0.03 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -47,11 +47,39 @@
|
||||
d405_roll:=0.0
|
||||
d405_pitch:=0.0
|
||||
d405_yaw:=0.0
|
||||
image_width:=640
|
||||
image_width:=848
|
||||
image_height:=480
|
||||
image_hz:=30
|
||||
">
|
||||
|
||||
<!-- =======================================================================
|
||||
图像尺寸（image_width / image_height）—— **必须与真机一致**
|
||||
=======================================================================
|
||||
⚠️ 注意：这段说明**不能**写成上面 params="..." 里的 XML 注释 ——
|
||||
XML 不允许在属性值里放注释，那样整个文件会 not well-formed，
|
||||
xacro 直接展开失败（踩过）。所以只能放在这里。
|
||||
|
||||
真机 D405 跑 848x480@30（见 config/site.yaml 的 camera.width/height）。
|
||||
仿真也必须 848x480，理由是**像素尺度的可迁移性**：
|
||||
|
||||
阶段二的孔检测（hole_detect_hough）直接在**原始分辨率**图上跑
|
||||
HoughCircles，不做缩放。所以 hole_detect.yaml 里那些以像素为单位的
|
||||
门槛（hough_min_radius 10 / min_radius 10 / hole_distance_min 90 …）
|
||||
只有在"每毫米对应多少像素"与真机相同时才有意义：
|
||||
fx = width / (2·tan(hfov/2))
|
||||
视场角两边都是 87°，所以像素尺度完全由 width 决定：
|
||||
真机 848 → fx ≈ 447
|
||||
曾用 640 → fx ≈ 337（每毫米像素数只有真机的 75%）
|
||||
用 640 的后果很具体：58mm 的两孔间距在真机上成像约 152px（过 90 的门槛），
|
||||
在仿真里只剩 115px —— 仍是能过，但整套像素门槛的余量都被吃掉 25%，
|
||||
而"看起来不对"的表现只是"孔检测偶尔不认"，极难归因。
|
||||
所以在仿真侧就把尺度对齐，别留给调试。
|
||||
|
||||
启动时会比对这里与 cameras.yaml 的 width/height，不一致会告警
|
||||
（见 sim_cameras.check_camera_consistency）—— 图幅写错不报错，
|
||||
只会让像素门槛静默失效，所以要主动查。
|
||||
======================================================================= -->
|
||||
|
||||
<!-- =======================================================================
|
||||
传感器系统插件放在 **world 文件**里，不放这里
|
||||
=======================================================================
|
||||
|
||||
@@ -97,10 +97,14 @@
|
||||
|
||||
<!-- =========================================================================
|
||||
工件台
|
||||
=========================================================================
|
||||
高度 0.75m 是常见工作台高度。工件就放在台面上（P5c-6 里会加），
|
||||
100| =========================================================================
|
||||
台面顶面在 z = 0.40（本模型 pose z=0 + link top 的 z=0.375 + 板厚 0.05÷2）。
|
||||
⚠️ 早先这里的注释写的是"高度 0.75m 是常见工作台高度"，与实际的 0.40
|
||||
矛盾（注释是从别处抄来的）。以**算出来的 0.40 为准**：相机验收脚本
|
||||
check_sim_cameras.py 就是按 z=0.40 反投影验收的，它才是契约。
|
||||
台面高度决定机械臂要不要伸到底 —— 先在仿真里确认工作空间够得着，
|
||||
再上真机试，能省掉很多次现场调试。
|
||||
工件/通止规**不在这个文件里**：它们由 scene.yaml 描述、启动时 spawn。
|
||||
|
||||
台面做成 static：台子本身不该被机械臂推着走。
|
||||
摩擦系数给足（0.8），否则柔顺下压时工件会被顶得滑动，
|
||||
@@ -149,25 +153,17 @@
|
||||
</model>
|
||||
|
||||
<!-- =========================================================================
|
||||
通止规起始位（留给人工/机械手放置用的标记区）
|
||||
工件 / 通止规 —— **不在这里**
|
||||
=========================================================================
|
||||
真机上通止规是被人放到夹爪里的（P6 里有"提示用户放置"的交互）。
|
||||
仿真里这里放一个视觉标记，方便对着它调整"返回极值角是否到位"。
|
||||
真正会被夹爪抓取的通止规模型在 P5c-6 里加到这个位置。
|
||||
它们**不进 world 文件**，而是在启动时由 gazebo_sim.launch.py 用
|
||||
`ros_gz_sim create` spawn（位姿在 devices/sim/gazebo/config/scene.yaml）。
|
||||
分工：world 放"固定不动"的东西（地面、台子、支架相机），
|
||||
物料按站点/工序换位置 —— 写进 world 就等于每次换料都改 world，
|
||||
而 world 里还有物理参数和 5 个必需的系统插件（动它有静默失效风险）。
|
||||
本文件里早期有过一个 gauge_marker（一个悬空的蓝色圆盘，用来标记
|
||||
"通止规该放这里"）。它已经没用了：真正的通止规模型已经在 models/ 里、
|
||||
由场景配置摆放，留着只会让人以为"那就是通止规"。
|
||||
========================================================================= -->
|
||||
<model name="gauge_marker">
|
||||
<static>true</static>
|
||||
<pose>0.15 0.35 0.755 0 0 0</pose>
|
||||
<link name="link">
|
||||
<visual name="visual">
|
||||
<geometry><cylinder><radius>0.04</radius><length>0.004</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.1 0.6 0.9 1</ambient>
|
||||
<diffuse>0.1 0.6 0.9 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- =========================================================================
|
||||
基础系统插件 —— 必须显式声明，否则整个仿真静默失效
|
||||
|
||||
@@ -15,10 +15,15 @@
|
||||
- 规则写在 [models/README.md](../models/README.md)。`export_yolo_trt.sh` 现在是它的兼容转发壳。
|
||||
**配置：** `show_site_config.sh` · `lib/export_site_env.py`
|
||||
**仿真：** `run_sim_gz.sh`（Gazebo 物理仿真，唯一能做接触/力控验证的入口）
|
||||
停：`stop_sim_gz.sh`（**别敲 `pkill -f gz`** —— 会杀掉调用它的 shell，见脚本文件头）
|
||||
- 环境是独立 conda 环境 `ros_gz_sim`（`install_sim_env.sh` 装、`sim_env.sh` 激活），与真机环境隔离
|
||||
- 相机验收：`python3 scripts/check_sim_cameras.py` —— **别用 `ros2 topic hz` 或肉眼看图验收相机**，
|
||||
仿真相机的错误几乎全是静默的（深度单位、frame_id、内参、位姿方向），都能出图却是错的。
|
||||
这条命令把深度反投影到 world，要求命中已知平面（地面/台面）才算通过。
|
||||
- 场景验收：`python3 scripts/check_sim_scene.py` —— 同样别靠肉眼。它管四件事：
|
||||
物体的摆放（悬空/穿模/出界/互相嵌入）、场景里的像素门槛与**真机生效值**是否漂移、
|
||||
孔检测的成像几何（亮环/孔间距换算成像素后够不够门槛）、以及粗定位用的手外相机
|
||||
能不能**完整**看见工件。第二条尤其重要：门槛抄了两份，不一致时仿真会"通过"而真机不认。
|
||||
- 注意 `run_sim.sh` 不是 Gazebo，它走 mock 驱动（假数据 + RViz），永远不会产生接触力
|
||||
**环境：** `env.sh` · `build_all.sh`
|
||||
**RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh` · `rdk_drag_bridge.py`
|
||||
|
||||
@@ -0,0 +1,438 @@
|
||||
#!/usr/bin/env python3
|
||||
"""仿真场景验收：摆放对不对、孔检测的成像几何够不够、粗定位看不看得见工件。
|
||||
|
||||
=============================================================================
|
||||
为什么需要这个工具
|
||||
=============================================================================
|
||||
场景里的错误和相机一样，几乎都是**静默**的 —— 跑得起来、报不出错，
|
||||
只是"结果不对"。这个脚本把四类问题在启动阶段就变成可打印的结论：
|
||||
|
||||
1. 摆放讲不通：悬空 / 穿进台面 / 超出台面 / 两个物体互相嵌入。
|
||||
表现是"工件自己会动、会飞"，而人会怀疑控制或标定。
|
||||
|
||||
2. 场景文件里的像素门槛与真机配置**漂移**。
|
||||
scene.yaml 为了能在启动时就算出"能不能看见"，抄了一份门槛值；
|
||||
抄一份就有漂移风险。这里读 hole_detect.yaml + site.yaml 的生效值做核对，
|
||||
不一致直接判失败 —— 不允许"仿真按旧门槛通过、真机按新门槛不认"。
|
||||
|
||||
3. 成像几何不够用：亮环太小 / 两孔间距不够像素门槛。
|
||||
这些门槛是**像素**量，只有在仿真侧用几何算出来才能提前发现；
|
||||
等上真机再发现要贵得多（要重新调悬停高度、甚至改工件工装）。
|
||||
|
||||
4. 二阶段的悬停高度**超出机械臂工作空间**。
|
||||
这个坑很实在：相机到工件的距离决定了像素尺度，而"距离够近"与
|
||||
"法兰别抬太高"是一对矛盾。脚本会把两者都算出来并核对工作空间上下界。
|
||||
|
||||
5. 粗定位（D435）看不看得见工件。
|
||||
二阶段粗居中靠 D435 出工件的 bbox；工件被裁掉一角时检测仍会出结果
|
||||
（框不全而已），于是算出的中心偏掉 —— 这属于"能出结果但结果是错的"。
|
||||
|
||||
用法（需要 ROS 环境以读 cameras.yaml 内参；查 Gazebo 位姿需要仿真在跑）：
|
||||
source scripts/sim_env.sh
|
||||
python3 scripts/check_sim_scene.py
|
||||
python3 scripts/check_sim_scene.py --no-poses # 跳过"查 Gazebo 实际位姿"
|
||||
|
||||
退出码 0 = 通过；非 0 = 有问题（可直接用于启动自检 / CI）。
|
||||
=============================================================================
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 小工具
|
||||
# ---------------------------------------------------------------------------
|
||||
def rpy_to_R(r: float, p: float, y: float):
|
||||
"""rpy（ZYX 复合顺序：R = Rz·Ry·Rx）→ 3x3 旋转矩阵。
|
||||
|
||||
顺序不能想当然：URDF/SDF 的 rpy 都是 Rz·Ry·Rx。
|
||||
"先俯 50° 再偏航 180°" 与 "先偏航再俯" 的结果**不同**，
|
||||
这个项目就为此把 D435 的视线翻到背后过（见 assembly.world 的注释）。
|
||||
"""
|
||||
cr, sr = math.cos(r), math.sin(r)
|
||||
cp, sp = math.cos(p), math.sin(p)
|
||||
cy, sy = math.cos(y), math.sin(y)
|
||||
return [
|
||||
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
|
||||
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
|
||||
[-sp, cp * sr, cp * cr],
|
||||
]
|
||||
|
||||
|
||||
def matmul3(a, b):
|
||||
return [[sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
|
||||
|
||||
|
||||
def matvec3(a, v):
|
||||
return [sum(a[i][k] * v[k] for k in range(3)) for i in range(3)]
|
||||
|
||||
|
||||
def transpose3(a):
|
||||
return [[a[j][i] for j in range(3)] for i in range(3)]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 门槛核对：scene.yaml 抄的那份 vs 真机真正生效的那份
|
||||
# ---------------------------------------------------------------------------
|
||||
def effective_gates(root: Optional[Path] = None) -> dict:
|
||||
"""读出真机**实际生效**的孔检测像素门槛。
|
||||
|
||||
生效规则（见 arm_application/hole_detector.config_from_site）：
|
||||
包内 config/hole_detect.yaml 给默认值，config/site.yaml 的 hole_detect.* 覆盖它。
|
||||
|
||||
半径取两个区间的**交集**：HoughCircles 的搜索范围（hough_min/max_radius）
|
||||
与工业校验的接受范围（min_radius/max_radius）必须同时满足才叫"能被用上"。
|
||||
只按其中一个判，会得出"能检测到"的乐观结论，而实际在另一个环节被滤掉。
|
||||
"""
|
||||
import yaml
|
||||
|
||||
base_root = Path(root) if root is not None else REPO_ROOT
|
||||
pkg = base_root / "src/arm_application/config/hole_detect.yaml"
|
||||
site = base_root / "config/site.yaml"
|
||||
if not site.is_file():
|
||||
site = base_root / "config/site.example.yaml"
|
||||
|
||||
base = {}
|
||||
if pkg.is_file():
|
||||
base = yaml.safe_load(pkg.read_text(encoding="utf-8")) or {}
|
||||
over = {}
|
||||
if site.is_file():
|
||||
doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||
over = (doc.get("hole_detect") or {}) if isinstance(doc, dict) else {}
|
||||
|
||||
def val(key, default):
|
||||
v = over.get(key, base.get(key, default))
|
||||
try:
|
||||
return float(v)
|
||||
except (TypeError, ValueError):
|
||||
return float(default)
|
||||
|
||||
lo = max(val("hough_min_radius", 10), val("min_radius", 10))
|
||||
hi = min(val("hough_max_radius", 30), val("max_radius", 30))
|
||||
return {
|
||||
"insert_ring_radius": (lo, hi),
|
||||
"hole_spacing": (val("hole_distance_min", 150), val("hole_distance_max", 700)),
|
||||
"_source": (str(site), str(pkg)),
|
||||
}
|
||||
|
||||
|
||||
def workspace_z_limits(root: Optional[Path] = None):
|
||||
"""从 site.yaml 读机械臂法兰的工作空间 Z 上下界（用于核对悬停高度）。"""
|
||||
import yaml
|
||||
|
||||
base_root = Path(root) if root is not None else REPO_ROOT
|
||||
site = base_root / "config/site.yaml"
|
||||
if not site.is_file():
|
||||
site = base_root / "config/site.example.yaml"
|
||||
if not site.is_file():
|
||||
return None
|
||||
doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||
servo = doc.get("servo") or {}
|
||||
try:
|
||||
return float(servo["workspace_xyz_min"][2]), float(servo["workspace_xyz_max"][2])
|
||||
except (KeyError, IndexError, TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 检查项
|
||||
# ---------------------------------------------------------------------------
|
||||
def check_placement(scene, root) -> list[str]:
|
||||
from arm_sim_gazebo.sim_scene import validate_scene
|
||||
|
||||
return validate_scene(scene, root)
|
||||
|
||||
|
||||
def check_gate_drift(scene, root=None) -> list[str]:
|
||||
"""scene.yaml 的 gates_px 是否与真机生效值一致。"""
|
||||
eff = effective_gates(root)
|
||||
out = []
|
||||
for key, gate in scene.gates_px.items():
|
||||
want = eff.get(key)
|
||||
if want is None:
|
||||
continue
|
||||
if abs(gate[0] - want[0]) > 1e-6 or abs(gate[1] - want[1]) > 1e-6:
|
||||
out.append(
|
||||
f"gates_px.{key} 写的是 [{gate[0]:g}, {gate[1]:g}]，"
|
||||
f"而真机生效值是 [{want[0]:g}, {want[1]:g}]。\n"
|
||||
f" 真机值来自 {eff['_source'][0]}（覆盖 {eff['_source'][1]}）。\n"
|
||||
f" 这不是小事：两边不一致时，仿真会按旧门槛判'能检测到'，"
|
||||
f"而真机按新门槛不认 —— 属于最费时间的一类问题。\n"
|
||||
f" 修法：把 devices/sim/gazebo/config/scene.yaml 的 gates_px 改成真机值。"
|
||||
)
|
||||
missing = set(eff) - {"_source"} - set(scene.gates_px)
|
||||
for key in sorted(missing):
|
||||
out.append(
|
||||
f"gates_px 里缺 {key}（真机有这个门槛 {eff[key]}）—— "
|
||||
f"缺了就不会校验它，属于「静默放行」。补上它。"
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def check_geometry(scene, root) -> tuple[list[str], dict]:
|
||||
"""孔检测的像素几何：算出实际像素值，与门槛比。"""
|
||||
from arm_sim_gazebo.sim_cameras import intrinsics, load_cameras
|
||||
from arm_sim_gazebo.sim_scene import check_pixel_gates, operating_pixel_report
|
||||
|
||||
specs = load_cameras(root=root)
|
||||
cam = next((s for s in specs if s.ros_ns == scene.operating.camera), None)
|
||||
if cam is None:
|
||||
return [f"cameras.yaml 里没有相机 {scene.operating.camera}"], {}
|
||||
k, _ = intrinsics(cam, cam.width or 640, cam.height or 480)
|
||||
px = operating_pixel_report(scene, k[0], root)
|
||||
if not px:
|
||||
return ["读不出工件模型里的检测特征（visual 名字约定变了？见 sim_scene.features_from_workpiece）"], {}
|
||||
return check_pixel_gates(px, scene.gates_px), px
|
||||
|
||||
|
||||
def check_hover_height(scene, root) -> list[str]:
|
||||
"""悬停高度是否落在机械臂工作空间的 Z 区间内。"""
|
||||
from arm_sim_gazebo.sim_scene import operating_flange_z
|
||||
|
||||
fz = operating_flange_z(scene, root)
|
||||
if fz is None:
|
||||
return ["读不出工件高度，算不出建议悬停高度"]
|
||||
lim = workspace_z_limits(root)
|
||||
if lim is None:
|
||||
return []
|
||||
lo, hi = lim
|
||||
out = []
|
||||
if fz > hi:
|
||||
out.append(
|
||||
f"按当前 camera_to_part_m={scene.operating.camera_to_part_m}m 算出的悬停法兰高度 "
|
||||
f"z≈{fz:.3f}m **超出工作空间上界 {hi}m**。\n"
|
||||
f" 二阶段的位姿检查会直接拒绝执行。\n"
|
||||
f" 两个旋钮：调小 camera_to_part_m（相机更贴近工件 → 像素尺度更大、"
|
||||
f"更容易过像素门槛），或把工件/台面改矮。\n"
|
||||
f" 注意这两者是对立的：离得越近像素越大（检测更好），但法兰越高（越够不着）。"
|
||||
)
|
||||
elif fz < lo:
|
||||
out.append(f"算出的悬停法兰高度 z≈{fz:.3f}m 低于工作空间下界 {lo}m。")
|
||||
return out
|
||||
|
||||
|
||||
def check_d435_visibility(scene, root) -> tuple[list[str], str]:
|
||||
"""把工件顶面的四角投影到 D435 画面，看是否完整可见。
|
||||
|
||||
这一步对应二阶段的**粗定位**：D435 出工件的 bbox，取其中心作为粗目标。
|
||||
工件被裁掉一角时检测照样出结果（框小了而已），算出的中心就偏了 ——
|
||||
"能出结果但结果是错的"，正是这个项目反复强调的那一类。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_cameras import intrinsics, load_cameras
|
||||
from arm_sim_gazebo.sim_scene import world_boxes
|
||||
from arm_sim_gazebo.robot_description import model_link_pose
|
||||
from arm_sim_gazebo.sim_site import resolve_world
|
||||
|
||||
specs = load_cameras(root=root)
|
||||
cam = next((s for s in specs if s.is_world_mounted), None)
|
||||
if cam is None:
|
||||
return [], "（cameras.yaml 里没有世界相机，跳过）"
|
||||
|
||||
part = next((o for o in scene.objects if o.name == scene.operating.part_object), None)
|
||||
if part is None:
|
||||
return [f"场景里没有 {scene.operating.part_object!r}，无法核对粗定位可见性"], ""
|
||||
|
||||
world_path, _ = resolve_world("", root)
|
||||
try:
|
||||
(px, py, pz), (rr, rp, ry) = model_link_pose(Path(world_path), cam.world_model, cam.world_link)
|
||||
except Exception as exc:
|
||||
return [f"读不出 {cam.world_model} 在世界里的位姿：{exc}"], ""
|
||||
|
||||
boxes = world_boxes(scene, part, root)
|
||||
if not boxes:
|
||||
return ["读不出工件的碰撞体，无法投影"], ""
|
||||
top = max(b[5] for b in boxes)
|
||||
x0 = min(b[0] for b in boxes)
|
||||
x1 = max(b[1] for b in boxes)
|
||||
y0 = min(b[2] for b in boxes)
|
||||
y1 = max(b[3] for b in boxes)
|
||||
corners = [(x0, y0, top), (x0, y1, top), (x1, y0, top), (x1, y1, top)]
|
||||
|
||||
w, h = cam.width or 640, cam.height or 480
|
||||
k, _ = intrinsics(cam, w, h)
|
||||
fx, fy, cx, cy = k[0], k[4], k[2], k[5]
|
||||
|
||||
R_wb = rpy_to_R(rr, rp, ry)
|
||||
# 本体 → 光学：相机本体是"X 朝前/Y 左/Z 上"，光学系是"Z 前/X 右/Y 下"
|
||||
R_bo = rpy_to_R(-math.pi / 2, 0.0, -math.pi / 2)
|
||||
R_wo = matmul3(R_wb, R_bo)
|
||||
R_ow = transpose3(R_wo)
|
||||
t = [px, py, pz]
|
||||
|
||||
probs: list[str] = []
|
||||
us, vs = [], []
|
||||
for c in corners:
|
||||
d = [c[i] - t[i] for i in range(3)]
|
||||
o = matvec3(R_ow, d)
|
||||
if o[2] <= 1e-6:
|
||||
probs.append(
|
||||
f"工件有一个角在 {cam.ros_ns} 的**背后**（光学系 z={o[2]:.3f}）——"
|
||||
f"相机朝向反了或工件跑到相机后面了。"
|
||||
)
|
||||
return probs, ""
|
||||
us.append(cx + fx * o[0] / o[2])
|
||||
vs.append(cy + fy * o[1] / o[2])
|
||||
|
||||
u0, u1, v0, v1 = min(us), max(us), min(vs), max(vs)
|
||||
info = (f"{cam.ros_ns}: 工件顶面四角投影 u∈[{u0:.0f},{u1:.0f}] v∈[{v0:.0f},{v1:.0f}] "
|
||||
f"（画面 {w}x{h}）")
|
||||
if u0 < 0 or u1 > w or v0 < 0 or v1 > h:
|
||||
probs.append(
|
||||
f"工件没有完整落在 {cam.ros_ns} 画面内（{info}）。\n"
|
||||
f" 粗定位会框不全工件 → bbox 中心偏 → **照样出结果但结果是错的**。\n"
|
||||
f" 修法：挪工件（scene.yaml 的 pose）、挪相机（world 里的 pose），"
|
||||
f"或确认这台相机的视野本来就该覆盖整个台面。"
|
||||
)
|
||||
return probs, info
|
||||
|
||||
|
||||
def check_gazebo_poses(scene, root) -> tuple[list[str], str]:
|
||||
"""查 Gazebo 里物体的实际位姿，与 scene.yaml 对照。
|
||||
|
||||
这一步能抓到"spawn 了但掉下去/被顶飞"这类只在运行后暴露的问题。
|
||||
需要仿真在跑；不在跑就跳过（不算失败）。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_scene import parse_spawned_poses, pose_mismatches
|
||||
|
||||
try:
|
||||
r = subprocess.run(
|
||||
["ign", "model", "--list"],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=20,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired) as exc:
|
||||
return [], f"（跳过：调不动 ign（{exc}））"
|
||||
if r.returncode != 0 or "Requesting state" not in (r.stdout + r.stderr):
|
||||
return [], "（跳过：Gazebo 没在跑）"
|
||||
|
||||
actual = {}
|
||||
for obj in scene.objects:
|
||||
try:
|
||||
rr = subprocess.run(
|
||||
["ign", "model", "-m", obj.name, "-p"],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=20,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
continue
|
||||
got = parse_spawned_poses(rr.stdout + rr.stderr)
|
||||
if obj.name in got:
|
||||
actual[obj.name] = got[obj.name]
|
||||
if not actual:
|
||||
return [], "（跳过：解析不出 Gazebo 的位姿输出）"
|
||||
return pose_mismatches(scene, actual), f"（已在 Gazebo 里核对 {len(actual)} 个物体）"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(
|
||||
description="仿真场景验收：摆放 + 孔检测成像几何 + 粗定位可见性。",
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
)
|
||||
ap.add_argument("--scene", default="", help="场景文件路径（留空=仓库默认）")
|
||||
ap.add_argument("--no-poses", action="store_true", help="跳过「查 Gazebo 实际位姿」")
|
||||
args = ap.parse_args()
|
||||
|
||||
try:
|
||||
from arm_sim_gazebo.sim_scene import SceneError, load_scene
|
||||
except ImportError as exc:
|
||||
print(
|
||||
f"✘ 导入 arm_sim_gazebo 失败：{exc}\n"
|
||||
f" 先执行： source scripts/sim_env.sh && . install_sim/setup.bash",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
root = REPO_ROOT
|
||||
try:
|
||||
scene = load_scene(Path(args.scene) if args.scene else None, root=root)
|
||||
except SceneError as exc:
|
||||
print(f"✘ 场景配置读不了：\n{exc}", file=sys.stderr)
|
||||
return 2
|
||||
|
||||
print(f"场景文件: {scene.source}")
|
||||
print(f"模型目录: {scene.models_dir} 台面顶面 z={scene.table_top_z}")
|
||||
print("物体:")
|
||||
for o in scene.objects:
|
||||
print(f" {o.name:<14} model={o.model:<14} pose=({o.x:+.3f}, {o.y:+.3f}, {o.z:+.3f}) yaw={o.yaw:+.3f}")
|
||||
|
||||
problems: list[str] = []
|
||||
|
||||
print("\n[1] 摆放校验（悬空/穿模/出界/嵌入）")
|
||||
p = check_placement(scene, root)
|
||||
problems += p
|
||||
print(" ✓ 通过" if not p else "\n".join(f" ✘ {x}" for x in p))
|
||||
|
||||
print("\n[2] 像素门槛与真机配置核对")
|
||||
p = check_gate_drift(scene, root)
|
||||
problems += p
|
||||
print(" ✓ 与真机生效值一致" if not p else "\n".join(f" ✘ {x}" for x in p))
|
||||
|
||||
print("\n[3] 孔检测成像几何")
|
||||
p, px = check_geometry(scene, root)
|
||||
problems += p
|
||||
for k, v in sorted(px.items()):
|
||||
gate = scene.gates_px.get(k)
|
||||
tag = f"门槛 [{gate[0]:g}, {gate[1]:g}]" if gate else "无门槛"
|
||||
ok = "" if (gate is None or gate[0] <= v <= gate[1]) else " ← 不合格"
|
||||
print(f" {k:<22} = {v:7.1f}px {tag}{ok}")
|
||||
if p:
|
||||
print("\n".join(f" ✘ {x}" for x in p))
|
||||
|
||||
print("\n[4] 二阶段悬停高度 vs 工作空间")
|
||||
p = check_hover_height(scene, root)
|
||||
problems += p
|
||||
from arm_sim_gazebo.sim_scene import operating_flange_z
|
||||
|
||||
fz = operating_flange_z(scene, root)
|
||||
lim = workspace_z_limits(root)
|
||||
if fz is not None:
|
||||
print(f" 建议悬停法兰 z≈{fz:.3f}m"
|
||||
+ (f" 工作空间 z∈[{lim[0]}, {lim[1]}]" if lim else ""))
|
||||
print(
|
||||
" （真机 site.yaml 写的是 0.23 —— 两者不同是已知的："
|
||||
"决定成像的是相机到工件的距离，不是绝对高度）"
|
||||
)
|
||||
if p:
|
||||
print("\n".join(f" ✘ {x}" for x in p))
|
||||
|
||||
print("\n[5] 粗定位可见性（工件是否完整落在手外相机画面内）")
|
||||
p, info = check_d435_visibility(scene, root)
|
||||
problems += p
|
||||
if info:
|
||||
print(f" {info}")
|
||||
if p:
|
||||
print("\n".join(f" ✘ {x}" for x in p))
|
||||
elif info and not info.startswith("（"):
|
||||
print(" ✓ 完整可见")
|
||||
|
||||
if not args.no_poses:
|
||||
print("\n[6] Gazebo 实际位姿核对")
|
||||
p, info = check_gazebo_poses(scene, root)
|
||||
problems += p
|
||||
if info:
|
||||
print(f" {info}")
|
||||
print(" ✓ 一致" if not p else "\n".join(f" ✘ {x}" for x in p))
|
||||
|
||||
print()
|
||||
if problems:
|
||||
print(f"=== ✘ 不通过：{len(problems)} 个问题 ===")
|
||||
print(" 场景与成像的问题大多是静默的：跑得起来、报不出错，只是结果不对。")
|
||||
return 1
|
||||
print("=== ✓ 全部通过 ===")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
Executable
+155
@@ -0,0 +1,155 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# scripts/stop_sim_gz.sh —— 干净地停掉 Gazebo 仿真栈
|
||||
#
|
||||
# 为什么要专门写一个脚本，而不是让你敲 pkill：
|
||||
#
|
||||
# ⚠️ `pkill -f <模式>` 在"用脚本启动仿真"的场景下会**把自己的启动壳一起杀掉**。
|
||||
# 因为那条命令自己的命令行里就含那个模式字符串，pkill -f 匹配整条命令行，
|
||||
# 于是它先匹配到自己所在的 shell。表现是：命令返回得飞快、什么输出都没有、
|
||||
# 仿真也没停成 —— 而这看起来像脚本写错了，极难往"进程匹配"上想。
|
||||
#
|
||||
# 本脚本的做法：扫描候选 PID → 排除自己/祖先/扫描自身的临时进程 → kill，
|
||||
# 最后**再扫一遍复查**（杀掉后仍有残留就报出来）。
|
||||
# 所以它可以在任何调用链里安全使用（包括被其它脚本 `source`/调用）。
|
||||
#
|
||||
# 用法：
|
||||
# bash scripts/stop_sim_gz.sh # 停仿真（TERM，等 5s，仍在则 KILL）
|
||||
# bash scripts/stop_sim_gz.sh --list # 只列出会被停掉的进程，不动手
|
||||
# =============================================================================
|
||||
set -uo pipefail
|
||||
|
||||
LIST_ONLY=0
|
||||
[ "${1:-}" = "--list" ] && LIST_ONLY=1
|
||||
|
||||
# 这些模式覆盖仿真栈的全部进程：
|
||||
# gz sim / ign gazebo 服务端与 GUI（conda 的 Fortress 里可执行名是 ign gazebo）
|
||||
# gazebo_sim.launch.py ros2 launch 的壳
|
||||
# gazebo_sim_launcher 入口可执行文件（展开描述后转交 launch）
|
||||
# parameter_bridge 相机/触觉/clock 的桥接
|
||||
# sim_camera_adapter / sim_tactile_adapter 两个适配器
|
||||
# robot_state_publisher 发 TF 与 /robot_description
|
||||
# controller_manager.*spawner / gz_ros2_control 控制器
|
||||
#
|
||||
# ⚠️ 这里漏过一个（robot_state_publisher），症状是"脚本说已停止，但它还在跑"：
|
||||
# 下一次启动时它继续发上一份 /robot_description 与 TF，而两个进程的
|
||||
# 参数可能不同 —— 表现成"改了描述却不生效"，极难往"有个残留进程"上想。
|
||||
# 所以脚本末尾加了一次**复查**：杀完再扫一遍，有残留就报出来并非零退出。
|
||||
# 加进新进程时，务必确认复查也能扫到它。
|
||||
#
|
||||
# 有意**不**包含 rviz2：本仿真栈的 headless 模式不起 RViz，而用户可能同时
|
||||
# 开着 RViz 看别的东西。把它写进来会误杀无关进程，比漏杀更糟。
|
||||
PATTERNS='ign gazebo|gz sim|gazebo_sim\.launch\.py|gazebo_sim_launcher|parameter_bridge|sim_camera_adapter|sim_tactile_adapter|robot_state_publisher|controller_manager.*spawner|gz_ros2_control'
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 扫描：输出 "PID<TAB>命令行"，供两次调用（收集 / 复查）共用。
|
||||
#
|
||||
# 两处必须共用同一个实现，否则"复查用的模式"和"杀的时候用的模式"会各自漂移，
|
||||
# 而漂移的表现就是"说已停止、其实有残留"——正是这个脚本要防的事。
|
||||
#
|
||||
# ⚠️ 模式串用**环境变量**传给 awk，不用 `-v pat=...`：
|
||||
# 用了 -v 的话，awk 自己的命令行里就含整串模式，于是它匹配到自己。
|
||||
# 这是 pkill -f 那个坑的同款，只是换了个位置 —— 实测表现为每次复查都会
|
||||
# 多报出一个"没有命令行"的幽灵 PID（awk 已经退出，ps 查不到它了）。
|
||||
#
|
||||
# 另外用 ps 而不是 pgrep -f：ps 的输出里可以用 awk 精确控制字段与排除规则。
|
||||
scan_pids() {
|
||||
# ⚠️ 必须 export：`VAR=x ps ... | awk` 只会把 VAR 给 ps，awk 看不到。
|
||||
export PAT="$PATTERNS"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）：跳过
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（进程替换 fork 出的子 shell）
|
||||
if (args ~ /stop_sim_gz\.sh/) next # 本脚本自己 / 调用它的壳的文本
|
||||
if (args ~ /^\s*awk/) next # 扫描用的 awk 本身
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
export ME=$$
|
||||
|
||||
# 排除自己与全部祖先 —— 这是本脚本存在的核心理由（见文件头）。
|
||||
exclude=()
|
||||
p=$$
|
||||
while [ "$p" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
|
||||
# 从扫描结果里剔掉自己/祖先，只输出 PID。
|
||||
# 用 `cut -f1` 而不是 read 到数组再拼接，避免命令行里再出现长的进程描述。
|
||||
filter_targets() {
|
||||
local pid
|
||||
while read -r pid _rest; do
|
||||
[ -z "$pid" ] && continue
|
||||
local skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
echo "$pid"
|
||||
done
|
||||
}
|
||||
|
||||
mapfile -t SCAN < <(scan_pids)
|
||||
mapfile -t targets < <(printf '%s\n' "${SCAN[@]:-}" | filter_targets)
|
||||
|
||||
if [ "${#targets[@]}" = "0" ]; then
|
||||
echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
echo "将停止以下进程："
|
||||
for pid in "${targets[@]}"; do
|
||||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
|
||||
done
|
||||
|
||||
if [ "$LIST_ONLY" = "1" ]; then
|
||||
echo "（--list：未动手）"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
for pid in "${targets[@]}"; do
|
||||
kill -TERM "$pid" 2>/dev/null
|
||||
done
|
||||
|
||||
# 给 HAL 一点时间退出。Gazebo 的渲染线程与 DART 的求解线程不会立刻收尾，
|
||||
# 立刻 KILL 有可能留下共享内存段，下一次启动表现成"渲染初始化失败"。
|
||||
for _ in $(seq 1 10); do
|
||||
alive=0
|
||||
for pid in "${targets[@]}"; do
|
||||
kill -0 "$pid" 2>/dev/null && alive=1
|
||||
done
|
||||
[ "$alive" = "0" ] && break
|
||||
sleep 0.5
|
||||
done
|
||||
|
||||
for pid in "${targets[@]}"; do
|
||||
if kill -0 "$pid" 2>/dev/null; then
|
||||
echo " PID $pid 没响应 TERM，用 KILL"
|
||||
kill -KILL "$pid" 2>/dev/null
|
||||
fi
|
||||
done
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 复查：杀完再扫一遍。
|
||||
# 这一步是为了让"模式漏了某个进程"变成**看得见的告警**，而不是
|
||||
# "脚本说已停止、实际有残留"。robot_state_publisher 就这么漏过一次。
|
||||
# ---------------------------------------------------------------------------
|
||||
sleep 0.5
|
||||
mapfile -t SCAN2 < <(scan_pids)
|
||||
mapfile -t leftover < <(printf '%s\n' "${SCAN2[@]:-}" | filter_targets)
|
||||
|
||||
if [ "${#leftover[@]}" != "0" ]; then
|
||||
echo "⚠ 仍有仿真相关进程存活（说明上面的模式没覆盖它，请把它加进 PATTERNS）："
|
||||
for pid in "${leftover[@]}"; do
|
||||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
|
||||
done
|
||||
echo "仿真已停止，但有残留 —— 残留会与下一次启动抢话题/TF。"
|
||||
exit 3
|
||||
fi
|
||||
|
||||
echo "仿真已停止。"
|
||||
@@ -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