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
devices/sim/gazebo/README.md
+426 -110
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_gz.sh` | **Gazebo Fortress**（真物理、真接触） | ✔ | 验证接触/力控/几何 |
| `scripts/run_sim_gz.sh` | **Gazebo Fortress**（真物理、真接触） | ✔ | 验证接触/力控/几何、装配状态机 |
**要验证装配的接触与力判据，只能用后者。** 前者永远不会产生接触力。
判断当前在跑哪个：`ros2 topic echo /clock --once` —— 有 `/clock` 且时间在走，是 Gazebo。
---
## 第一部分：现状（截至 P5c-4，2026-09-23）
## 1. 使用方法
### 已就绪
### 1.1 装环境（只做一次）
```bash
bash scripts/install_sim_env.sh # 建独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
bash scripts/build_sim.sh # 编仿真用的包（--merge-install）
```
**环境是独立的一份**，与真机环境 `ros_flexiv` 隔离。理由与踩坑见
[坑 3](#坑-3conda-环境里-gz-插件路径缺-conda_prefixlib) 和 `scripts/install_sim_env.sh` 顶部的长注释。
### 1.2 启动
```bash
bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个）
bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI
bash scripts/run_sim_gz.sh --check # 只自检描述，不启动任何东西
bash scripts/run_sim_gz.sh --world /abs/other.world # 换 world
bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源，只验运动学
```
首次启动约 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。启动日志末尾会打印一份
**验收命令清单**，照它做一遍就能确认仿真真的活了。
### 1.3 验收（★ 必做，别只看"有没有图像"）
```bash
ros2 topic echo /clock --once # 仿真时间在走
ros2 control list_controllers # 两个控制器 active
ros2 topic echo /joint_states --once # 7 个 sim_joint
ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查
ros2 topic echo /tactile/left/status --once # 触觉三力在发
ign model -m workpiece -p # 工件在台面上
python3 scripts/check_sim_cameras.py # ★ 相机的真正验收
python3 scripts/check_sim_scene.py # ★ 场景摆放 + 孔检测几何验收
```
机械臂应当**在重力下保持站住**。若它瘫下去或消失，说明描述有问题 ——
见 [坑 1](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)。
**两条 `check_*.py` 才是验收标准，"有图像"证明不了什么。**
它们做的判断分别是：
| 工具 | 它凭什么判"通过" | 它防的是哪一类错 |
|------|------------------|------------------|
| `check_sim_cameras.py` | 把深度**反投影**到 world，要求命中已知平面（地面 z=0、台面 z=0.40） | 深度单位差 1000 倍、`frame_id` 盖成本体系、内参是 gz 的错值、相机位姿朝向反了 —— 全都**能出图** |
| `check_sim_scene.py` | 摆放自洽 + 像素门槛与真机一致 + 孔检测特征的像素值达标 + 手外相机**完整**看得见工件 | 物体悬空/穿模/嵌入、场景里的门槛与真机漂移、成像尺度不够、粗定位框不全 |
### 1.4 停止
```bash
bash scripts/stop_sim_gz.sh # 稳妥地停掉整套仿真
```
**不要用 `pkill -f gz`**：它会把调用它的那个 shell 一起杀掉（`-f` 匹配命令行，
而 `pkill` 自己的命令行里就含 `gz`），也会误杀同名的其他进程。`stop_sim_gz.sh`
按进程树精确识别，排除了自己和自己的祖先进程。
---
## 2. 建模要求
### 2.1 三条通用规矩（违反哪一条都会"看起来正常"）
| 规矩 | 为什么 | 违反了会怎样 |
|------|--------|--------------|
| **单位必须是米** | Gazebo 没有单位系统，SDF 里的数字就是米 | 用毫米建的 mesh 会变成 1000 倍大的怪物，或者小到看不见 |
| **模型原点放在底面中心** | 放置时 `pose.z` 就等于"放在台面上的高度" | 原点在质心时物体一半埋进台面，或者悬空；`sim_scene` 会校验并报错 |
| **`<model name>` 必须与推理用的类别名一致** | 仿真里算出的坐标要与 YOLO 结果对齐 | 视觉算出的类别名与场景里的名字对不上，坐标静默错位 |
`sim_scene.validate_scene()` 会在启动时把上面第 1、2 条自动查一遍
（模型原点、是否穿台面/悬空/出界/互相嵌入），有问题的物体**不会**被 spawn 出去。
### 2.2 质量与惯性（`<inertial>`）
**`<inertial>` 不是可选的。** 缺了它 Gazebo 会给一个默认值，物体表现得像另一个东西；
更糟的是惯性为 0 或 NaN 时物体直接消失（见 [坑 1](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)）。
- 质量与真实值同量级即可，不必精确。装配里要紧的是**质量比**：
工件太重会让夹爪"压进去"的读数异常大（见 `config/tactile.yaml` 的复测记录）。
- `inertia` 用简化公式算（薄板、圆柱的公式够用），只要量级对。
- 参考 `models/workpiece/model.sdf`（铝板：`mass=0.08`）与 `models/gauge/model.sdf`（钢规：`mass=0.02`）。
### 2.3 `<collision>` 与 `<visual>` 的分工
| | 作用 | 建模要求 |
|---|------|----------|
| `<collision>` | 决定物理：穿不穿、卡不卡、接触力多大 | 用**简单几何体**（box/cylinder），不要用 mesh —— mesh 碰撞在接触力计算上又慢又不稳 |
| `<visual>` | 决定"相机看见什么" | 可以加入**只为人眼/算法准备**的细节（亮环、色块），它们不参与物理 |
**这两个可以不一样，而且这个框架有意让它们不一样**：
`models/workpiece/model.sdf` 的 collision 是"一块带三个方孔的平板"，
而 visual 额外加了两个**亮环**（φ14 外 / φ7 内）来模拟真机上的压入衬套 ——
因为真机的 HoughCircles 就是靠这两个亮环找孔的。若只按 collision 建视觉，
仿真里"孔检测"会因为看不见环而永远失败，而这不是算法问题。
> ⚠️ 由此带来一条约定：**visual 的名字有意义**，`sim_scene.features_from_workpiece`
> 按名字前缀（`insert_*_ring` / `center_bore`）去读出特征尺寸，用来算像素值。
> 改 visual 的名字必须同步改那个函数（有单测守着）。
### 2.4 接触参数（`<surface>`）：只在必要时改，且要写清理由
默认接触参数（`kp` 很大 = 刚性）对"位置控制 + 刚性夹持"是灾难：
夹爪把工件"压进去"的力会到几百牛，工件被弹飞。
`models/gauge/model.sdf` 因此对规**单独放软**了接触刚度（`kp`/`kd` 比台面软得多），
注释里写明了原因。**这是有意为之，不是随手抄的参数** —— 照抄前先想清楚你的物体
是不是也要被夹爪夹。
### 2.5 落地判据：怎么知道模型建对了
```bash
ign sdf -p models/<名>/model.sdf # 先过语法
python3 scripts/check_sim_scene.py # 再过摆放与成像
```
两条都过了，才认为"模型建好了"。**不要以"Gazebo 里能看到它"为判据。**
---
## 3. 导入格式
### 3.1 物料放置总表
| 类型 | 放哪 | 格式 | 命名要求 | 谁读它 |
|------|------|------|----------|--------|
| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world` | `site.yaml` → `sim.world` |
| 机器人描述 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） | `gazebo_sim_launcher` |
| 场景物体 | `models/<模型名>/` | `<模型名>.sdf` + `<模型名>.config` | 目录名 = `<model name>` = 推理类别名 | `config/scene.yaml` → 启动时 spawn |
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米** | 被 SDF 的 `<mesh><uri>` 引用 |
| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用的 `camera_name` | 桥接 + 适配器 + 静态 TF |
| 场景布局 | `config/scene.yaml` | YAML | — | 启动时 spawn + 像素门槛校验 |
| 触觉标定 | `config/tactile.yaml` | YAML | 话题名必须与 URDF 里的 `<topic>` 一致 | `sim_tactile_adapter` |
| 控制器 | `config/controllers.yaml` | YAML | 控制器名与真机一致 | `ros2_control` |
> `sensor_plugins/` 目前是**空的**（只有 `.gitkeep`）。触觉走的是 Gazebo 内置的
> `force_torque` + `contact` 传感器 + 本框架的适配器，不需要自己写 C++ 插件。
> 那个目录留给"以后真要写插件"的场景。
### 3.2 一个模型目录长什么样
```
models/workpiece/
├── model.sdf # 几何 + 惯性 + 接触参数
└── model.config # 元数据：<name>、版本、指向 model.sdf、描述
```
`model.config` 里的 `<name>` **必须等于目录名**，也必须等于 YOLO 的类别名。
`models/workpiece/model.config` 里写了这条约定，照它抄。
### 3.3 加一个场景物体：改 `scene.yaml` 一处即可
物体**不在 world 文件里**，而在 `config/scene.yaml` 的 `objects` 列表里，
启动时由 `ros_gz_sim create` 动态 spawn：
```yaml
objects:
- name: workpiece # Gazebo 里的实体名（也是 ign model -m 的名字）
model: workpiece # models/ 下的目录名
pose: {x: 0.52, y: 0.0, z: 0.40, yaw: 0.0} # z = 台面顶面高度（原点在底面）
note: 为什么摆这儿 —— 请写清，否则半年后没人敢动
```
这么设计的原因：world 文件是"环境"（地面、台面、重力、固定相机），
场景是"这一批活要做什么"（工件在哪、规座在哪）。混在一起会导致
"为了挪一下工件而去改 world 文件"，而 world 文件一旦改错（比如丢了
UserCommands 系统）会让**所有**模型都 spawn 不进去（见 [坑 2](#坑-2world-里声明了任何-plugin-都会挤掉默认系统)）。
### 3.4 URDF → Gazebo：中间隔着 `sdformat`，会丢东西
`flexiv_description` 的 xacro 展开后交给 `sdformat` 转成 SDF。**这个转换是有损的**：
| 会被丢/被改的 | 后果 | 本框架的应对 |
|---------------|------|--------------|
| `<mimic>` 标签 | 夹爪的从动指节不再跟随 → 夹爪"驱动了但不动" | `urdf_normalize` 把 mimic 关系**记下来**，改成把全部 7 个夹爪关节都接入 `ros2_control`，由控制器按记录的关系驱动 |
| 缺 `<inertial>` 的 link | 惯性 NaN → 物体消失 | 启动时补默认惯性 |
| 零质量的 link | 惯性叠加出 NaN | 同上，一律归一化 |
| 世界 link（无质量的根 link） | **整棵子树被静默丢掉** | D435 因此必须放在 world 文件里，不能挂在 URDF 的 world link 上 |
这些都不是"理论上可能"，是实测踩过的。归一化逻辑在
`src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py`（37 项单测）。
单独排查：`gazebo_sim_launcher --check`。
---
## 4. 配置方式
### 4.1 ★ 改哪里才真的生效（先看这张表）
| 想改什么 | 改哪里 | 真的生效吗 |
|----------|--------|------------|
| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 优先级：`--world` > `site.yaml` > 内置默认 |
| 场景里有哪些物体、摆在哪 | `config/scene.yaml` | ✔ 启动时 spawn，并做摆放/像素校验 |
| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致告警 |
| 相机图幅（影响像素尺度） | `config/cameras.yaml` 的 `width`/`height` **且** SDF 的 `image_width/height` | ✔ 启动时自动比对（这是最容易漏的一处，见 4.3） |
| 触觉标度/轴向/基线 | `config/tactile.yaml` | ✔ 适配器每次启动都读 |
| 装配的悬高/速度 | **复用真机那套**（`servo.*` / `hole_detect.stage2.*`） | ✔ 有意共用 |
| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 的 `<gravity>` |
| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。改要动三处：`urdf/arm.sim.urdf.xacro` 的 `arm_prefix`、`config/controllers.yaml` 的 `sim_joint1..7`、本项 |
> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何"仿真专用参数"都是
> 未来"仿真里调好的值搬到真机不生效"的隐患。只允许**物理引擎自身的**参数
> （重力、步长）放在 `sim.*` 下。
>
> ✘ 的两个键由 `arm_sim_gazebo.sim_site.unwired_warnings()` 守着：写的值与实际状态
> 不一致时启动会**告警**。**不要让它们重回静默。**
### 4.2 配置文件之间的分工
```
config/site.yaml ← 站点真相：真机有哪些设备、序列号、阈值（仿真与真机共用）
│
├── devices/sim/gazebo/config/cameras.yaml ← "Gazebo 里有什么相机、桥到哪个 ROS 名字"
├── devices/sim/gazebo/config/scene.yaml ← "这一批活：工件/规座摆在哪"（含像素门槛）
├── devices/sim/gazebo/config/tactile.yaml ← 仿真的力标定口径与轴向映射
└── devices/sim/gazebo/config/controllers.yaml ← 控制器（名字与真机一致）
```
**一条原则：能派的就别抄。** 相机位姿从 world 文件里**解析**（不在 `cameras.yaml` 重复写）、
内参从 `horizontal_fov` 与图幅**算出**（不手抄 fx/fy）—— 抄一份就多一处能漂移的地方。
唯一的例外是 `scene.yaml` 里的 `gates_px`：像素门槛本来只在真机的
`hole_detect.yaml`/`site.yaml` 里，但仿真要在**启动时**就判"能不能看见"，
所以抄了一份。为了不让它漂移，`check_sim_scene.py` 会读那两个文件做**交叉核对**，
不一致直接判失败。**抄一份可以，但必须有人守着。**
### 4.3 改相机图幅会牵动三处（最常见的漏改）
```bash
# 想改 D405 的分辨率，三处必须同时改：
devices/sim/gazebo/config/cameras.yaml # width / height（内参由此算出）
devices/sim/gazebo/urdf/arm.sim.sensors.xacro # image_width / image_height（真正渲染的）
```
第三处是**下游**：`hole_detect.yaml` 的像素门槛（`hough_min_radius`、`hole_distance_min`）
只在特定像素尺度下才对。仿真用了 640x480（fx=337）而不是真机的 848x480（fx=447）时，
58mm 的两孔间距成像只剩 85px，低于 `hole_distance_min=90` ——
**孔检测在仿真里根本不可能通过**，而报出来的现象会像是"算法不行"。
启动时会比对 `cameras.yaml` 与 SDF 的视场**和**图幅，不一致会告警并说明影响。
---
## 5. 实测踩到的坑
这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，但结果是错的。
所以每条都写了"怎么发现"。
### 坑 1：官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉
7 轴本体缺 `<inertial>` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。
**只有 Gazebo 会暴露，RViz/MoveIt 完全正常。**
→ 启动时归一化（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check`。
### 坑 2：world 里声明了任何 `<plugin>` 都会挤掉默认系统
world 里只要有一个 `<plugin>`，Gazebo 就**不再**自动加载默认的
Physics / UserCommands / SceneBroadcaster。
少了 UserCommands 的后果最阴：`/world/<名>/create` 服务不存在 →
模型根本没进 Gazebo，而 `create` **仍然打印** `OK creation of entity`，
控制器永远起不来。
→ 这三个系统必须在 world 里显式声明（`assembly.world` 里已有）。启动时会检查并告警。
### 坑 3：conda 环境里 gz 插件路径缺 `$CONDA_PREFIX/lib`
`ros_gz_sim` 的 launch 只拼 `LD_LIBRARY_PATH`，而 conda 激活不往那里加。
症状是 `gz_ros2_control` 报 `couldn't find shared library`，而库其实就在那儿（`ldd` 全齐）。
→ 必须在 `ros2 launch` **之前**设 `GZ_SIM_SYSTEM_PLUGIN_PATH`；资源路径同理（否则 mesh 全加载不出来）。两件事都由 `scripts/sim_env.sh` 负责 —— **不要绕过它直接 `ros2 launch`**。
### 坑 4：gz 发的 `camera_info` 是错的
恒为 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分辨率主点），
而**渲染**用的是配置的 87° 视场与 640x480。
照抄它做三维反投影，点会偏**半米量级**；而单目预览完全正常。
→ 用已知地面平面最小二乘反解内参，得 `fy=337.2 / cy=239.5`，与
`640/(2·tan(87°/2))=337.2`、`480/2=240` 吻合（残差中位 0.2mm）。
已改为由 `sim_cameras.intrinsics()` 按 `cameras.yaml` 的视场**重算并覆盖**。
代价：`cameras.yaml` 的 `horizontal_fov` 必须与 SDF 真正渲染用的值一致，
启动时自动比对告警（`sim_cameras.check_camera_consistency`）。
### 坑 5：相机位姿的 `rpy` 复合顺序
URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；而 `rpy` 的复合顺序是 `Rz·Ry·Rx`。
"先俯 50° 再偏航 180°"会把视线翻到**背面**。
D435 就这么翻过：背对台面、只看得见地面。什么都不报，**而且几何自洽**
（点确实落在真实地面上），只查数值发现不了。
→ 用 `scripts/check_sim_cameras.py` 检查"该看见的平面有没有看见"。已修正 + 加回归测试。
### 坑 6：接触传感器只发"接触"，不发"离开"
Gazebo 的 contact 是**事件式**的：接触时发消息，离开时**一条都不发**。
若写成"收到消息=接触 / 没收到=不管"，那么离开工件后 `contact` 会永远停在 `True`。
→ 判据必须是"最近 `contact_timeout_s` 内收到过消息"（`config/tactile.yaml`）。
`config/tactile.yaml` 里对这点有专门说明。
### 坑 7：`contact-system` 发的 `Contacts` 消息里**没有力**
`ros_gz_interfaces/msg/Contact` 的 `wrenches` 字段在 Fortress 的默认
contact-system 下**永远是空的**（实测 `ign topic -e -t /tactile/left/raw -n 1`，
只有几何名与接触点，没有任何力）。
**这是这套链路里最耗时的一个坑**：消息结构里有 `wrenches` 字段，看起来"应该有"。
→ 三向力改由 `<sensor type="force_torque">` 提供（挂在指尖关节上，读该关节传递的约束力）。
接触布尔仍用 contact 传感器（它只有这个用途）。
> 附带一个教训：**光看消息定义判断不了运行时会填什么**，必须 `ign topic -e` 实测。
### 坑 8：`forcetorque-system` 也必须显式声明
与坑 2 同源。只声明了 `contact-system` 时，`force_torque` 传感器**话题根本不出现**，
而 URDF 里写得好好的。
→ `assembly.world` 里显式声明 `libignition-gazebo6-forcetorque-system.so`。
### 坑 9：`sdformat` 用**占位值**填它不认识/缺失的东西
这是坑 1 的"隐藏变体"，也是**验收检查本身**最容易失效的地方：
| 你写的 | `sdformat` 的输出 |
|--------|-------------------|
| 有 `<contact>` 但没 `<collision>` | `__default__` |
| 有 `<contact>` 但没 `<topic>` | `__default_topic__` |
| `<topic>` 写在 `<sensor>` 里而不是 `<contact>` 里 | 话题跑到默认作用域，订阅方永远收不到 |
于是"检查这个字段是不是空"的代码**永远通过** —— 因为它不是空的，它是个占位字符串。
→ `robot_description.check_tactile_collisions()` 现在显式判这两个占位值
（`__default__` / `__default_topic__`），并检查 `<topic>` 的位置对不对。
有负向单测守着（`test_robot_description.py`）。
> 通用教训：**校验代码自己会静默失效**。凡是"检查 X 是否为空"的地方，
> 都要问一句"它会不会被填成一个假值"。
### 坑 10：`<mimic>` 标签被 `sdformat` 丢掉
夹爪的从动指节在 URDF 里靠 `<mimic>` 定义跟随关系，而 `sdformat` 转换时**把它丢了**。
症状是"夹爪控制器 active，但手指不动" —— 控制器在驱动一个不存在的关节。
→ `urdf_normalize` 在转换前把 mimic 关系**记录成一张表**（`mimic_relations()`），
去掉标签，并把全部 7 个夹爪关节都接入 `ros2_control`，由适配器按关系驱动。
> 顺带：从动关节原本 `effort="0"`，归一化时一并改成可驱动的值，
> 否则它在控制器里是个"只能看不能动"的关节。
### 坑 11：`gz model -m <名> -p` 的输出格式没有稳定契约
不同版本的文本格式不同。验收脚本因此**宽松解析**，解析不出来时降级成
"跳过位姿核对"，而不是报一个假错。
> 这是有意的取舍：**宁可漏报，不可误报**。误报会让人不再相信工具。
---
## 6. 现状与进度
### 已就绪（端到端验证过）
| 部分 | 位置 | 说明 |
|------|------|------|
| 运行环境 | `scripts/install_sim_env.sh` / `sim_env.sh` | 独立 conda 环境 `ros_gz_sim`（Gazebo Fortress 6.16 + ROS 2 Humble），与真机环境 `ros_flexiv` 隔离 |
| 世界 | `worlds/assembly.world` | 地面 + 工作台 + D435 + 通止规标记位 |
| 机器人描述 | `urdf/*.xacro` | 复用官方 `flexiv_description`，启动时**归一化**后喂 Gazebo |
| 控制器 | `config/controllers.yaml` | 控制器名与真机一致（`rizon_arm_controller` + `joint_state_broadcaster`） |
| 启动器 | `src/arm_sim_gazebo` 的 `gazebo_sim_launcher` | 起 Gazebo → 发归一化 URDF → spawn → 拉起控制器 |
| 相机 | `config/cameras.yaml` | 两台相机的**唯一真相**：话题、坐标系、视场 |
| 运行环境 | `scripts/install_sim_env.sh` / `sim_env.sh` | 独立 conda 环境 `ros_gz_sim`（Gazebo Fortress 6.16 + ROS 2 Humble） |
| 世界 | `worlds/assembly.world` | 地面 + 工作台（顶面 z=0.40）+ D435（世界相机） |
| 机器人描述 | `urdf/*.xacro` | 复用官方 `flexiv_description`，启动时**归一化** |
| 控制器 | `config/controllers.yaml` | 控制器名与真机一致 |
| 启动器 | `src/arm_sim_gazebo` 的 `gazebo_sim_launcher` | 起 Gazebo → 发归一化 URDF → spawn → 拉控制器 → spawn 场景 |
| 相机 | `config/cameras.yaml` + `sim_camera_adapter` | D405/D435 桥接到 ROS，内参/单位/frame 都已校正 |
| 触觉 | `config/tactile.yaml` + `sim_tactile_adapter` | `force_torque` 三向力 + `contact` 布尔 → `arm_msgs/TactileStatus` |
| 场景物体 | `models/` + `config/scene.yaml` | 工件（带亮环的孔板）、通止规、规座，可校验摆放 |
| 夹爪 | 7 关节接入 `ros2_control` | 实测开合 27→127mm |
### 还没做
| 部分 | 缺什么 | 影响 |
|------|--------|------|
| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 |
| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） |
> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 +
> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据，
> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到
> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。
> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。
| 视触觉图像 | 仿真的深度图/力矢量图只能是**占位**（空/NaN） | `TactileStatus` 的图字段在仿真里不可信；三力是可信的 |
| 夹具/料框 | 目前只有工件、规、规座 | 上料流程（若有）没法验 |
| 夹爪服务适配器 | `~/open|~/close|~/move` 服务（mm ↔ 弧度换算） | 仿真里目前只能直接发关节指令开合，无法走真机那套服务接口 |
---
## 第二部分：日常怎么用
## 7. 有意保留的规格差异（不是 bug，但会让人困惑）
### 启动
```bash
bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个）
bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI
bash scripts/run_sim_gz.sh --check # 只自检描述，不启动任何东西
bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源，只验运动学
```
首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。
### 验收（★ 必做，别只看"有没有图像"）
```bash
ros2 topic echo /clock --once # 仿真时间在走
ros2 control list_controllers # 两个控制器 active
ros2 topic echo /joint_states --once # 7 个 sim_joint
python3 scripts/check_sim_cameras.py # ★ 相机的真正验收
```
**`check_sim_cameras.py` 才是相机的验收标准。** 它把深度反投影到 world，
检查落点是否命中已知平面（地面 z=0、台面 z=0.40）。"有图像"证明不了什么 ——
详见下面的坑 4 与坑 5。
### 放置格式与命名要求
| 类型 | 放哪 | 格式 | 命名要求 |
|------|------|------|----------|
| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` |
| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） |
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米**。用毫米建模的必须先缩放到米 |
| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用启动时的 `camera_name` |
### 改配置：改哪里才会真正生效
| 想改什么 | 改哪里 | 真的生效吗 |
|----------|--------|------------|
| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 已接线（优先级：`--world` > `site.yaml` > 内置默认） |
| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致会告警 |
| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 里的 `<gravity>` |
| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。前缀由 `urdf/arm.sim.urdf.xacro` 的 `arm_prefix` 决定，且 `config/controllers.yaml` 硬编码了 `sim_joint1..7` —— 三处要一起改 |
| 仿真里的悬高/速度 | **复用真机那套参数**（`servo.*` / `hole_detect.stage2.*`） | ✔ |
> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何"仿真专用参数"都是
> 未来"仿真调好的值搬到真机不生效"的隐患。只允许物理引擎自身的参数（重力、步长）
> 放在 `sim.*` 下。
>
> ✘ 标记的两个键由 `arm_sim_gazebo.sim_site.unwired_warnings()` 守着：只要你写的值
> 和实际状态不一致，启动时就会告警。**不要让它们重回静默。**
| 项 | 真机 | 仿真 | 后果 |
|----|------|------|------|
| 镜头畸变 | 有（`d` 非 0） | **无**（`d` 全 0、`R` 单位阵） | "仿真里用畸变矫正调出来的参数"搬到真机仍需重调 |
| D435 图幅 | 1280x720 | 640x480 | 像素尺度不同，`hole_detect` 的像素门槛搬过去要重算 |
| 触觉单位 | Orisys 形变任意单位（非牛顿） | 牛顿 × `force_scale` | 见 `config/tactile.yaml` 头部的"单位口径"；选错**不报错**，只是阈值不触发或一碰就炸 |
| 触觉图像 | 有真实深度图/力矢量图 | 占位 | 下游不用改代码，但图内容不可信 |
---
## 第三部分：实测踩到的坑（都属于"静默失效"）
这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，
但结果是错的。所以每条都配了"怎么发现"。
| # | 坑 | 症状 | 怎么发现 / 怎么修 |
|---|----|------|-------------------|
| 1 | 官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉：7 轴本体缺 `<inertial>` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN | 只有 Gazebo 拒绝，RViz/MoveIt 完全正常 | 启动时**归一化**（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check` |
| 2 | world 里只要声明了**任何一个** `<plugin>`，就会挤掉默认的 Physics / UserCommands / SceneBroadcaster | 少了 UserCommands → `/world/<名>/create` 服务不存在 → 模型根本没进 Gazebo，而 `create` 仍打印 `OK creation of entity`；控制器永远起不来 | 启动时会检查并告警。修法：在 world 里显式声明这三个系统 |
| 3 | gz 的插件搜索路径不含 `$CONDA_PREFIX/lib`（`ros_gz_sim` 的 launch 只拼 `LD_LIBRARY_PATH`，而 conda 激活不往那里加） | `gz_ros2_control` 报 `couldn't find shared library`，而库其实就在那儿（`ldd` 全齐） | 必须在 `ros2 launch` **之前**设 `GZ_SIM_SYSTEM_PLUGIN_PATH`；资源路径同理（否则 mesh 全加载不出来）。都由 `scripts/sim_env.sh` 负责 |
| 4 | **gz 发的 `camera_info` 是错的**：恒为 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分辨率主点），而**渲染**用的是配置的 87° 视场 | 照抄它做三维反投影，点会偏**半米量级**；单目预览完全正常 | 用已知地面平面做最小二乘反解内参，得到 `fy=337.2 / cy=239.5`，与 `640/(2·tan(87°/2))=337.2`、`480/2=240` 吻合（残差中位 0.2mm）。已改为由 `sim_cameras.intrinsics()` 按 `cameras.yaml` 的视场**重算并覆盖** |
| 5 | **相机位姿的 `rpy` 顺序**：URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；而 `rpy` 复合顺序是 `Rz·Ry·Rx`，"先俯 50° 再偏航 180°"会把视线翻到**背面** | D435 背对台面、只看得见地面。什么都不报，**而且几何自洽**（点确实落在真实地面上），只查数值发现不了 | 用 `scripts/check_sim_cameras.py` 检查"该看见的平面有没有看见"。已修正 + 加回归测试 |
关于坑 4：代价是 `cameras.yaml` 的 `horizontal_fov` 必须与 SDF 真正渲染用的值一致。
启动时会自动比对并告警（`sim_cameras.check_fov_consistency`）。
关于相机还有两条**规格差异**，是有意保留的（不是 bug，但会让人困惑）：
- **畸变**：gz 不模拟镜头畸变（`d` 全 0，`R` 单位阵）。所以"仿真里用畸变矫正
调出来的参数"搬到真机仍需重调。
- **分辨率**：真机 D405 是 848x480、D435 是 1280x720，仿真建模用 640x480。
对"算法能不能跑通"没影响（内参从 `cameras.yaml` 自洽推导），
但"仿真里调好的像素阈值搬到真机"会因像素尺度不同而需要重调。
---
## 相关文件一览
## 8. 文件一览
```
devices/sim/gazebo/
├── manifest.yaml # 设备清单：进度、配置声明、放置要求
├── manifest.yaml # 设备清单：进度、配置声明、放置要求（与本文档同源）
├── README.md # 本文档
├── config/
│ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场）
│ └── controllers.yaml # 控制器配置（名字与真机一致）
├── urdf/ # 仿真用机器人描述（含 ros2_control 与传感器）
├── worlds/assembly.world # 场景
├── models/ # ✘ 待做：工件/通止规/夹具
└── sensor_plugins/ # ✘ 待做：Orisys 触觉插件
│ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场/图幅）
│ ├── controllers.yaml # 控制器配置（名字与真机一致）
│ ├── scene.yaml # ★ 场景布局（工件/规/规座）+ 像素门槛
│ └── tactile.yaml # ★ 触觉标定：单位口径、轴向映射（实测）、基线策略
├── urdf/
│ ├── arm.sim.urdf.xacro # 顶层：拼装 + 前缀
│ ├── arm.sim.gazebo.xacro # Gazebo 扩展（接触/摩擦参数）
│ ├── arm.sim.ros2_control.xacro# 控制器接口（7 轴 + 7 个夹爪关节）
│ └── arm.sim.sensors.xacro # D405 + 触觉传感器（contact + force_torque）
├── worlds/assembly.world # 环境：地面 + 台面 + D435 + 显式声明的系统插件
├── models/
│ ├── workpiece/ # 工件：带三个孔的铝板 + 两个检测用亮环
│ ├── gauge/ # 通止规：通端/套环/握柄/止端，接触参数刻意放软
│ └── gauge_stand/ # 规座：static，把规立住
└── sensor_plugins/ # （空）触觉走内置传感器 + 适配器，不需要自定义插件
```
配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器）。
配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/check_sim_cameras.py`（相机验收）。
配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器、触觉适配器、场景逻辑）
配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/stop_sim_gz.sh`（停止）、
`scripts/check_sim_cameras.py`（相机验收）、`scripts/check_sim_scene.py`（场景验收）
单测：`src/arm_sim_gazebo/test/`（`test_urdf_normalize` / `test_sim_cameras` /
`test_sim_site` / `test_sim_tactile` / `test_robot_description` / `test_check_sim_scene`）
devices/sim/gazebo/config/cameras.yaml
+8 -2
View File
@@ -36,7 +36,7 @@
# 因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。
#
# 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。
# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_fov_consistency）：
# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_camera_consistency）：
# 腕上相机 → devices/sim/gazebo/urdf/arm.sim.sensors.xacro 的 horizontal_fov
# 世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 <sensor> 的 horizontal_fov
# =============================================================================
@@ -59,7 +59,13 @@ cameras:
# 仿真的传感器也按这个建，这样两边的像素尺度接近。
# 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。
horizontal_fov: 1.5184
width: 640
# 图像尺寸必须与真机一致：真机 D405 跑 848x480@30（site.yaml 的 camera.width/height）。
# 这**不是**显示偏好 —— 阶段二的孔检测在原始分辨率图上跑 HoughCircles，
# hole_detect.yaml 的像素门槛（hough_min_radius / hole_distance_min）只在这个
# 尺度下才对。曾经用 640x480（fx 337 而非真机 447），58mm 的两孔间距
# 成像只剩 85px，低于 hole_distance_min=90 → 孔检测在仿真里根本不可能通过。
# 启动时会比对这里与 xacro 宏默认值，不一致会告警（sim_cameras）。
width: 848
height: 480
note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供
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 仿真平台清单。
#
# 进度（截至 P5c-4，如实记录，不要凭印象改）：
# 进度（截至 P5c-6，如实记录，不要凭印象改）：
# ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
# 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh
# ✔ 世界文件 worlds/assembly.world（sdformat 校验通过）
@@ -14,8 +14,12 @@
# 起 Gazebo → 发归一化 URDF → spawn → 拉起两个控制器（active）
# 入口脚本 scripts/run_sim_gz.sh（此前缺失，被 install_sim_env.sh
# 与 build_sim.sh 的提示语引用，照做会报文件不存在）
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做）
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做）
# ✔ 场景模型 models/ 已有工件（带检测亮环的孔板）、通止规、规座
# 布局在 config/scene.yaml，启动时动态 spawn 并做摆放/几何校验
# ✔ 触觉 **不需要自定义插件**：用 Gazebo 内置的 force_torque（三向力）
# + contact（接触布尔）传感器，由 sim_tactile_adapter 合成
# arm_msgs/TactileStatus。标定与轴向映射在 config/tactile.yaml
# （轴向是实测的，不是猜的）。视触觉图像只能是占位。
# ✔ 相机出图 D405/D435 已注册并桥接到 ROS，且**几何正确性已验证**
# 桥接+适配：config/cameras.yaml → sim_cameras（纯逻辑：
# 话题映射 / 深度换算 / 内参，单测覆盖）
@@ -24,7 +28,9 @@
# 实测：d435 台面 11.9% / 地面 75.3%；d405 台面 37.8% / 地面 54.0%
# ✔ 配置接线 sim.world 现在真的从 config/site.yaml 读（此前硬编码，改了无效）
#
# 所以：D405/D435 可用，触觉还要等 P5c-5，装配状态机（P6）依赖它。
# 所以：相机、触觉三力、场景物体都可用，装配状态机（P6）可以在这里验几何与接触。
# 仍然做不到的：视触觉的**图像**（深度图/力矢量图只有占位），以及走真机那套
# 夹爪服务接口（仿真里目前直接发关节指令）。
#
# 实测踩到的坑（都属于"静默失效"，后续改仿真务必先看这几条）：
# 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
@@ -49,6 +55,22 @@
# d435 就踩了这个坑：相机背对台面、只看得见地面，却什么都不报，
# 而且几何自洽（点确实落在真实地面上），只查数值发现不了。
# 已修正并加回归测试（test_robot_description 的 d435 位姿用例）。
# 6. 接触传感器的消息是**事件式**的：只在接触时发，离开时不发。写成
# "收到=接触/没收到=不管"会让 contact 永远停在 True。判据必须是
# "最近 contact_timeout_s 内收到过"。
# 7. **contact-system 发的 Contacts 消息里没有力**（wrenches 恒空，实测确认）。
# 消息结构里有这个字段，看起来"应该有"—— 这是最耗时的一个坑。三向力改由
# force_torque 传感器提供；且它所需的 forcetorque-system 也必须像坑 2 那样
# 在 world 里显式声明，否则话题根本不出现。
# 8. **sdformat 会用占位值填缺失的东西**：<contact> 缺 <collision> → "__default__"，
# 缺 <topic> → "__default_topic__"。于是"检查字段是否为空"的校验代码永远通过。
# check_tactile_collisions 已显式判这两个占位值（有负向单测）。
# 通用教训：校验代码自己会静默失效。
# 9. **<mimic> 标签会被 sdformat 丢掉**：夹爪从动指节不再跟随（控制器 active
# 但手指不动）。解法是在归一化时记录 mimic 关系、去掉标签、把全部 7 个夹爪
# 关节都接入 ros2_control（从动关节原先 effort=0，也一并修掉）。
# 10. `ign model -m <名> -p` 的文本输出没有稳定契约，验收脚本按"宽松解析 +
# 解析不出来就跳过"处理 —— 宁可漏报，不可误报。
# =============================================================================
id: gazebo
@@ -63,11 +85,14 @@ description: >
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
D405/D435 已桥接到 ROS，且几何正确性有自动验收（scripts/check_sim_cameras.py）。
场景模型（工件/通止规）与 Orisys 触觉插件尚未实现；
场景物体（工件/通止规/规座）在 devices/sim/gazebo/models/，布局在 config/scene.yaml，
摆放与成像几何有自动验收（scripts/check_sim_scene.py）。
触觉用 Gazebo 内置的 force_torque + contact 传感器，由 sim_tactile_adapter
合成 arm_msgs/TactileStatus；轴向映射是实测的（config/tactile.yaml）。
视触觉的**图像**（深度图/力矢量图）在仿真里只能是占位，不可信。
sim.world 已真正从 config/site.yaml 读取（physics_enable / joint_prefix 尚未接线，
写了不一致的值会告警而非静默忽略，见文件头 config_schema）。
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
手册见 devices/sim/gazebo/README.md（含建模要求、导入格式、配置方式、避坑清单）。
# --- 物料放置要求 ----------------------------------------------------------
assets:
@@ -81,11 +106,18 @@ assets:
- path: models
required: false
what: 工件 / 夹具 / 通止规模型
format: 每个模型一个子目录，内含 <模型名>.sdf 与 meshes/
format: >
每个模型一个子目录，内含 model.sdf 与 model.config（两个文件名都是固定的，
Gazebo 按这个约定查找）。原点必须在**底面中心**，这样 scene.yaml 里的
pose.z 就等于"放在台面上的高度"。碰撞体用 box/cylinder 等简单几何，
视觉可以额外加只服务于算法的细节（如工件上的检测亮环）。
naming: >
模型名必须与推理用的类别名一致（如 workpiece），否则仿真里算出的坐标
与推理结果对不上。网格单位必须是米，用毫米建模的必须先缩放。
where_used: Gazebo 场景加载
目录名 = SDF 里的 <model name> = model.config 里的 <name> = 推理用的类别名
（如 workpiece），四者必须一致，否则仿真里算出的坐标与推理结果对不上。
网格单位必须是米，用毫米建模的必须先缩放。
where_used: >
由 config/scene.yaml 的 objects[].model 引用，启动时动态 spawn；
摆放合法性与成像几何由 scripts/check_sim_scene.py 验收。
- path: urdf
required: false
@@ -96,10 +128,13 @@ assets:
- path: sensor_plugins
required: false
what: 传感器插件（含 Orisys 触觉自定义插件）
what: 传感器插件（自定义 C++ 插件用；当前**为空**）
format: 插件源码 + CMakeLists.txt
naming: 插件类名以 Orisys 开头
where_used: 仿真传感器数据发布
where_used: >
目前没有使用者：触觉走 Gazebo 内置的 force_torque + contact 传感器，
由 sim_tactile_adapter（Python）合成 TactileStatus，无需自研插件。
保留此目录用于"以后真需要写插件"的场景（如自研视触觉弹性体成像）。
# --- 适配器 ----------------------------------------------------------------
driver:
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_pitch:=0.0
d405_yaw:=0.0
image_width:=640
image_width:=848
image_height:=480
image_hz:=30
">
<!-- =======================================================================
图像尺寸（image_width / image_height）—— **必须与真机一致**
=======================================================================
⚠️ 注意：这段说明**不能**写成上面 params="..." 里的 XML 注释 ——
XML 不允许在属性值里放注释，那样整个文件会 not well-formed，
xacro 直接展开失败（踩过）。所以只能放在这里。
真机 D405 跑 848x480@30（见 config/site.yaml 的 camera.width/height）。
仿真也必须 848x480，理由是**像素尺度的可迁移性**：
阶段二的孔检测（hole_detect_hough）直接在**原始分辨率**图上跑
HoughCircles，不做缩放。所以 hole_detect.yaml 里那些以像素为单位的
门槛（hough_min_radius 10 / min_radius 10 / hole_distance_min 90 …）
只有在"每毫米对应多少像素"与真机相同时才有意义：
fx = width / (2·tan(hfov/2))
视场角两边都是 87°，所以像素尺度完全由 width 决定：
真机 848 → fx ≈ 447
曾用 640 → fx ≈ 337（每毫米像素数只有真机的 75%）
用 640 的后果很具体：58mm 的两孔间距在真机上成像约 152px（过 90 的门槛），
在仿真里只剩 115px —— 仍是能过，但整套像素门槛的余量都被吃掉 25%，
而"看起来不对"的表现只是"孔检测偶尔不认"，极难归因。
所以在仿真侧就把尺度对齐，别留给调试。
启动时会比对这里与 cameras.yaml 的 width/height，不一致会告警
（见 sim_cameras.check_camera_consistency）—— 图幅写错不报错，
只会让像素门槛静默失效，所以要主动查。
======================================================================= -->
<!-- =======================================================================
传感器系统插件放在 **world 文件**里，不放这里
=======================================================================
devices/sim/gazebo/worlds/assembly.world
+15 -19
View File
@@ -97,10 +97,14 @@
<!-- =========================================================================
工件台
=========================================================================
高度 0.75m 是常见工作台高度。工件就放在台面上（P5c-6 里会加），
100| =========================================================================
台面顶面在 z = 0.40（本模型 pose z=0 + link top 的 z=0.375 + 板厚 0.05÷2）。
⚠️ 早先这里的注释写的是"高度 0.75m 是常见工作台高度"，与实际的 0.40
矛盾（注释是从别处抄来的）。以**算出来的 0.40 为准**：相机验收脚本
check_sim_cameras.py 就是按 z=0.40 反投影验收的，它才是契约。
台面高度决定机械臂要不要伸到底 —— 先在仿真里确认工作空间够得着，
再上真机试，能省掉很多次现场调试。
工件/通止规**不在这个文件里**：它们由 scene.yaml 描述、启动时 spawn。
台面做成 static：台子本身不该被机械臂推着走。
摩擦系数给足（0.8），否则柔顺下压时工件会被顶得滑动，
@@ -149,25 +153,17 @@
</model>
<!-- =========================================================================
通止规起始位（留给人工/机械手放置用的标记区）
工件 / 通止规 —— **不在这里**
=========================================================================
真机上通止规是被人放到夹爪里的（P6 里有"提示用户放置"的交互）。
仿真里这里放一个视觉标记，方便对着它调整"返回极值角是否到位"。
真正会被夹爪抓取的通止规模型在 P5c-6 里加到这个位置。
它们**不进 world 文件**，而是在启动时由 gazebo_sim.launch.py 用
`ros_gz_sim create` spawn（位姿在 devices/sim/gazebo/config/scene.yaml）。
分工：world 放"固定不动"的东西（地面、台子、支架相机），
物料按站点/工序换位置 —— 写进 world 就等于每次换料都改 world，
而 world 里还有物理参数和 5 个必需的系统插件（动它有静默失效风险）。
本文件里早期有过一个 gauge_marker（一个悬空的蓝色圆盘，用来标记
"通止规该放这里"）。它已经没用了：真正的通止规模型已经在 models/ 里、
由场景配置摆放，留着只会让人以为"那就是通止规"。
========================================================================= -->
<model name="gauge_marker">
<static>true</static>
<pose>0.15 0.35 0.755 0 0 0</pose>
<link name="link">
<visual name="visual">
<geometry><cylinder><radius>0.04</radius><length>0.004</length></cylinder></geometry>
<material>
<ambient>0.1 0.6 0.9 1</ambient>
<diffuse>0.1 0.6 0.9 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- =========================================================================
基础系统插件 —— 必须显式声明，否则整个仿真静默失效