orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
f5ed44a98740a2cbe00d4e3eee17854ae3d220ce
ros_contorl/devices/sim/gazebo/README.md
T
arm_dev d32e6ae53f P5c-6 场景物料落地 + Gazebo 中文手册：工件/通止规模型、场景校验、两处静默失效修复 
一、场景物料（新增）
  devices/sim/gazebo/models/workpiece/   带三个孔的铝板。碰撞体是"带方孔的平板"，
    视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔，
    只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。
  devices/sim/gazebo/models/gauge/       通止规：通端/套环/握柄/止端四段，各段异色
    便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"，
    用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。
  devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒，
    加个方孔底座把它立住。
  三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。

二、场景配置与校验（新增）
  config/scene.yaml     物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。
    理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。
    动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。
    含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。
  sim_scene.py          摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移），
    检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。
    重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 ——
    正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报，
    而误报的后果是人把检查整个关掉，之后真重叠也抓不到。
    另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。
  scripts/check_sim_scene.py  验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 +
    悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。

三、修掉两处静默失效（都是"看起来一切正常"）
  1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析，
     而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 →
     调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**，
     而日志只说"跳过"，看起来像正常降级。
     现在两种格式都支持，并用真实抓下来的输出样本做回归测试。
     教训：宽容降级会把"没检查"伪装成"检查通过"。
  2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）：
     脚本报"已停止"，它却还在发上一份 /robot_description 与 TF，
     下一次启动就会"改了描述却不生效"。
     顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报
     一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。
     现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**：
     有残留就报出来并非零退出 —— 下次再有漏项，不再静默。
     实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。

四、相机：D405 分辨率对齐真机
  仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是
  58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 ——
  孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。
  已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场"
  扩成"视场 + 图幅"（sim_cameras.check_camera_consistency），
  不一致时说明它对像素门槛的影响。

五、文档
  devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 /
    配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。
  manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。
  scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。
  cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。

六、测试
  新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是
    位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条），
  含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、
  center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。
  全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。

同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步，
它的 parity 测试是红的）。

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-09-23 17:13:00 +08:00

465 lines
25 KiB
Markdown
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# devices/sim/gazebo/ —— Gazebo 仿真平台（中文手册）
> **这份文档是仿真的"使用说明书 + 建模规范 + 避坑清单"。**
>
> 读法建议：
> - 只想**跑起来** → 看 [第 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**（真物理、真接触） | ✔ | 验证接触/力控/几何、装配状态机 |
**要验证装配的接触与力判据，只能用后者。** 前者永远不会产生接触力。
判断当前在跑哪个：`ros2 topic echo /clock --once` —— 有 `/clock` 且时间在走，是 Gazebo。
---
## 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） |
| 世界 | `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/
├── manifest.yaml # 设备清单：进度、配置声明、放置要求（与本文档同源）
├── README.md # 本文档
├── config/
│ ├── 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/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`）
Reference in New Issue View Git Blame Copy Permalink