P5c-4 相机出图联调：桥接 d405/d435 到 ROS，并修掉两处静默的几何错误
相机链路（devices/sim/gazebo/config/cameras.yaml 是唯一真相）
- sim_cameras.py 纯逻辑：话题映射 / 深度换算 / 内参 / 视场一致性比对
- sim_camera_adapter.py 改光学系 frame_id、32FC1 米 → 16UC1 毫米、覆盖内参
- gazebo_sim.launch.py 桥接 + 适配器 + 世界相机的静态 TF（位姿从 world 解析）
本轮修掉两个**静默**的几何错误（都能出图、都能查到 TF，但点位是错的）
1. 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 的视场重算并覆盖。
代价：cameras.yaml 的 horizontal_fov 必须与 SDF 一致，启动时自动比对告警。
2. assembly.world 里 D435 的 rpy 写成 (0, 50°, 180°)，
而 URDF 相机的视线是**本体 X 轴**、rpy 复合顺序又是 Rz·Ry·Rx，
于是"先俯 50° 再偏航 180°"把视线翻到了背面：相机背对台面、只看地面。
它不报错，而且几何自洽（点确实落在真实地面上），只查数值发现不了。
改为 (0, 50°, 0)，并加回归测试锁住。
新增验收工具 + 测试
- scripts/check_sim_cameras.py 反投影深度、要求命中地面/台面才算通过。
"有图像"不是验收标准；实测 d435 台面 11.9%/地面 75.3%、d405 台面 37.8%。
- 97 项单测（新增 sim_cameras 22 项、sim_site 16 项）
顺带修掉一处"改了配置静默无效"
- sim.world 此前硬编码在 robot_description._WORLD_REL，而 site.yaml 与
manifest.yaml 都宣称改它就生效。现在真的读了：
优先级 --world > site.yaml:sim.world > 内置默认（相对路径按仓库根解析）。
- physics_enable / joint_prefix 仍未接线，但若其值与实际状态冲突，
启动时会告警（sim_site.unwired_warnings），不再静默忽略。
文档
- devices/sim/gazebo/README.md 重写：原文还写着"仿真不是 Gazebo"、
相机插件用 libgazebo_ros_camera.so，与实现早已不符。现分三部分：
哪个"仿真"是真的 / 现状与缺什么 / 五条静默失效的坑及怎么验。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+117
-64
@@ -1,95 +1,148 @@
|
||||
# devices/sim/gazebo/ —— Gazebo 仿真平台
|
||||
|
||||
## 先读这段：当前仿真是假的
|
||||
|
||||
**现在项目里的「仿真」并不是 Gazebo。** 实际情况是：
|
||||
|
||||
- `src/arm_description/urdf/arm.gazebo.xacro` 是零引用的死文件；
|
||||
- `arm_bringup/stack.launch.py` 的 `isaac` 分支只有一行日志，没有任何实际动作；
|
||||
- `run_sim.sh` 走的是 `sim.launch.py`，底层是 **mock 驱动**（`mock_arm_driver.py`），
|
||||
即用假数据模拟机器人状态，不是物理仿真；
|
||||
- Gazebo 从未真正接线。
|
||||
|
||||
所以这份文档分两部分：**第一部分是怎么把 Gazebo 真正跑起来**（当前必须做的前置工作），
|
||||
**第二部分才是日常怎么用**。不要跳过第一部分。
|
||||
> 这份文档记录的是**已经跑通的事实**和**实测踩到的坑**。仿真里绝大多数错误都是
|
||||
> 静默的（不报错、能出图、能查到 TF，结果却是错的），所以下面每条坑都写了
|
||||
> **怎么验**。改仿真之前先把"坑"那一节看完。
|
||||
|
||||
---
|
||||
|
||||
## 第一部分：把 Gazebo 真正接起来（前置工作）
|
||||
## 先读这段：哪个"仿真"是真的
|
||||
|
||||
要让 Gazebo 跑起来，缺的是这几块，按顺序补齐：
|
||||
仓库里有两个都叫"仿真"的东西，很容易混：
|
||||
|
||||
### 1. URDF 加 Gazebo 标签
|
||||
| 入口 | 底层 | 有物理吗 | 用途 |
|
||||
|------|------|----------|------|
|
||||
| `scripts/run_sim.sh` | `arm_bringup` 的 **mock 驱动**（`mock_arm_driver.py`） | ✘ 假数据 + RViz | 验证话题/流程/UI，不耗资源 |
|
||||
| `scripts/run_sim_gz.sh` | **Gazebo Fortress**（真物理、真接触） | ✔ | 验证接触/力控/几何 |
|
||||
|
||||
现有 `arm_description/urdf/arm.gazebo.xacro` 是死文件，需要重写。每个 link 必须补：
|
||||
**要验证装配的接触与力判据，只能用后者。** 前者永远不会产生接触力。
|
||||
|
||||
```xml
|
||||
<!-- 视觉碰撞体 -->
|
||||
<gazebo reference="${prefix}link1">
|
||||
<material>Gazebo/Grey</material>
|
||||
</gazebo>
|
||||
```
|
||||
---
|
||||
|
||||
并在 `arm.ros2_control.xacro` 里把 `ros2_control` 标签的 hardware 插件从
|
||||
`flexiv_hardware/FlexivHardware` 换成 `gazebo_ros2_control/GazeboSystem`。
|
||||
## 第一部分：现状（截至 P5c-4，2026-09-23）
|
||||
|
||||
### 2. 关节传动与控制器
|
||||
### 已就绪
|
||||
|
||||
Gazebo 需要 `transmission` 标签把关节映射到 `ros2_control`。控制器配置可以直接复用
|
||||
`arm_control/config/controllers.yaml` 里的 `rizon_arm_controller`，只需把关节名前缀
|
||||
从真实 SN（如 `Rizon4-063989_`）换成仿真专用的前缀（如 `sim_`）。
|
||||
| 部分 | 位置 | 说明 |
|
||||
|------|------|------|
|
||||
| 运行环境 | `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` | 两台相机的**唯一真相**：话题、坐标系、视场 |
|
||||
|
||||
### 3. 传感器插件
|
||||
### 还没做
|
||||
|
||||
| 仿真对象 | 插件 | 对应真实设备 |
|
||||
|----------|------|--------------|
|
||||
| 腕部相机 | `libgazebo_ros_camera.so` | D405 |
|
||||
| 手外相机 | `libgazebo_ros_camera.so` | D435 |
|
||||
| 深度 | `libgazebo_ros_depth_camera.so` | 对齐深度流 |
|
||||
| 触觉力 | **需要自己写插件** | Orisys |
|
||||
| 部分 | 缺什么 | 影响 |
|
||||
|------|--------|------|
|
||||
| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 |
|
||||
| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） |
|
||||
|
||||
**最关键的一条：Orisys 视触觉没有现成的 Gazebo 插件。**
|
||||
3 路切向力 + 1 路法向力 + 深度图 + 力矢量图 这套数据需要自定义 Gazebo 插件来模拟。
|
||||
在它做出来之前，仿真里**无法验证通止规装配的力控部分**（S07 接触、S08 拧紧判定）。
|
||||
建议的自定义插件做法：订阅 Gazebo 的接触传感器数据，按接触深度和相对滑动速度合成
|
||||
出一组「类触觉」读数，再按真实传感器的量纲（任意单位增量）发布到同样的内部话题上。
|
||||
|
||||
### 4. 工件与通止规模型
|
||||
|
||||
需要给工件（含 k0 中心孔、k1/k2 螺纹孔）和通止规建简化网格，并保证：
|
||||
|
||||
- 工件的 k0/k1/k2 三个点在模型坐标系里的位置与实际一致 —— 否则推理出的坐标映射到仿真里会对不上；
|
||||
- 螺纹孔要能表达「拧入」的几何阻力，否则 S08 的力反馈无法触发。
|
||||
> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 +
|
||||
> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据，
|
||||
> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到
|
||||
> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。
|
||||
> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。
|
||||
|
||||
---
|
||||
|
||||
## 第二部分：日常怎么用
|
||||
|
||||
### 启动
|
||||
|
||||
```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_` 为关节前缀，与真实臂区分 |
|
||||
| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` |
|
||||
| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） |
|
||||
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
|
||||
| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
|
||||
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | 单位必须是米。用毫米建模的必须先缩放到米 |
|
||||
| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米**。用毫米建模的必须先缩放到米 |
|
||||
| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用启动时的 `camera_name` |
|
||||
|
||||
### 导入一个新场景
|
||||
### 改配置：改哪里才会真正生效
|
||||
|
||||
1. 把 `.world` 放进 `worlds/`，网格放进 `models/<模型名>/`
|
||||
2. 改 `config/site.yaml` 的 `sim.world` 指向该文件
|
||||
3. 改 `devices/registry.yaml` 的 `sim: gazebo`（通常已是）
|
||||
4. 启动 `scripts/run_sim.sh`
|
||||
| 想改什么 | 改哪里 | 真的生效吗 |
|
||||
|----------|--------|------------|
|
||||
| 用哪个世界 | `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()` 守着：只要你写的值
|
||||
> 和实际状态不一致，启动时就会告警。**不要让它们重回静默。**
|
||||
|
||||
| 想改什么 | 改哪里 |
|
||||
|----------|--------|
|
||||
| 用哪个场景 | `site.yaml` → `sim.world` |
|
||||
| 仿真是否启用物理 | `site.yaml` → `sim.physics_enable` |
|
||||
| 仿真里的悬高/速度 | **复用真实的那套参数**（`servo.*` / `hole_detect.stage2.*`），不要另建一套，否则仿真调好的值搬到真机不生效 |
|
||||
| 关节前缀 | `site.yaml` → `sim.joint_prefix`（默认 `sim_`） |
|
||||
---
|
||||
|
||||
> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何「仿真专用参数」都是未来
|
||||
> 仿真调参搬到真机失效的隐患。只允许物理引擎本身的参数（如重力、步长）放在 `sim.*` 下。
|
||||
## 第三部分：实测踩到的坑（都属于"静默失效"）
|
||||
|
||||
这些坑的共同特征是：**不报错**。话题在、图像有、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` 自洽推导），
|
||||
但"仿真里调好的像素阈值搬到真机"会因像素尺度不同而需要重调。
|
||||
|
||||
---
|
||||
|
||||
## 相关文件一览
|
||||
|
||||
```
|
||||
devices/sim/gazebo/
|
||||
├── manifest.yaml # 设备清单：进度、配置声明、放置要求
|
||||
├── README.md # 本文档
|
||||
├── config/
|
||||
│ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场）
|
||||
│ └── controllers.yaml # 控制器配置（名字与真机一致）
|
||||
├── urdf/ # 仿真用机器人描述（含 ros2_control 与传感器）
|
||||
├── worlds/assembly.world # 场景
|
||||
├── models/ # ✘ 待做：工件/通止规/夹具
|
||||
└── sensor_plugins/ # ✘ 待做：Orisys 触觉插件
|
||||
```
|
||||
|
||||
配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器）。
|
||||
配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/check_sim_cameras.py`（相机验收）。
|
||||
|
||||
Reference in New Issue
Block a user