orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

P5c-6 场景物料落地 + Gazebo 中文手册：工件/通止规模型、场景校验、两处静默失效修复

Browse Source 
一、场景物料（新增）
  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
2026-09-23 17:13:00 +08:00
parent 01315773e7
commit d32e6ae53f
23 changed files with 4489 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+3 -1
View File
@@ -53,6 +53,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
| 双爪 ROS | `scripts/run_dual_gripper.sh` | | 双爪 ROS | `scripts/run_dual_gripper.sh` |
| 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` | | 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` |
| Orisys 触觉 | `scripts/run_tactile.sh [--cli]` | | Orisys 触觉 | `scripts/run_tactile.sh [--cli]` |
| Gazebo 仿真 | `scripts/run_sim_gz.sh [--gui]`（停：`scripts/stop_sim_gz.sh`） |
| 看配置 | `scripts/show_site_config.sh` | | 看配置 | `scripts/show_site_config.sh` |
89 与 91 **各有** 手眼、相机序列号、锁高、named poses，禁止混用。 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/GRIPPER.md](docs/GRIPPER.md) | PGEA + RGI |
| [docs/HOLE_DETECT.md](docs/HOLE_DETECT.md) | 入孔检测 | | [docs/HOLE_DETECT.md](docs/HOLE_DETECT.md) | 入孔检测 |
| [docs/TACTILE.md](docs/TACTILE.md) | Orisys 视触觉 | | [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/INTERFACES.md](docs/INTERFACES.md) | ROS / 脚本接口 |
| [docs/TRAJECTORIES.md](docs/TRAJECTORIES.md) | 轨迹回放 | | [docs/TRAJECTORIES.md](docs/TRAJECTORIES.md) | 轨迹回放 |
## 上传 git 时 ## 上传 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/` - **不要提交**：`config/site.yaml`（含本机 IP/序列号）、`handeye/tmp/`（临时产物）、`build/` `install/` `log/`、YOLO `.pt/.onnx/.engine`、`thirdparty/` `vendor/`
- 规则已写在 `.gitignore` - 规则已写在 `.gitignore`
config/site.example.yaml
+9 -4
View File
@@ -111,11 +111,15 @@ paths:
# bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640 # bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640
# 4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640） # 4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640）
# 5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control] # 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 输入） # 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入）
yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx" yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx"
# 备选模型：直接换成 models/yolo/ 下的其他 .onnx / .pt 绝对路径即可。 # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
# 换完建议确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
# 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用上面的工件权重。 # 入孔 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" hole_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。 # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml" 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 # bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
hole_detect: hole_detect:
backend: "two_stage" # two_stage | yolo | hough backend: "two_stage" # two_stage | yolo | hough
# backend=yolo 时用。原为 hole_insert/best.pt（文件不存在），已改为实际存在的模型。
weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx" weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
# 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔） # 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔）
workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx" workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
@@ -491,7 +496,7 @@ sim:
# 决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。 # 决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。
# 写了个不同的值会告警。要真改，三处必须一起改。 # 写了个不同的值会告警。要真改，三处必须一起改。
# #
# 另外注意：`run_sim.sh` 走的是 mock 驱动（假数据 + RViz），**不是**物理仿真； # 另外注意：`run_sim.sh` 走的仍是 mock 驱动（假数据 + RViz），不是物理仿真；
# 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。 # 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。
platform: "gazebo" platform: "gazebo"
world: "devices/sim/gazebo/worlds/assembly.world" world: "devices/sim/gazebo/worlds/assembly.world"
devices/sim/gazebo/README.md
+403 -87
View File
@@ -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.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）
### 已就绪
| 部分 | 位置 | 说明 |
|------|------|------|
| 运行环境 | `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` | 两台相机的**唯一真相**：话题、坐标系、视场 |
### 还没做
| 部分 | 缺什么 | 影响 |
|------|--------|------|
| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 |
| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） |
> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 +
> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据，
> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到
> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。
> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。
--- ---
## 第二部分：日常怎么用 ## 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
bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个） bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个）
bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI
bash scripts/run_sim_gz.sh --check # 只自检描述，不启动任何东西 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 # 最省资源，只验运动学 bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源，只验运动学
``` ```
首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。 首次启动约 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。启动日志末尾会打印一份
**验收命令清单**，照它做一遍就能确认仿真真的活了。
### 验收（★ 必做，别只看"有没有图像"） ### 1.3 验收（★ 必做，别只看"有没有图像"）
```bash ```bash
ros2 topic echo /clock --once # 仿真时间在走 ros2 topic echo /clock --once # 仿真时间在走
ros2 control list_controllers # 两个控制器 active ros2 control list_controllers # 两个控制器 active
ros2 topic echo /joint_states --once # 7 个 sim_joint 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_cameras.py # ★ 相机的真正验收
python3 scripts/check_sim_scene.py # ★ 场景摆放 + 孔检测几何验收
``` ```
**`check_sim_cameras.py` 才是相机的验收标准。** 它把深度反投影到 world， 机械臂应当**在重力下保持站住**。若它瘫下去或消失，说明描述有问题 ——
检查落点是否命中已知平面（地面 z=0、台面 z=0.40）。"有图像"证明不了什么 —— 见 [坑 1](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)。
详见下面的坑 4 与坑 5。
### 放置格式与命名要求 **两条 `check_*.py` 才是验收标准，"有图像"证明不了什么。**
它们做的判断分别是：
| 类型 | 放哪 | 格式 | 命名要求 | | 工具 | 它凭什么判"通过" | 它防的是哪一类错 |
|------|------|------|----------| |------|------------------|------------------|
| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` | | `check_sim_cameras.py` | 把深度**反投影**到 world，要求命中已知平面（地面 z=0、台面 z=0.40） | 深度单位差 1000 倍、`frame_id` 盖成本体系、内参是 gz 的错值、相机位姿朝向反了 —— 全都**能出图** |
| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） | | `check_sim_scene.py` | 摆放自洽 + 像素门槛与真机一致 + 孔检测特征的像素值达标 + 手外相机**完整**看得见工件 | 物体悬空/穿模/嵌入、场景里的门槛与真机漂移、成像尺度不够、粗定位框不全 |
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米**。用毫米建模的必须先缩放到米 |
| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用启动时的 `camera_name` |
### 改配置：改哪里才会真正生效 ### 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` > 内置默认） | | 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 优先级：`--world` > `site.yaml` > 内置默认 |
| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致会告警 | | 场景里有哪些物体、摆在哪 | `config/scene.yaml` | ✔ 启动时 spawn，并做摆放/像素校验 |
| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 里的 `<gravity>` | | 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致告警 |
| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。前缀由 `urdf/arm.sim.urdf.xacro` 的 `arm_prefix` 决定，且 `config/controllers.yaml` 硬编码了 `sim_joint1..7` —— 三处要一起改 | | 相机图幅（影响像素尺度） | `config/cameras.yaml` 的 `width`/`height` **且** SDF 的 `image_width/height` | ✔ 启动时自动比对（这是最容易漏的一处，见 4.3） |
| 仿真里的悬高/速度 | **复用真机那套参数**（`servo.*` / `hole_detect.stage2.*`） | ✔ | | 触觉标度/轴向/基线 | `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` 参数。任何"仿真专用参数"都是 > **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何"仿真专用参数"都是
> 未来"仿真调好的值搬到真机不生效"的隐患。只允许物理引擎自身的参数（重力、步长） > 未来"仿真里调好的值搬到真机不生效"的隐患。只允许**物理引擎自身的**参数
> 放在 `sim.*` 下。 > （重力、步长）放在 `sim.*` 下。
> >
> ✘ 标记的两个键由 `arm_sim_gazebo.sim_site.unwired_warnings()` 守着：只要你写的值 > ✘ 的两个键由 `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` 打印成功， 这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，但结果是错的。
但结果是错的。所以每条都配了"怎么发现"。 所以每条都写了"怎么发现"。
| # | 坑 | 症状 | 怎么发现 / 怎么修 | ### 坑 1：官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉
|---|----|------|-------------------|
| 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 真正渲染用的值一致。 7 轴本体缺 `<inertial>` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。
启动时会自动比对并告警（`sim_cameras.check_fov_consistency`）。 **只有 Gazebo 会暴露，RViz/MoveIt 完全正常。**
关于相机还有两条**规格差异**，是有意保留的（不是 bug，但会让人困惑）： → 启动时归一化（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check`。
- **畸变**：gz 不模拟镜头畸变（`d` 全 0，`R` 单位阵）。所以"仿真里用畸变矫正 ### 坑 2：world 里声明了任何 `<plugin>` 都会挤掉默认系统
调出来的参数"搬到真机仍需重调。
- **分辨率**：真机 D405 是 848x480、D435 是 1280x720，仿真建模用 640x480。 world 里只要有一个 `<plugin>`，Gazebo 就**不再**自动加载默认的
对"算法能不能跑通"没影响（内参从 `cameras.yaml` 自洽推导）， 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） |
| 世界 | `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 |
### 还没做
| 部分 | 缺什么 | 影响 |
|------|--------|------|
| 视触觉图像 | 仿真的深度图/力矢量图只能是**占位**（空/NaN） | `TactileStatus` 的图字段在仿真里不可信；三力是可信的 |
| 夹具/料框 | 目前只有工件、规、规座 | 上料流程（若有）没法验 |
| 夹爪服务适配器 | `~/open|~/close|~/move` 服务（mm ↔ 弧度换算） | 仿真里目前只能直接发关节指令开合，无法走真机那套服务接口 |
---
## 7. 有意保留的规格差异（不是 bug，但会让人困惑）
| 项 | 真机 | 仿真 | 后果 |
|----|------|------|------|
| 镜头畸变 | 有（`d` 非 0） | **无**（`d` 全 0、`R` 单位阵） | "仿真里用畸变矫正调出来的参数"搬到真机仍需重调 |
| D435 图幅 | 1280x720 | 640x480 | 像素尺度不同，`hole_detect` 的像素门槛搬过去要重算 |
| 触觉单位 | Orisys 形变任意单位（非牛顿） | 牛顿 × `force_scale` | 见 `config/tactile.yaml` 头部的"单位口径"；选错**不报错**，只是阈值不触发或一碰就炸 |
| 触觉图像 | 有真实深度图/力矢量图 | 占位 | 下游不用改代码，但图内容不可信 |
---
## 8. 文件一览
``` ```
devices/sim/gazebo/ devices/sim/gazebo/
├── manifest.yaml # 设备清单：进度、配置声明、放置要求 ├── manifest.yaml # 设备清单：进度、配置声明、放置要求（与本文档同源）
├── README.md # 本文档 ├── README.md # 本文档
├── config/ ├── config/
│ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场） │ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场/图幅）
│ └── controllers.yaml # 控制器配置（名字与真机一致） │ ├── controllers.yaml # 控制器配置（名字与真机一致）
├── urdf/ # 仿真用机器人描述（含 ros2_control 与传感器） │ ├── scene.yaml # ★ 场景布局（工件/规/规座）+ 像素门槛
├── worlds/assembly.world # 场景 │ └── tactile.yaml # ★ 触觉标定：单位口径、轴向映射（实测）、基线策略
├── models/ # ✘ 待做：工件/通止规/夹具 ├── urdf/
└── sensor_plugins/ # ✘ 待做：Orisys 触觉插件 │ ├── 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/`（启动器、描述归一化、相机适配器）。 配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器、触觉适配器、场景逻辑）
配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/check_sim_cameras.py`（相机验收）。 配套脚本：`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`）
devices/sim/gazebo/config/cameras.yaml
+8 -2
View File
@@ -36,7 +36,7 @@
# 因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。 # 因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。
# #
# 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。 # 所以每台相机必须声明 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/urdf/arm.sim.sensors.xacro 的 horizontal_fov
# 世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 <sensor> 的 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。 # 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。
horizontal_fov: 1.5184 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 height: 480
note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供 note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供
devices/sim/gazebo/config/scene.yaml
+146
View File
@@ -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 里有提示交互），仿真里先立在座上，
便于确认夹爪能不能够到、开合范围够不够。
devices/sim/gazebo/manifest.yaml
+48 -13
View File
@@ -3,7 +3,7 @@
# #
# Gazebo 仿真平台清单。 # Gazebo 仿真平台清单。
# #
# 进度（截至 P5c-4，如实记录，不要凭印象改）： # 进度（截至 P5c-6，如实记录，不要凭印象改）：
# ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble） # ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
# 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh # 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh
# ✔ 世界文件 worlds/assembly.world（sdformat 校验通过） # ✔ 世界文件 worlds/assembly.world（sdformat 校验通过）
@@ -14,8 +14,12 @@
# 起 Gazebo → 发归一化 URDF → spawn → 拉起两个控制器（active） # 起 Gazebo → 发归一化 URDF → spawn → 拉起两个控制器（active）
# 入口脚本 scripts/run_sim_gz.sh（此前缺失，被 install_sim_env.sh # 入口脚本 scripts/run_sim_gz.sh（此前缺失，被 install_sim_env.sh
# 与 build_sim.sh 的提示语引用，照做会报文件不存在） # 与 build_sim.sh 的提示语引用，照做会报文件不存在）
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做） # ✔ 场景模型 models/ 已有工件（带检测亮环的孔板）、通止规、规座
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做） # 布局在 config/scene.yaml，启动时动态 spawn 并做摆放/几何校验
# ✔ 触觉 **不需要自定义插件**：用 Gazebo 内置的 force_torque（三向力）
# + contact（接触布尔）传感器，由 sim_tactile_adapter 合成
# arm_msgs/TactileStatus。标定与轴向映射在 config/tactile.yaml
# （轴向是实测的，不是猜的）。视触觉图像只能是占位。
# ✔ 相机出图 D405/D435 已注册并桥接到 ROS，且**几何正确性已验证** # ✔ 相机出图 D405/D435 已注册并桥接到 ROS，且**几何正确性已验证**
# 桥接+适配：config/cameras.yaml → sim_cameras（纯逻辑： # 桥接+适配：config/cameras.yaml → sim_cameras（纯逻辑：
# 话题映射 / 深度换算 / 内参，单测覆盖） # 话题映射 / 深度换算 / 内参，单测覆盖）
@@ -24,7 +28,9 @@
# 实测：d435 台面 11.9% / 地面 75.3%；d405 台面 37.8% / 地面 54.0% # 实测：d435 台面 11.9% / 地面 75.3%；d405 台面 37.8% / 地面 54.0%
# ✔ 配置接线 sim.world 现在真的从 config/site.yaml 读（此前硬编码，改了无效） # ✔ 配置接线 sim.world 现在真的从 config/site.yaml 读（此前硬编码，改了无效）
# #
# 所以：D405/D435 可用，触觉还要等 P5c-5，装配状态机（P6）依赖它。 # 所以：相机、触觉三力、场景物体都可用，装配状态机（P6）可以在这里验几何与接触。
# 仍然做不到的：视触觉的**图像**（深度图/力矢量图只有占位），以及走真机那套
# 夹爪服务接口（仿真里目前直接发关节指令）。
# #
# 实测踩到的坑（都属于"静默失效"，后续改仿真务必先看这几条）： # 实测踩到的坑（都属于"静默失效"，后续改仿真务必先看这几条）：
# 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺 # 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
@@ -49,6 +55,22 @@
# d435 就踩了这个坑：相机背对台面、只看得见地面，却什么都不报， # d435 就踩了这个坑：相机背对台面、只看得见地面，却什么都不报，
# 而且几何自洽（点确实落在真实地面上），只查数值发现不了。 # 而且几何自洽（点确实落在真实地面上），只查数值发现不了。
# 已修正并加回归测试（test_robot_description 的 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 id: gazebo
@@ -63,11 +85,14 @@ description: >
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证 物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。 （起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
D405/D435 已桥接到 ROS，且几何正确性有自动验收（scripts/check_sim_cameras.py）。 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 尚未接线， sim.world 已真正从 config/site.yaml 读取（physics_enable / joint_prefix 尚未接线，
写了不一致的值会告警而非静默忽略，见文件头 config_schema）。 写了不一致的值会告警而非静默忽略，见文件头 config_schema）。
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前， 手册见 devices/sim/gazebo/README.md（含建模要求、导入格式、配置方式、避坑清单）。
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
# --- 物料放置要求 ---------------------------------------------------------- # --- 物料放置要求 ----------------------------------------------------------
assets: assets:
@@ -81,11 +106,18 @@ assets:
- path: models - path: models
required: false required: false
what: 工件 / 夹具 / 通止规模型 what: 工件 / 夹具 / 通止规模型
format: 每个模型一个子目录，内含 <模型名>.sdf 与 meshes/ format: >
每个模型一个子目录，内含 model.sdf 与 model.config（两个文件名都是固定的，
Gazebo 按这个约定查找）。原点必须在**底面中心**，这样 scene.yaml 里的
pose.z 就等于"放在台面上的高度"。碰撞体用 box/cylinder 等简单几何，
视觉可以额外加只服务于算法的细节（如工件上的检测亮环）。
naming: > naming: >
模型名必须与推理用的类别名一致（如 workpiece），否则仿真里算出的坐标 目录名 = SDF 里的 <model name> = model.config 里的 <name> = 推理用的类别名
与推理结果对不上。网格单位必须是米，用毫米建模的必须先缩放。 （如 workpiece），四者必须一致，否则仿真里算出的坐标与推理结果对不上。
where_used: Gazebo 场景加载 网格单位必须是米，用毫米建模的必须先缩放。
where_used: >
由 config/scene.yaml 的 objects[].model 引用，启动时动态 spawn；
摆放合法性与成像几何由 scripts/check_sim_scene.py 验收。
- path: urdf - path: urdf
required: false required: false
@@ -96,10 +128,13 @@ assets:
- path: sensor_plugins - path: sensor_plugins
required: false required: false
what: 传感器插件（含 Orisys 触觉自定义插件） what: 传感器插件（自定义 C++ 插件用；当前**为空**）
format: 插件源码 + CMakeLists.txt format: 插件源码 + CMakeLists.txt
naming: 插件类名以 Orisys 开头 naming: 插件类名以 Orisys 开头
where_used: 仿真传感器数据发布 where_used: >
目前没有使用者：触觉走 Gazebo 内置的 force_torque + contact 传感器，
由 sim_tactile_adapter（Python）合成 TactileStatus，无需自研插件。
保留此目录用于"以后真需要写插件"的场景（如自研视触觉弹性体成像）。
# --- 适配器 ---------------------------------------------------------------- # --- 适配器 ----------------------------------------------------------------
driver: driver:
devices/sim/gazebo/models/gauge/model.config
+28
View File
@@ -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>
devices/sim/gazebo/models/gauge/model.sdf
+193
View File
@@ -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>
devices/sim/gazebo/models/gauge_stand/model.config
+22
View File
@@ -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>
devices/sim/gazebo/models/gauge_stand/model.sdf
+107
View File
@@ -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>
devices/sim/gazebo/models/workpiece/model.config
+30
View File
@@ -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>
devices/sim/gazebo/models/workpiece/model.sdf
+325
View File
@@ -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>
devices/sim/gazebo/urdf/arm.sim.sensors.xacro
+29 -1
View File
@@ -47,11 +47,39 @@
d405_roll:=0.0 d405_roll:=0.0
d405_pitch:=0.0 d405_pitch:=0.0
d405_yaw:=0.0 d405_yaw:=0.0
image_width:=640 image_width:=848
image_height:=480 image_height:=480
image_hz:=30 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 文件**里，不放这里 传感器系统插件放在 **world 文件**里，不放这里
======================================================================= =======================================================================
devices/sim/gazebo/worlds/assembly.world
+15 -19
View File
@@ -97,10 +97,14 @@
<!-- ========================================================================= <!-- =========================================================================
工件台 工件台
========================================================================= 100| =========================================================================
高度 0.75m 是常见工作台高度。工件就放在台面上（P5c-6 里会加）， 台面顶面在 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：台子本身不该被机械臂推着走。 台面做成 static：台子本身不该被机械臂推着走。
摩擦系数给足（0.8），否则柔顺下压时工件会被顶得滑动， 摩擦系数给足（0.8），否则柔顺下压时工件会被顶得滑动，
@@ -149,25 +153,17 @@
</model> </model>
<!-- ========================================================================= <!-- =========================================================================
通止规起始位（留给人工/机械手放置用的标记区） 工件 / 通止规 —— **不在这里**
========================================================================= =========================================================================
真机上通止规是被人放到夹爪里的（P6 里有"提示用户放置"的交互）。 它们**不进 world 文件**，而是在启动时由 gazebo_sim.launch.py 用
仿真里这里放一个视觉标记，方便对着它调整"返回极值角是否到位"。 `ros_gz_sim create` spawn（位姿在 devices/sim/gazebo/config/scene.yaml）。
真正会被夹爪抓取的通止规模型在 P5c-6 里加到这个位置。 分工：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>
<!-- ========================================================================= <!-- =========================================================================
基础系统插件 —— 必须显式声明，否则整个仿真静默失效 基础系统插件 —— 必须显式声明，否则整个仿真静默失效
scripts/README.md
+5
View File
@@ -15,10 +15,15 @@
- 规则写在 [models/README.md](../models/README.md)。`export_yolo_trt.sh` 现在是它的兼容转发壳。 - 规则写在 [models/README.md](../models/README.md)。`export_yolo_trt.sh` 现在是它的兼容转发壳。
**配置：** `show_site_config.sh` · `lib/export_site_env.py` **配置：** `show_site_config.sh` · `lib/export_site_env.py`
**仿真：** `run_sim_gz.sh`（Gazebo 物理仿真，唯一能做接触/力控验证的入口） **仿真：** `run_sim_gz.sh`（Gazebo 物理仿真，唯一能做接触/力控验证的入口）
停：`stop_sim_gz.sh`（**别敲 `pkill -f gz`** —— 会杀掉调用它的 shell，见脚本文件头）
- 环境是独立 conda 环境 `ros_gz_sim`（`install_sim_env.sh` 装、`sim_env.sh` 激活），与真机环境隔离 - 环境是独立 conda 环境 `ros_gz_sim`（`install_sim_env.sh` 装、`sim_env.sh` 激活），与真机环境隔离
- 相机验收：`python3 scripts/check_sim_cameras.py` —— **别用 `ros2 topic hz` 或肉眼看图验收相机**， - 相机验收：`python3 scripts/check_sim_cameras.py` —— **别用 `ros2 topic hz` 或肉眼看图验收相机**，
仿真相机的错误几乎全是静默的（深度单位、frame_id、内参、位姿方向），都能出图却是错的。 仿真相机的错误几乎全是静默的（深度单位、frame_id、内参、位姿方向），都能出图却是错的。
这条命令把深度反投影到 world，要求命中已知平面（地面/台面）才算通过。 这条命令把深度反投影到 world，要求命中已知平面（地面/台面）才算通过。
- 场景验收：`python3 scripts/check_sim_scene.py` —— 同样别靠肉眼。它管四件事：
物体的摆放（悬空/穿模/出界/互相嵌入）、场景里的像素门槛与**真机生效值**是否漂移、
孔检测的成像几何（亮环/孔间距换算成像素后够不够门槛）、以及粗定位用的手外相机
能不能**完整**看见工件。第二条尤其重要：门槛抄了两份，不一致时仿真会"通过"而真机不认。
- 注意 `run_sim.sh` 不是 Gazebo，它走 mock 驱动（假数据 + RViz），永远不会产生接触力 - 注意 `run_sim.sh` 不是 Gazebo，它走 mock 驱动（假数据 + RViz），永远不会产生接触力
**环境：** `env.sh` · `build_all.sh` **环境：** `env.sh` · `build_all.sh`
**RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh` · `rdk_drag_bridge.py` **RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh` · `rdk_drag_bridge.py`
scripts/check_sim_scene.py
+438
View File
@@ -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())
scripts/stop_sim_gz.sh
Executable
+155
View File
@@ -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 "仿真已停止。"
src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py
+72 -6
View File
@@ -239,25 +239,32 @@ def intrinsics(spec: CameraSpec, width: int, height: int) -> Tuple[List[float],
return k, p return k, p
def check_fov_consistency( def check_camera_consistency(
specs: Sequence[CameraSpec], specs: Sequence[CameraSpec],
world_file: Optional[Path] = None, world_file: Optional[Path] = None,
sensors_xacro: Optional[Path] = None, sensors_xacro: Optional[Path] = None,
) -> List[str]: ) -> List[str]:
"""比对 cameras.yaml 里声明的视场与 SDF 里真正渲染用的视场。 """比对 cameras.yaml 的声明与 SDF 里真正渲染用的值（视场 + 图幅）。
为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 算出， 为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 与 width/height
而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 —— 算出，而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 ——
症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头 症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头
那一长串说明所警告的那类错误，所以要主动比对。 那一长串说明所警告的那类错误，所以要主动比对。
⚠️ 图幅（width/height）为什么必须一起比：它决定 fx = W/(2·tan(hfov/2))，
也就是"每毫米多少像素"。阶段二的孔检测在原始分辨率图上跑 HoughCircles，
hole_detect.yaml 的像素门槛（hole_distance_min 等）只在图幅与真机相同时
才有意义。图幅写错不会报任何错，只会让孔检测"莫名其妙不认孔" ——
比视场写错更难查（视场错还会让点位整体偏，图幅错只是尺度不对）。
返回人类可读的告警列表（空 = 没问题）。 返回人类可读的告警列表（空 = 没问题）。
精度说明（有意为之，不做过度解析）： 精度说明（有意为之，不做过度解析）：
- 世界相机：按 <sensor> 里的 <topic> 精确匹配到对应相机，读到的是精确值。 - 世界相机：按 <sensor> 里的 <topic> 精确匹配到对应相机，读到的是精确值。
- 腕上相机：它在 xacro 宏里、值由参数传入，展开前无法可靠地归属到具体 - 腕上相机：它在 xacro 宏里、值由参数传入，展开前无法可靠地归属到具体
相机。所以退化成"文件里出现过这个值就算过"—— 偏宽松，但足以抓住 相机。所以视场退化成"文件里出现过这个值就算过"，图幅则读宏的
"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。 **参数默认值**（image_width:=/image_height:=，launch 不会覆盖它）——
偏宽松，但足以抓住"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。
""" """
warnings: List[str] = [] warnings: List[str] = []
@@ -268,6 +275,7 @@ def check_fov_consistency(
continue continue
# 找到 <topic> 等于本相机 gz_ns 的那个 <sensor> 块 # 找到 <topic> 等于本相机 gz_ns 的那个 <sensor> 块
fov = None fov = None
size = (None, None)
for m in re.finditer(r"<sensor\b[^>]*>(.*?)</sensor>", text, re.S): for m in re.finditer(r"<sensor\b[^>]*>(.*?)</sensor>", text, re.S):
body = m.group(1) body = m.group(1)
tm = re.search(r"<topic>\s*([^<\s]+)\s*</topic>", body) 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) fm = re.search(r"<horizontal_fov>\s*([0-9.eE+-]+)\s*</horizontal_fov>", body)
if fm: if fm:
fov = float(fm.group(1)) 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 break
if fov is None: if fov is None:
warnings.append( warnings.append(
@@ -291,6 +305,7 @@ def check_fov_consistency(
f" 内参按前者算、图像按后者渲染 → 三维点位会整体错，" f" 内参按前者算、图像按后者渲染 → 三维点位会整体错，"
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(): if sensors_xacro is not None and Path(sensors_xacro).is_file():
text = Path(sensors_xacro).read_text(encoding="utf-8", errors="replace") 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"{s.horizontal_fov}，而 {Path(sensors_xacro).name} 里出现的值是 "
f"{found}。若渲染用的不是 {s.horizontal_fov}，三维点位会整体错。" 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 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]: def bridge_args(specs: Sequence[CameraSpec]) -> List[str]:
"""生成 ros_gz_bridge parameter_bridge 的话题规则。 """生成 ros_gz_bridge parameter_bridge 的话题规则。
src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py
+851
View File
@@ -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)
src/arm_sim_gazebo/launch/gazebo_sim.launch.py
+113 -7
View File
@@ -87,9 +87,15 @@ def _prepare(context, *args, **kwargs):
world = LaunchConfiguration("world").perform(context).strip() world = LaunchConfiguration("world").perform(context).strip()
load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower() load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower()
with_sensors = LaunchConfiguration("with_sensors").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() headless = LaunchConfiguration("headless").perform(context).strip().lower()
verbose = LaunchConfiguration("verbose").perform(context).strip().lower() verbose = LaunchConfiguration("verbose").perform(context).strip().lower()
# 相机清单在下面的传感器分支里才读，但场景分支（成像几何自检）也要用它。
# 先置空再赋值，避免"因为分支没进而在别处 NameError"这种低级但难查的错误。
specs = []
try: try:
root = repo_root() root = repo_root()
# 与 gazebo_sim_launcher 用同一套优先级，直接 `ros2 launch` 的人也不会 # 与 gazebo_sim_launcher 用同一套优先级，直接 `ros2 launch` 的人也不会
@@ -194,7 +200,7 @@ def _prepare(context, *args, **kwargs):
DEFAULT_CAMERAS_REL, DEFAULT_CAMERAS_REL,
CameraConfigError, CameraConfigError,
bridge_args, bridge_args,
check_fov_consistency, check_camera_consistency,
load_cameras, load_cameras,
rpy_to_quat, rpy_to_quat,
) )
@@ -204,12 +210,13 @@ def _prepare(context, *args, **kwargs):
except CameraConfigError as exc: except CameraConfigError as exc:
raise RuntimeError(f"\n[gazebo_sim] 仿真相机清单有问题：\n{exc}\n") from exc raise RuntimeError(f"\n[gazebo_sim] 仿真相机清单有问题：\n{exc}\n") from exc
# 视场一致性：内参由 cameras.yaml 的 horizontal_fov 算，而**渲染**用的是 # 一致性：内参由 cameras.yaml 的 horizontal_fov 与 width/height 算，
# SDF 里的值。两者不一致时图像与内参对不上，三维点位会整体错 —— # 而**渲染**用的是 SDF 里的值。两者不一致时图像与内参对不上，
# 而单目预览完全看不出来（gz 自己发的 camera_info 本来就是错的， # 三维点位会整体错 —— 而单目预览完全看不出来（gz 自己发的
# 所以这份内参是"唯一真相"，更要保证它与渲染一致）。 # camera_info 本来就是错的，所以这份内参是"唯一真相"，更要保证它与渲染一致）。
# 图幅尤其要查：它决定 fx 的尺度，写错会让孔检测的像素门槛静默失效。
# 这里只告警不拦截：真有偏差时，仍应能起仿真去现场比对。 # 这里只告警不拦截：真有偏差时，仍应能起仿真去现场比对。
_fov_issues = check_fov_consistency( _cam_issues = check_camera_consistency(
specs, specs,
world_file=Path(world_path), world_file=Path(world_path),
sensors_xacro=Path(root) sensors_xacro=Path(root)
@@ -219,7 +226,7 @@ def _prepare(context, *args, **kwargs):
# LogWarn（Iron 才加），照抄新版本会直接 ImportError 让整个 launch # LogWarn（Iron 才加），照抄新版本会直接 ImportError 让整个 launch
# 加载失败 —— 一个"只是想说句话"的动作把仿真起不来，代价太大。 # 加载失败 —— 一个"只是想说句话"的动作把仿真起不来，代价太大。
# 前缀里带 ⚠ 保证它在满屏日志里仍然显眼。 # 前缀里带 ⚠ 保证它在满屏日志里仍然显眼。
for _w in _fov_issues: for _w in _cam_issues:
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ {_w}")) actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ {_w}"))
# 4a) 世界相机的 TF 兜底 # 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) 控制器 ----------------------------------------------------------- # --- 6) 控制器 -----------------------------------------------------------
# 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按 # 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
# rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。 # 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 topic echo /joint_states --once # 7 个 sim_joint\n"
" ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查\n" " ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查\n"
" ros2 topic echo /tactile/left/status --once # 触觉三力在发\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_cameras.py # ★ 相机的真正验收\n"
" python3 scripts/check_sim_scene.py # ★ 场景摆放 + 孔检测几何验收\n"
" 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，" " 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n" "参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n"
" ★ 别用 'ros2 topic hz' 或肉眼预览来验收相机 —— 那些只能证明'有图像'。\n" " ★ 别用 'ros2 topic hz' 或肉眼预览来验收相机 —— 那些只能证明'有图像'。\n"
@@ -445,6 +541,16 @@ def generate_launch_description():
default_value="true", default_value="true",
description="是否挂 D405 与触觉。关掉跑得更快，用于纯运动学验证", description="是否挂 D405 与触觉。关掉跑得更快，用于纯运动学验证",
), ),
DeclareLaunchArgument(
"with_scene",
default_value="true",
description="是否 spawn 场景物料（工件/通止规）。false=只用空台面",
),
DeclareLaunchArgument(
"scene",
default_value="",
description="场景配置文件路径。留空=用 devices/sim/gazebo/config/scene.yaml",
),
DeclareLaunchArgument( DeclareLaunchArgument(
"headless", "headless",
default_value="true", default_value="true",
src/arm_sim_gazebo/test/test_check_sim_scene.py
+407
View File
@@ -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)
src/arm_sim_gazebo/test/test_sim_cameras.py
+120 -18
View File
@@ -321,10 +321,10 @@ cameras:
assert "horizontal_fov" in str(ei.value) 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 不一致。""" """世界相机：按 <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 = tmp_path / "assembly.world"
world.write_text( world.write_text(
@@ -332,7 +332,8 @@ def test_fov_check_flags_world_camera_mismatch(tmp_path):
<sdf><world name="w"> <sdf><world name="w">
<model name="d435"><link name="link"><sensor name="s" type="rgbd_camera"> <model name="d435"><link name="link"><sensor name="s" type="rgbd_camera">
<topic>/d435</topic> <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> </sensor></link></model>
</world></sdf> </world></sdf>
""", """,
@@ -346,31 +347,132 @@ def test_fov_check_flags_world_camera_mismatch(tmp_path):
horizontal_fov=1.5184, horizontal_fov=1.5184,
world_model="d435", world_model="d435",
world_link="link", 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}) 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] 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 里，只能"文件里出现过才算过"，但要能抓到缺失。""" """腕上相机：视场在 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 = tmp_path / "arm.sim.sensors.xacro"
xacro.write_text("<horizontal_fov>1.5184</horizontal_fov>", encoding="utf-8") xacro.write_text(
spec = _spec() "<horizontal_fov>1.5184</horizontal_fov>"
assert check_fov_consistency([spec], sensors_xacro=xacro) == [] '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") xacro.write_text(
issues = check_fov_consistency([spec], sensors_xacro=xacro) "<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] 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> 时要告警，而不是静默放过。""" """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 = tmp_path / "assembly.world"
world.write_text("<sdf><world name='w'/></sdf>", encoding="utf-8") 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, horizontal_fov=1.5184,
world_model="d435", 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] 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 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] root = Path(__file__).resolve().parents[3]
specs = load_cameras(root=root) specs = load_cameras(root=root)
issues = check_fov_consistency( issues = check_camera_consistency(
specs, specs,
world_file=root / "devices/sim/gazebo/worlds/assembly.world", world_file=root / "devices/sim/gazebo/worlds/assembly.world",
sensors_xacro=root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro", sensors_xacro=root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
src/arm_sim_gazebo/test/test_sim_scene.py
+939
View File
@@ -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)}