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
@@ -480,13 +480,19 @@ network:
|
|||||||
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
|
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
|
||||||
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
|
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
|
||||||
sim:
|
sim:
|
||||||
# ⚠️ 这一段目前**没有代码读取**（gazebo_sim_launcher 还没实现，见
|
# 这一段由 arm_sim_gazebo.sim_site 真正读取（此前是"声明了但没人读"，
|
||||||
# devices/sim/gazebo/manifest.yaml 的进度段）。现在跑 `run_sim.sh` 得到的
|
# 改了毫无反应也不报错）。三个键的接线状况**不一样**，务必看清：
|
||||||
# 仍是 mock 驱动（假数据 + RViz），不是物理仿真。
|
|
||||||
#
|
#
|
||||||
# 之所以先把 world 填上：世界文件与机器人描述**已经做好并通过 sdformat 校验**，
|
# world ✔ 已接线。优先级：命令行 --world > 这里 > 内置默认。
|
||||||
# 填在这里等于把"参数该从哪来"确定下来，等启动器一接上就自动生效，
|
# 相对路径按**仓库根**解析。
|
||||||
# 不用再改配置。路径相对仓库根。
|
# physics_enable ✘ 未接线。世界文件里的重力/步长是写死的。若这里写的值
|
||||||
|
# 与世界的实际状态不符，启动时会打印告警（不再静默）。
|
||||||
|
# joint_prefix ✘ 未接线。前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix
|
||||||
|
# 决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。
|
||||||
|
# 写了个不同的值会告警。要真改，三处必须一起改。
|
||||||
|
#
|
||||||
|
# 另外注意：`run_sim.sh` 走的是 mock 驱动（假数据 + RViz），**不是**物理仿真；
|
||||||
|
# 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。
|
||||||
platform: "gazebo"
|
platform: "gazebo"
|
||||||
world: "devices/sim/gazebo/worlds/assembly.world"
|
world: "devices/sim/gazebo/worlds/assembly.world"
|
||||||
physics_enable: true
|
physics_enable: true
|
||||||
|
|||||||
+117
-64
@@ -1,95 +1,148 @@
|
|||||||
# devices/sim/gazebo/ —— Gazebo 仿真平台
|
# devices/sim/gazebo/ —— Gazebo 仿真平台
|
||||||
|
|
||||||
## 先读这段：当前仿真是假的
|
> 这份文档记录的是**已经跑通的事实**和**实测踩到的坑**。仿真里绝大多数错误都是
|
||||||
|
> 静默的（不报错、能出图、能查到 TF，结果却是错的），所以下面每条坑都写了
|
||||||
**现在项目里的「仿真」并不是 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 真正跑起来**（当前必须做的前置工作），
|
|
||||||
**第二部分才是日常怎么用**。不要跳过第一部分。
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 第一部分：把 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 插件从
|
## 第一部分：现状（截至 P5c-4，2026-09-23）
|
||||||
`flexiv_hardware/FlexivHardware` 换成 `gazebo_ros2_control/GazeboSystem`。
|
|
||||||
|
|
||||||
### 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 |
|
| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 |
|
||||||
| 手外相机 | `libgazebo_ros_camera.so` | D435 |
|
| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） |
|
||||||
| 深度 | `libgazebo_ros_depth_camera.so` | 对齐深度流 |
|
|
||||||
| 触觉力 | **需要自己写插件** | Orisys |
|
|
||||||
|
|
||||||
**最关键的一条：Orisys 视触觉没有现成的 Gazebo 插件。**
|
> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 +
|
||||||
3 路切向力 + 1 路法向力 + 深度图 + 力矢量图 这套数据需要自定义 Gazebo 插件来模拟。
|
> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据，
|
||||||
在它做出来之前，仿真里**无法验证通止规装配的力控部分**（S07 接触、S08 拧紧判定）。
|
> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到
|
||||||
建议的自定义插件做法：订阅 Gazebo 的接触传感器数据，按接触深度和相对滑动速度合成
|
> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。
|
||||||
出一组「类触觉」读数，再按真实传感器的量纲（任意单位增量）发布到同样的内部话题上。
|
> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。
|
||||||
|
|
||||||
### 4. 工件与通止规模型
|
|
||||||
|
|
||||||
需要给工件（含 k0 中心孔、k1/k2 螺纹孔）和通止规建简化网格，并保证：
|
|
||||||
|
|
||||||
- 工件的 k0/k1/k2 三个点在模型坐标系里的位置与实际一致 —— 否则推理出的坐标映射到仿真里会对不上；
|
|
||||||
- 螺纹孔要能表达「拧入」的几何阻力，否则 S08 的力反馈无法触发。
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 第二部分：日常怎么用
|
## 第二部分：日常怎么用
|
||||||
|
|
||||||
|
### 启动
|
||||||
|
|
||||||
|
```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` |
|
| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` |
|
||||||
| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 必须以 `sim_` 为关节前缀，与真实臂区分 |
|
| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） |
|
||||||
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
|
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
|
||||||
| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
|
| 传感器插件 | `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`（通常已是）
|
| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 已接线（优先级：`--world` > `site.yaml` > 内置默认） |
|
||||||
4. 启动 `scripts/run_sim.sh`
|
| 相机的视场/坐标系/话题 | `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`（相机验收）。
|
||||||
|
|||||||
@@ -0,0 +1,86 @@
|
|||||||
|
# =============================================================================
|
||||||
|
# devices/sim/gazebo/config/cameras.yaml
|
||||||
|
#
|
||||||
|
# 仿真相机的**唯一数据源**：gazebo_sim.launch.py 里的 ros_gz_bridge 参数、
|
||||||
|
# sim_camera_adapter 节点、以及为世界相机补的静态 TF，全部从这份文件派生。
|
||||||
|
# 想加一台仿真相机，改这里一处即可（前提是它在 URDF / world 里已经有 <sensor>）。
|
||||||
|
#
|
||||||
|
# 与 site.yaml 的分工：
|
||||||
|
# 这里只描述"Gazebo 里有什么、要桥到 ROS 的哪个名字"。
|
||||||
|
# site.yaml 描述"真机上有哪些相机、序列号是多少"。仿真不读序列号 ——
|
||||||
|
# Gazebo 里没有 USB，序列号无从绑定。
|
||||||
|
# 两者的**话题命名约定必须一致**，这是应用层能不改代码跑仿真的前提。
|
||||||
|
#
|
||||||
|
# 话题命名约定（与真机 realsense 完全对齐，见 arm_application/handeye_realsense.py
|
||||||
|
# 的 REALSENSE_TOPIC_DEFAULTS 与 realsense_color_pub.py）：
|
||||||
|
# /<ros_ns>/color/image_raw rgb8
|
||||||
|
# /<ros_ns>/color/camera_info plumb_bob
|
||||||
|
# /<ros_ns>/aligned_depth_to_color/image_raw 16UC1，单位毫米，0 = 无读数
|
||||||
|
#
|
||||||
|
# ⚠️ 单位与编码是关键差异，不能只做话题改名就完事：
|
||||||
|
# Gazebo 的深度相机发的是 **32FC1、单位米**（本机实测 step=2560 即 4 字节/像素），
|
||||||
|
# 而真机 realsense 的 aligned_depth 是 **16UC1、单位毫米**。
|
||||||
|
# 直接把 32FC1 换个名字发出去，应用侧按毫米解释会整体差 1000 倍，
|
||||||
|
# 且 numpy 的 frombuffer 会因 dtype 不符报错或读出垃圾。
|
||||||
|
# 所以深度必须**真正转换**，由 sim_camera_adapter 负责。
|
||||||
|
#
|
||||||
|
# ⚠️⚠️ 更要紧的一条：**gz 发布的 camera_info 是错的，不能直接用。**
|
||||||
|
# 本机实测（Fortress 6.16 / RoboStack）：无论怎么配，它都给
|
||||||
|
# fx=fy=277, cx=160, cy=120
|
||||||
|
# 这组值对应的是 **gz 的默认值** —— 60° 视场 + 半分辨率图幅(320x240)，
|
||||||
|
# 而实际渲染用的是配置里的 87° 视场与 640x480。
|
||||||
|
# 用已知地面平面反解真实内参得到 fy=337.2、cy=239.5，
|
||||||
|
# 与 640/2/tan(1.5184/2)=337.2、480/2=240 完全吻合（残差中位 0.2mm）。
|
||||||
|
# 照抄 gz 的 camera_info 去做三维反投影，点位会错 0.5 米量级 ——
|
||||||
|
# 而单目预览里**完全看不出来**，只会让"仿真里调好的参数搬到真机"整个失效。
|
||||||
|
# 因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。
|
||||||
|
#
|
||||||
|
# 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。
|
||||||
|
# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_fov_consistency）：
|
||||||
|
# 腕上相机 → devices/sim/gazebo/urdf/arm.sim.sensors.xacro 的 horizontal_fov
|
||||||
|
# 世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 <sensor> 的 horizontal_fov
|
||||||
|
# =============================================================================
|
||||||
|
|
||||||
|
cameras:
|
||||||
|
# --- 腕上相机（eye-in-hand），挂在机械臂法兰上 ------------------------------
|
||||||
|
- gz_ns: /d405 # Gazebo 传感器话题前缀（see arm.sim.sensors.xacro 的 <topic>）
|
||||||
|
ros_ns: d405 # ROS 侧命名空间。应用按 camera_name 参数用这个名字取话题，
|
||||||
|
# 所以它必须等于启动应用时给的 camera_name（默认 d405）
|
||||||
|
link_frame: sim_d405_link
|
||||||
|
# 图像上盖的坐标系必须是**光学系**，不能是本体系。
|
||||||
|
# gz 与 URDF 里的相机本体遵循"X 朝前 / Y 朝左 / Z 朝上"（相机机身约定），
|
||||||
|
# 而 ROS 的 image / camera_info 必须用光学约定"Z 朝前 / X 朝右 / Y 朝下"
|
||||||
|
# （REP-103/105）。两者差一个 rpy=(-90°, 0, -90°)。
|
||||||
|
# 若直接把 frame_id 盖成本体系，应用做 tf2 查询能查到、不报错，但
|
||||||
|
# 深度反投影出的三维点会整体转错 —— 这类错误在单目预览里完全看不出来。
|
||||||
|
optical_frame: sim_d405_color_optical_frame
|
||||||
|
depth: true
|
||||||
|
# 水平视场（弧度）。真机 D405 彩色的水平视场约 87° = 1.5184 rad，
|
||||||
|
# 仿真的传感器也按这个建，这样两边的像素尺度接近。
|
||||||
|
# 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。
|
||||||
|
horizontal_fov: 1.5184
|
||||||
|
width: 640
|
||||||
|
height: 480
|
||||||
|
note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供
|
||||||
|
|
||||||
|
# --- 手外相机（eye-to-hand），固定在世界里看工作台 -------------------------
|
||||||
|
# ⚠️ 这台相机在 world 文件里，不在 URDF 里 —— 因为它的正确物理归属就是"世界"，
|
||||||
|
# 而不是机器人本体。（曾经把它挂在 URDF 的 world link 上，结果 sdformat 把
|
||||||
|
# 无质量的根 link 连同整棵子树静默丢掉，相机与它的话题一起消失。详见
|
||||||
|
# devices/sim/gazebo/README.md。）
|
||||||
|
# 代价是：gz 不会为世界模型发布 TF，所以必须由启动方补一条静态 TF
|
||||||
|
# world → d435_link。位姿从 world 文件里**解析**得到（不在这里重复配置），
|
||||||
|
# 避免"改了一处忘了另一处"。
|
||||||
|
- gz_ns: /d435
|
||||||
|
ros_ns: d435
|
||||||
|
link_frame: d435_link
|
||||||
|
optical_frame: d435_color_optical_frame
|
||||||
|
depth: true
|
||||||
|
horizontal_fov: 1.5184
|
||||||
|
width: 640
|
||||||
|
height: 480
|
||||||
|
# 非空 = 这台相机挂在世界模型上，需要补静态 TF。
|
||||||
|
# world_model 是 world 文件里的 <model name>，world_link 是模型里的 <link name>。
|
||||||
|
world_model: d435
|
||||||
|
world_link: link
|
||||||
|
note: height 也与真机不同（真机 1280x720），同 D405 的说明
|
||||||
@@ -3,7 +3,7 @@
|
|||||||
#
|
#
|
||||||
# Gazebo 仿真平台清单。
|
# Gazebo 仿真平台清单。
|
||||||
#
|
#
|
||||||
# 进度（截至 P5c-3，如实记录，不要凭印象改）：
|
# 进度（截至 P5c-4，如实记录，不要凭印象改）：
|
||||||
# ✔ 环境 独立 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 校验通过）
|
||||||
@@ -16,12 +16,17 @@
|
|||||||
# 与 build_sim.sh 的提示语引用，照做会报文件不存在）
|
# 与 build_sim.sh 的提示语引用，照做会报文件不存在）
|
||||||
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做）
|
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做）
|
||||||
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做）
|
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做）
|
||||||
# ✘ 相机出图 D405/D435 传感器已在 Gazebo 里成功注册并发布话题，
|
# ✔ 相机出图 D405/D435 已注册并桥接到 ROS，且**几何正确性已验证**
|
||||||
# 但还没接 ros_gz_bridge 到 ROS，也还没验证图像内容
|
# 桥接+适配：config/cameras.yaml → sim_cameras（纯逻辑：
|
||||||
|
# 话题映射 / 深度换算 / 内参，单测覆盖）
|
||||||
|
# → sim_camera_adapter（改光学系 frame_id、米→毫米、覆盖内参）
|
||||||
|
# 验收工具：scripts/check_sim_cameras.py（反投影必须命中地面/台面）
|
||||||
|
# 实测：d435 台面 11.9% / 地面 75.3%；d405 台面 37.8% / 地面 54.0%
|
||||||
|
# ✔ 配置接线 sim.world 现在真的从 config/site.yaml 读（此前硬编码，改了无效）
|
||||||
#
|
#
|
||||||
# 所以：D405/D435 与触觉还要等 P5c-4 / P5c-5，装配状态机（P6）依赖它们。
|
# 所以：D405/D435 可用，触觉还要等 P5c-5，装配状态机（P6）依赖它。
|
||||||
#
|
#
|
||||||
# 三个实测踩到、且都属于"静默失效"的坑（后续改仿真时务必先看这三条）：
|
# 实测踩到的坑（都属于"静默失效"，后续改仿真务必先看这几条）：
|
||||||
# 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
|
# 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
|
||||||
# <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。两者都只有
|
# <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。两者都只有
|
||||||
# Gazebo 会暴露，RViz/MoveIt 完全正常。已在启动时归一化解决。
|
# Gazebo 会暴露，RViz/MoveIt 完全正常。已在启动时归一化解决。
|
||||||
@@ -29,11 +34,21 @@
|
|||||||
# Physics/UserCommands/SceneBroadcaster。少了 UserCommands，
|
# Physics/UserCommands/SceneBroadcaster。少了 UserCommands，
|
||||||
# /world/<名>/create 服务不存在 → 模型根本没进 Gazebo，而 create 仍打印
|
# /world/<名>/create 服务不存在 → 模型根本没进 Gazebo，而 create 仍打印
|
||||||
# "OK creation of entity"。所以这三个系统必须在 world 里显式声明。
|
# "OK creation of entity"。所以这三个系统必须在 world 里显式声明。
|
||||||
# 3. gz 的插件搜索路径不含 $CONDA_PREFIX/lib（ros_gz_sim 只拼 LD_LIBRARY_PATH，
|
# 3. gz 的插件搜索路径不含 $CONDA_PREFIX/lib（ros_gz_sim 的 launch 只拼
|
||||||
# 而 conda 激活不往那里加），于是 gz_ros2_control 报 "couldn't find shared
|
# LD_LIBRARY_PATH，而 conda 激活不往那里加），于是 gz_ros2_control 报
|
||||||
# library" 而库其实就在那儿。必须在 ros2 launch 之前设
|
# "couldn't find shared library" 而库其实就在那儿。必须在 ros2 launch 之前设
|
||||||
# GZ_SIM_SYSTEM_PLUGIN_PATH；资源路径同理（否则 mesh 全加载不出来）。
|
# GZ_SIM_SYSTEM_PLUGIN_PATH；资源路径同理（否则 mesh 全加载不出来）。
|
||||||
# 两者都由 scripts/sim_env.sh 负责。详见该文件里的长注释。
|
# 两者都由 scripts/sim_env.sh 负责。详见该文件里的长注释。
|
||||||
|
# 4. **gz 发的 camera_info 是错的**：恒为 fx=fy=277, cx=160, cy=120
|
||||||
|
# （等于它的默认 60° 视场 + 半分辨率主点），而渲染用的是配置的 87° 视场。
|
||||||
|
# 照抄它做三维反投影，点会偏半米量级，而单目预览完全看不出来。
|
||||||
|
# 已改为由 sim_cameras.intrinsics() 按 cameras.yaml 的视场重算并覆盖。
|
||||||
|
# 代价：cameras.yaml 的 horizontal_fov 必须与 SDF 一致，启动时会自动比对告警。
|
||||||
|
# 5. **相机位姿的 rpy 顺序**：URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；
|
||||||
|
# 而 rpy 复合顺序是 Rz·Ry·Rx，"先俯 50° 再偏航 180°"会把视线翻到背面。
|
||||||
|
# d435 就踩了这个坑：相机背对台面、只看得见地面，却什么都不报，
|
||||||
|
# 而且几何自洽（点确实落在真实地面上），只查数值发现不了。
|
||||||
|
# 已修正并加回归测试（test_robot_description 的 d435 位姿用例）。
|
||||||
# =============================================================================
|
# =============================================================================
|
||||||
|
|
||||||
id: gazebo
|
id: gazebo
|
||||||
@@ -47,8 +62,10 @@ version: "Fortress 6.16 (ignition-gazebo6, ros_gz + gz_ros2_control 0.7.15)"
|
|||||||
description: >
|
description: >
|
||||||
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
|
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
|
||||||
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
|
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
|
||||||
|
D405/D435 已桥接到 ROS，且几何正确性有自动验收（scripts/check_sim_cameras.py）。
|
||||||
场景模型（工件/通止规）与 Orisys 触觉插件尚未实现；
|
场景模型（工件/通止规）与 Orisys 触觉插件尚未实现；
|
||||||
相机传感器已在 Gazebo 内注册并出话题，但还没接到 ROS 侧（P5c-4）。
|
sim.world 已真正从 config/site.yaml 读取（physics_enable / joint_prefix 尚未接线，
|
||||||
|
写了不一致的值会告警而非静默忽略，见文件头 config_schema）。
|
||||||
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
|
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
|
||||||
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
|
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
|
||||||
|
|
||||||
@@ -100,28 +117,44 @@ driver:
|
|||||||
# --- 配置项声明 ------------------------------------------------------------
|
# --- 配置项声明 ------------------------------------------------------------
|
||||||
# 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），
|
# 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），
|
||||||
# 避免「仿真调好的值搬到真机不生效」。只有物理引擎自身的参数放在 sim.* 下。
|
# 避免「仿真调好的值搬到真机不生效」。只有物理引擎自身的参数放在 sim.* 下。
|
||||||
|
#
|
||||||
|
# ⚠️ 每个键都必须如实标注"是否真的接线"（wired）。声明了却没接线的键是最难查的
|
||||||
|
# 一类问题：用户按文档改了 site.yaml，行为毫无变化，也不报错。
|
||||||
|
# 目前由 arm_sim_gazebo.sim_site 读取的是 world（已接线）；
|
||||||
|
# physics_enable / joint_prefix 尚未接线，若其值与世界/描述的实际状态冲突，
|
||||||
|
# 启动时会打印告警 —— 见 sim_site.unwired_warnings()。
|
||||||
config_schema:
|
config_schema:
|
||||||
- key: world
|
- key: world
|
||||||
site_path: sim.world
|
site_path: sim.world
|
||||||
type: str
|
type: str
|
||||||
required: false
|
required: false
|
||||||
default: ""
|
default: ""
|
||||||
|
wired: true
|
||||||
doc: >
|
doc: >
|
||||||
仿真世界文件路径。留空表示使用 Gazebo 默认空场景。
|
仿真世界文件路径。留空 = 用内置默认（devices/sim/gazebo/worlds/assembly.world）。
|
||||||
Gazebo 接线完成后填真实路径，如 devices/sim/gazebo/worlds/assembly.world。
|
相对路径按仓库根解析。优先级：命令行 --world > 本项 > 内置默认。
|
||||||
|
实现在 arm_sim_gazebo/sim_site.resolve_world()。
|
||||||
|
|
||||||
- key: physics_enable
|
- key: physics_enable
|
||||||
site_path: sim.physics_enable
|
site_path: sim.physics_enable
|
||||||
type: bool
|
type: bool
|
||||||
required: false
|
required: false
|
||||||
default: true
|
default: true
|
||||||
doc: 是否启用物理引擎。关闭可用于纯运动学验证。
|
wired: false
|
||||||
|
doc: >
|
||||||
|
是否启用物理引擎。**尚未接线**：世界文件里的 <physics>（重力/步长）是写死的，
|
||||||
|
本项不会传导过去。若这里写的值与世界实际状态不符，启动时告警而不是静默忽略。
|
||||||
|
要真正生效，需改 world 文件里的 <gravity>，或把本项接进启动流程。
|
||||||
|
|
||||||
- key: joint_prefix
|
- key: joint_prefix
|
||||||
site_path: sim.joint_prefix
|
site_path: sim.joint_prefix
|
||||||
type: str
|
type: str
|
||||||
required: false
|
required: false
|
||||||
default: sim_
|
default: sim_
|
||||||
|
wired: false
|
||||||
doc: >
|
doc: >
|
||||||
仿真关节名前缀。必须与真实臂的前缀（Rizon4-xxxxxx_）不同，
|
仿真关节名前缀。必须与真实臂的前缀（Rizon4-xxxxxx_）不同，
|
||||||
否则仿真与真机的 TF 会互相污染。
|
否则仿真与真机的 TF 会互相污染。
|
||||||
|
**尚未接线**：实际前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix 决定，
|
||||||
|
且 config/controllers.yaml 里硬编码了 sim_joint1..7。写了个不同的值会告警。
|
||||||
|
要真改，这三处必须一起改。
|
||||||
|
|||||||
@@ -229,15 +229,36 @@
|
|||||||
（早期把它挂在 URDF 的 world link 上，结果转 SDF 时整台相机消失 ——
|
（早期把它挂在 URDF 的 world link 上，结果转 SDF 时整台相机消失 ——
|
||||||
sdformat 丢弃无质量根 link 时连带丢了整棵子树，且不报错。）
|
sdformat 丢弃无质量根 link 时连带丢了整棵子树，且不报错。）
|
||||||
|
|
||||||
位姿：pitch≈50° 俯视，yaw=180° 让光轴转向机械臂一侧。
|
位姿：pitch=50° 俯视。**yaw 必须是 0**，原因见下面的坑。
|
||||||
|
|
||||||
|
⚠⚠ 实测踩到的坑（第四个"静默失效"）：这里的 rpy 曾经写成
|
||||||
|
`0.0 0.872665 3.14159`（pitch 50° + yaw 180°），注释写的是
|
||||||
|
"yaw=180° 让光轴转向机械臂一侧"。但 rpy 的复合顺序是 Rz·Ry·Rx，
|
||||||
|
在 50° 俯角**之后**再做 180° 的 Z 旋转，会把视线从 "+X 斜向下"
|
||||||
|
翻成 "-X 斜向下" —— 也就是**背对台面**，只看得见地面。
|
||||||
|
而它不报任何错：话题有、图像有、深度有、TF 也对，
|
||||||
|
唯一症状是"相机拍不到该拍的东西"。用地面平面反投影验收时，
|
||||||
|
点是落在真实地面上的（几何自洽），所以只查数值根本发现不了。
|
||||||
|
正确理解：URDF 相机是"X 朝前"约定，**视线 = 本体 X 轴**，
|
||||||
|
Ry(50°) 已经把本体 X 从 (1,0,0) 转到 (cos50,0,-sin50)，
|
||||||
|
正好是"朝 +X 俯视 50°"，yaw 再转就过头了。
|
||||||
|
|
||||||
视野必须完整覆盖台面 —— 粗定位失败要回退到这个视角，看不全工件就等于
|
视野必须完整覆盖台面 —— 粗定位失败要回退到这个视角，看不全工件就等于
|
||||||
没有兜底方案。验收方法：
|
没有兜底方案。验收方法（**必须看命中率，不能只看"有图像"**）：
|
||||||
ros2 topic hz /d435/color/image_raw # 有数
|
ros2 topic hz /d435/color/image_raw # 有数
|
||||||
rqt_image_view /d435/color/image_raw # 工件三个孔都在画面内
|
python3 scripts/check_sim_cameras.py # 反投影必须命中 z=0.40 台面
|
||||||
|
后者会自动检查两台相机是否真的看到了该看的平面。
|
||||||
|
|
||||||
|
2026-09-23 实测基线（改位姿后请重测并更新，别凭印象）：
|
||||||
|
台面四角投影到画面 u∈[172,468]、v∈[144,286]（画面 640x480）→ 完整在视野内
|
||||||
|
通止规标记 → 像素 (180, 209)
|
||||||
|
台面像素占画面 11.9%，地面 75.3%
|
||||||
|
机械臂自身会遮住台面靠中偏下的一小块 —— 物理上必然，可接受；
|
||||||
|
但 P5c-6 放上工件后要用上面那条命令复核"工件是否完整可见"。
|
||||||
========================================================================= -->
|
========================================================================= -->
|
||||||
<model name="d435">
|
<model name="d435">
|
||||||
<static>true</static>
|
<static>true</static>
|
||||||
<pose>-0.45 0.0 1.35 0.0 0.872665 3.14159</pose>
|
<pose>-0.45 0.0 1.35 0.0 0.872665 0.0</pose>
|
||||||
<link name="link">
|
<link name="link">
|
||||||
<visual name="body">
|
<visual name="body">
|
||||||
<geometry><box><size>0.09 0.025 0.025</size></box></geometry>
|
<geometry><box><size>0.09 0.025 0.025</size></box></geometry>
|
||||||
|
|||||||
@@ -14,6 +14,12 @@
|
|||||||
- 体检：`weights.sh doctor --check-engines`（能查出 engine 与 TensorRT 版本不匹配这类静默失效）
|
- 体检：`weights.sh doctor --check-engines`（能查出 engine 与 TensorRT 版本不匹配这类静默失效）
|
||||||
- 规则写在 [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 物理仿真，唯一能做接触/力控验证的入口）
|
||||||
|
- 环境是独立 conda 环境 `ros_gz_sim`（`install_sim_env.sh` 装、`sim_env.sh` 激活），与真机环境隔离
|
||||||
|
- 相机验收：`python3 scripts/check_sim_cameras.py` —— **别用 `ros2 topic hz` 或肉眼看图验收相机**，
|
||||||
|
仿真相机的错误几乎全是静默的（深度单位、frame_id、内参、位姿方向），都能出图却是错的。
|
||||||
|
这条命令把深度反投影到 world，要求命中已知平面（地面/台面）才算通过。
|
||||||
|
- 注意 `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`
|
||||||
**搁置未用：** `fetch_isaac_flexiv.sh`（拉 Flexiv 的 Isaac Sim / RDK 源码到 `thirdparty/`；Isaac 路线目前搁置，Gazebo 是选定平台）
|
**搁置未用：** `fetch_isaac_flexiv.sh`（拉 Flexiv 的 Isaac Sim / RDK 源码到 `thirdparty/`；Isaac 路线目前搁置，Gazebo 是选定平台）
|
||||||
|
|||||||
Executable
+328
@@ -0,0 +1,328 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""仿真相机的几何验收：把深度反投影到 world，看它是否真的落在该落的平面上。
|
||||||
|
|
||||||
|
=============================================================================
|
||||||
|
为什么需要这个工具（而不是"看图有图像就行"）
|
||||||
|
=============================================================================
|
||||||
|
仿真里相机的错误几乎都是**静默**的：话题在、图像有、深度有、TF 也查得到，
|
||||||
|
而三维点位整体是错的。下面每一类都真实踩过：
|
||||||
|
|
||||||
|
1. 深度单位错了（gz 发 32FC1 米，应用当 16UC1 毫米读）→ 差 1000 倍。
|
||||||
|
2. frame_id 盖成了相机**本体**系而不是光学系 → tf2 查得到、不报错，
|
||||||
|
但反投影出的点整体转错。
|
||||||
|
3. **gz 发的 camera_info 是错的**（恒为 fx=fy=277, cx=160, cy=120，
|
||||||
|
即它的默认 60° 视场 + 半分辨率主点），而渲染用的是配置的 87° 视场。
|
||||||
|
照抄它反投影，点会偏半米量级。
|
||||||
|
4. 相机位姿的 rpy 写错 → 相机**背对**目标，只看得见地面。
|
||||||
|
这种情况几何仍然自洽（点确实落在真实地面上），所以只查数值发现不了 ——
|
||||||
|
必须比对"该看见什么"，也就是本工具要说的平面。
|
||||||
|
|
||||||
|
所以本工具的判据是：**反投影出的点必须命中预期平面**。
|
||||||
|
"有图像"不算通过，"落在正确的位置"才算。
|
||||||
|
|
||||||
|
用法（必须先 source scripts/sim_env.sh，因为需要 ROS 环境）：
|
||||||
|
python3 scripts/check_sim_cameras.py
|
||||||
|
python3 scripts/check_sim_cameras.py --expect d435:0.40:30 --cameras d435
|
||||||
|
|
||||||
|
退出码 0 = 全部通过；非 0 = 有不通过的相机（可直接用于 CI / 启动自检）。
|
||||||
|
=============================================================================
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import math
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
try:
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from rclpy.qos import qos_profile_sensor_data
|
||||||
|
from sensor_msgs.msg import CameraInfo, Image
|
||||||
|
from tf2_ros import Buffer, TransformListener
|
||||||
|
except ImportError as exc: # pragma: no cover - 环境问题，不是逻辑问题
|
||||||
|
print(
|
||||||
|
f"✘ 导入 ROS 失败：{exc}\n"
|
||||||
|
f" 这个脚本要在仿真环境里跑，先执行：\n"
|
||||||
|
f" source scripts/sim_env.sh && . install_sim/setup.bash",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
sys.exit(2)
|
||||||
|
|
||||||
|
|
||||||
|
# 默认期望：哪台相机"必须"看见哪个平面。
|
||||||
|
# d435 是手外相机，它的职责就是看见台面（P6 的粗定位要靠它），
|
||||||
|
# 所以要求 ≥10% 的像素落在台面上。
|
||||||
|
# d405 是腕上相机，视野窄且会拍到机械臂自己，台面占比要求放宽到 5%。
|
||||||
|
# 地面(z=0) 不作为要求：它被拍到多少取决于姿态，不构成"对不对"的判据。
|
||||||
|
DEFAULT_EXPECT = ("d435:0.40:10", "d405:0.40:5")
|
||||||
|
|
||||||
|
|
||||||
|
def quat_to_R(q) -> np.ndarray:
|
||||||
|
"""四元数 (x, y, z, w) → 3x3 旋转矩阵。"""
|
||||||
|
x, y, z, w = q
|
||||||
|
n = x * x + y * y + z * z + w * w
|
||||||
|
if n <= 0.0:
|
||||||
|
return np.eye(3)
|
||||||
|
s = 2.0 / n
|
||||||
|
return np.array(
|
||||||
|
[
|
||||||
|
[1 - s * (y * y + z * z), s * (x * y - z * w), s * (x * z + y * w)],
|
||||||
|
[s * (x * y + z * w), 1 - s * (x * x + z * z), s * (y * z - x * w)],
|
||||||
|
[s * (x * z - y * w), s * (y * z + x * w), 1 - s * (x * x + y * y)],
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class Grab(Node):
|
||||||
|
"""抓一帧深度 + 一份内参。"""
|
||||||
|
|
||||||
|
def __init__(self, ns: str) -> None:
|
||||||
|
super().__init__(f"check_sim_cameras_{ns}")
|
||||||
|
self.ns = ns
|
||||||
|
self.img = None
|
||||||
|
self.info = None
|
||||||
|
self.buf = Buffer()
|
||||||
|
self.tl = TransformListener(self.buf, self)
|
||||||
|
# QoS 必须与发布端一致：适配器与真机都用 sensor data，
|
||||||
|
# 用默认 RELIABLE 订阅会一条都收不到（表现是"话题在但 echo 为空"）。
|
||||||
|
self.create_subscription(
|
||||||
|
Image,
|
||||||
|
f"/{ns}/aligned_depth_to_color/image_raw",
|
||||||
|
self._on_img,
|
||||||
|
qos_profile_sensor_data,
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
CameraInfo, f"/{ns}/color/camera_info", self._on_info, qos_profile_sensor_data
|
||||||
|
)
|
||||||
|
|
||||||
|
def _on_img(self, m) -> None:
|
||||||
|
self.img = m
|
||||||
|
|
||||||
|
def _on_info(self, m) -> None:
|
||||||
|
self.info = m
|
||||||
|
|
||||||
|
|
||||||
|
def spin_until(node: Node, pred, timeout: float) -> bool:
|
||||||
|
t0 = time.time()
|
||||||
|
while rclpy.ok() and time.time() - t0 < timeout:
|
||||||
|
rclpy.spin_once(node, timeout_sec=0.3)
|
||||||
|
if pred():
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def expected_intrinsics(ns: str):
|
||||||
|
"""从 cameras.yaml 算出该相机的应有内参。拿不到就返回 None（不阻断检查）。"""
|
||||||
|
try:
|
||||||
|
from arm_sim_gazebo.sim_cameras import intrinsics, load_cameras
|
||||||
|
except ImportError:
|
||||||
|
return None
|
||||||
|
try:
|
||||||
|
for s in load_cameras():
|
||||||
|
if s.ros_ns == ns:
|
||||||
|
return s, intrinsics(s, s.width or 640, s.height or 480)
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def check_one(ns: str, args) -> tuple[bool, list[str]]:
|
||||||
|
"""检查一台相机。返回 (是否通过, 问题列表)。"""
|
||||||
|
problems: list[str] = []
|
||||||
|
node = Grab(ns)
|
||||||
|
|
||||||
|
if not spin_until(
|
||||||
|
node, lambda: node.img is not None and node.info is not None, args.timeout
|
||||||
|
):
|
||||||
|
node.destroy_node()
|
||||||
|
return False, [f"拿不到图像或内参（等 {args.timeout:.0f}s）"]
|
||||||
|
|
||||||
|
frame = node.info.header.frame_id
|
||||||
|
if not spin_until(
|
||||||
|
node, lambda: node.buf.can_transform("world", frame, rclpy.time.Time()), 20.0
|
||||||
|
):
|
||||||
|
node.destroy_node()
|
||||||
|
return False, [f"TF world ← {frame} 查不到（相机在世上没有位姿）"]
|
||||||
|
|
||||||
|
K = np.array(node.info.k, dtype=np.float64).reshape(3, 3)
|
||||||
|
fx, fy, cx, cy = K[0, 0], K[1, 1], K[0, 2], K[1, 2]
|
||||||
|
if not (frame.endswith("_optical_frame") or "optical" in frame):
|
||||||
|
problems.append(
|
||||||
|
f"frame_id 是 {frame}，看着不像光学系（应以 _optical_frame 结尾）。"
|
||||||
|
f"盖成本体系时 tf2 查得到、不报错，但反投影出的点会整体转错。"
|
||||||
|
)
|
||||||
|
|
||||||
|
mm = np.frombuffer(node.img.data, dtype=np.uint16)
|
||||||
|
if mm.size != node.img.height * node.img.width:
|
||||||
|
node.destroy_node()
|
||||||
|
return False, [
|
||||||
|
f"深度图像不是 16UC1 单通道（{node.img.encoding}, "
|
||||||
|
f"{len(node.img.data)} 字节 / 期望 {node.img.height * node.img.width * 2}）"
|
||||||
|
f"—— gz 的 32FC1 米必须转成 16UC1 毫米，这一步没生效"
|
||||||
|
]
|
||||||
|
mm = mm.reshape(node.img.height, node.img.width)
|
||||||
|
|
||||||
|
# 与 cameras.yaml 推导出的应有内参比对：这能直接抓住"gz 的默认内参被透传"
|
||||||
|
# 那个坑（它会给出 fx=277, cx=160, cy=120）。
|
||||||
|
exp = expected_intrinsics(ns)
|
||||||
|
if exp is not None:
|
||||||
|
spec, (exp_k, _) = exp
|
||||||
|
if abs(exp_k[0] - fx) > 1.0 or abs(exp_k[2] - cx) > 1.0:
|
||||||
|
problems.append(
|
||||||
|
f"内参不是按 cameras.yaml 算出来的：实测 fx={fx:.1f} cx={cx:.1f}，"
|
||||||
|
f"应为 fx={exp_k[0]:.1f} cx={exp_k[2]:.1f}"
|
||||||
|
f"（视场 {spec.horizontal_fov}）。\n"
|
||||||
|
f" 这正是 gz 透传默认内参（fx=277, cx=160）时的症状，"
|
||||||
|
f"会让所有三维点位偏半米量级。"
|
||||||
|
)
|
||||||
|
|
||||||
|
valid = (mm > 200) & (mm < 8000)
|
||||||
|
vs, us = np.nonzero(valid)
|
||||||
|
if len(us) < 500:
|
||||||
|
node.destroy_node()
|
||||||
|
return False, [f"有效深度像素太少（{len(us)} 个），图像基本是空的"]
|
||||||
|
rng = np.random.default_rng(0)
|
||||||
|
if len(us) > args.max_pixels:
|
||||||
|
sel = rng.choice(len(us), args.max_pixels, replace=False)
|
||||||
|
us, vs = us[sel], vs[sel]
|
||||||
|
|
||||||
|
Z = mm[vs, us].astype(np.float64) / 1000.0
|
||||||
|
pts = np.stack([(us - cx) * Z / fx, (vs - cy) * Z / fy, Z], axis=1)
|
||||||
|
|
||||||
|
tf = node.buf.lookup_transform("world", frame, rclpy.time.Time())
|
||||||
|
R = quat_to_R(
|
||||||
|
(
|
||||||
|
tf.transform.rotation.x,
|
||||||
|
tf.transform.rotation.y,
|
||||||
|
tf.transform.rotation.z,
|
||||||
|
tf.transform.rotation.w,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
t = np.array(
|
||||||
|
[
|
||||||
|
tf.transform.translation.x,
|
||||||
|
tf.transform.translation.y,
|
||||||
|
tf.transform.translation.z,
|
||||||
|
]
|
||||||
|
)
|
||||||
|
zw = (pts @ R.T + t)[:, 2]
|
||||||
|
node.destroy_node()
|
||||||
|
|
||||||
|
print(f"\n[{ns}] 光学系={frame} fx={fx:.1f} fy={fy:.1f} cx={cx:.1f} cy={cy:.1f}")
|
||||||
|
print(f" world-z 中位={np.median(zw):+.3f} m 范围[{zw.min():+.2f}, {zw.max():+.2f}]")
|
||||||
|
|
||||||
|
# 逐平面命中率
|
||||||
|
rates = {}
|
||||||
|
for pz, name in args.planes:
|
||||||
|
rate = float(np.mean(np.abs(zw - pz) < args.tol)) * 100.0
|
||||||
|
rates[pz] = rate
|
||||||
|
print(f" 命中 {name}(z={pz:+.2f}) : {rate:5.1f}%")
|
||||||
|
|
||||||
|
# 期望判据
|
||||||
|
for want_z, want_min in args.expect.get(ns, []):
|
||||||
|
name = dict(args.planes).get(want_z, f"z={want_z}")
|
||||||
|
got = rates.get(want_z, 0.0)
|
||||||
|
if got < want_min:
|
||||||
|
problems.append(
|
||||||
|
f"只看见 {name} 的 {got:.1f}%，要求 ≥{want_min:.0f}%。\n"
|
||||||
|
f" 相机很可能是**背对**目标（位姿 rpy 写错）—— "
|
||||||
|
f"这种错几何自洽、不报错，只会让相机拍不到该拍的东西。"
|
||||||
|
)
|
||||||
|
|
||||||
|
total = sum(rates.values())
|
||||||
|
if total < 1.0:
|
||||||
|
problems.append(f"所有预期平面加起来命中率只有 {total:.1f}%，几乎没有指向场景")
|
||||||
|
|
||||||
|
ok = not problems
|
||||||
|
print(f" → {'✓ 通过' if ok else '✘ 不通过'}")
|
||||||
|
for p in problems:
|
||||||
|
print(f" ✘ {p}")
|
||||||
|
return ok, problems
|
||||||
|
|
||||||
|
|
||||||
|
def parse_planes(text: str):
|
||||||
|
out = []
|
||||||
|
for item in text.split(","):
|
||||||
|
item = item.strip()
|
||||||
|
if not item:
|
||||||
|
continue
|
||||||
|
if ":" in item:
|
||||||
|
z, name = item.split(":", 1)
|
||||||
|
else:
|
||||||
|
z, name = item, f"z={item}"
|
||||||
|
out.append((float(z), name.strip()))
|
||||||
|
if not out:
|
||||||
|
raise argparse.ArgumentTypeError("--planes 不能为空")
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def parse_expect(items):
|
||||||
|
"""把 'd435:0.40:10' 解析成 {ns: [(z, min_pct), ...]}。"""
|
||||||
|
out: dict[str, list[tuple[float, float]]] = {}
|
||||||
|
for it in items:
|
||||||
|
parts = it.split(":")
|
||||||
|
if len(parts) < 2:
|
||||||
|
raise argparse.ArgumentTypeError(f"--expect 格式应为 相机:z[:最低百分比]，收到 {it!r}")
|
||||||
|
ns = parts[0]
|
||||||
|
z = float(parts[1])
|
||||||
|
pct = float(parts[2]) if len(parts) > 2 else 5.0
|
||||||
|
out.setdefault(ns, []).append((z, pct))
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
ap = argparse.ArgumentParser(
|
||||||
|
description="仿真相机几何验收：反投影深度，检查是否命中预期平面。",
|
||||||
|
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||||
|
)
|
||||||
|
ap.add_argument("--cameras", default="d405,d435", help="要检查的相机命名空间，逗号分隔")
|
||||||
|
ap.add_argument(
|
||||||
|
"--planes",
|
||||||
|
type=parse_planes,
|
||||||
|
default=parse_planes("0.0:地面,0.40:台面"),
|
||||||
|
help="要统计命中率的平面，格式 z:名称，逗号分隔（默认 0.0:地面,0.40:台面）",
|
||||||
|
)
|
||||||
|
ap.add_argument(
|
||||||
|
"--expect",
|
||||||
|
action="append",
|
||||||
|
dest="expect_raw",
|
||||||
|
default=None,
|
||||||
|
help="要求某相机命中某平面，格式 相机:z[:最低百分比]，可重复。"
|
||||||
|
f"默认 {list(DEFAULT_EXPECT)}",
|
||||||
|
)
|
||||||
|
ap.add_argument("--tol", type=float, default=0.02, help="平面命中容差（米），默认 0.02")
|
||||||
|
ap.add_argument("--timeout", type=float, default=25.0, help="等图像/内参的秒数")
|
||||||
|
ap.add_argument("--max-pixels", type=int, default=40000, help="最多采样多少像素")
|
||||||
|
args = ap.parse_args()
|
||||||
|
args.expect = parse_expect(args.expect_raw or list(DEFAULT_EXPECT))
|
||||||
|
|
||||||
|
rclpy.init()
|
||||||
|
try:
|
||||||
|
results = {}
|
||||||
|
for ns in [c.strip() for c in args.cameras.split(",") if c.strip()]:
|
||||||
|
try:
|
||||||
|
results[ns] = check_one(ns, args)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
raise
|
||||||
|
except Exception as exc: # 单台相机出错不该拖垮整轮检查
|
||||||
|
print(f"\n[{ns}] ✘ 检查时异常：{type(exc).__name__}: {exc}")
|
||||||
|
results[ns] = (False, [str(exc)])
|
||||||
|
finally:
|
||||||
|
rclpy.try_shutdown()
|
||||||
|
|
||||||
|
failed = [ns for ns, (ok, _) in results.items() if not ok]
|
||||||
|
print()
|
||||||
|
if failed:
|
||||||
|
print(f"=== ✘ 不通过：{', '.join(failed)} ===")
|
||||||
|
print(" 仿真里相机出错几乎都不报错，别只看'有没有图像'。")
|
||||||
|
return 1
|
||||||
|
print(f"=== ✓ 全部通过：{', '.join(results)} ===")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
sys.exit(main())
|
||||||
@@ -33,10 +33,23 @@ from .robot_description import (
|
|||||||
repo_root,
|
repo_root,
|
||||||
world_name,
|
world_name,
|
||||||
)
|
)
|
||||||
|
from .sim_site import (
|
||||||
|
SimSiteError,
|
||||||
|
load_sim_site,
|
||||||
|
physics_enabled_in_world,
|
||||||
|
resolve_world,
|
||||||
|
unwired_warnings,
|
||||||
|
)
|
||||||
|
|
||||||
LAUNCH_PKG = "arm_sim_gazebo"
|
LAUNCH_PKG = "arm_sim_gazebo"
|
||||||
LAUNCH_FILE = "gazebo_sim.launch.py"
|
LAUNCH_FILE = "gazebo_sim.launch.py"
|
||||||
|
|
||||||
|
# 仿真实际使用的关节前缀，用来跟 site.yaml 的 sim.joint_prefix 比对。
|
||||||
|
# 它由 arm.sim.urdf.xacro 的 arm_prefix 默认值决定，并且 controllers.yaml
|
||||||
|
# 里硬编码了 sim_joint1..7，所以暂时**不可配置** —— 改了这里不改那两处会散架。
|
||||||
|
# site.yaml 若写了一个不一样的值，unwired_warnings() 会告警，而不是静默忽略。
|
||||||
|
ACTUAL_JOINT_PREFIX = "sim_"
|
||||||
|
|
||||||
|
|
||||||
def _build_launch_cmd(extra: List[str]) -> List[str]:
|
def _build_launch_cmd(extra: List[str]) -> List[str]:
|
||||||
# 用 python -m 调 ros2 而不是 exec "ros2"：这样在 ros2 CLI 不在 PATH 的
|
# 用 python -m 调 ros2 而不是 exec "ros2"：这样在 ros2 CLI 不在 PATH 的
|
||||||
@@ -95,8 +108,12 @@ def main(argv: Optional[List[str]] = None) -> int:
|
|||||||
root = repo_root()
|
root = repo_root()
|
||||||
xacro_path = default_xacro(root)
|
xacro_path = default_xacro(root)
|
||||||
controllers = default_controllers(root)
|
controllers = default_controllers(root)
|
||||||
world = args.world or str(default_world(root))
|
# world 的优先级：--world > site.yaml 的 sim.world > 内置默认。
|
||||||
|
# 以前这里只认 --world 和硬编码默认值，而 site.yaml / manifest.yaml
|
||||||
|
# 都宣称改 sim.world 就生效 —— 结果改了毫无反应且不报错。
|
||||||
|
world, world_origin = resolve_world(args.world, root, fallback=default_world(root))
|
||||||
|
world = str(world)
|
||||||
|
site = load_sim_site(root)
|
||||||
urdf, report = prepare_urdf(
|
urdf, report = prepare_urdf(
|
||||||
xacro_path=xacro_path,
|
xacro_path=xacro_path,
|
||||||
controllers_file=controllers,
|
controllers_file=controllers,
|
||||||
@@ -105,10 +122,19 @@ def main(argv: Optional[List[str]] = None) -> int:
|
|||||||
)
|
)
|
||||||
wname = world_name(world)
|
wname = world_name(world)
|
||||||
missing = check_base_plugins(Path(world))
|
missing = check_base_plugins(Path(world))
|
||||||
except SimDescriptionError as exc:
|
except (SimDescriptionError, SimSiteError) as exc:
|
||||||
print(f"[gazebo_sim_launcher] 失败：\n{exc}", file=sys.stderr)
|
print(f"[gazebo_sim_launcher] 失败：\n{exc}", file=sys.stderr)
|
||||||
return 2
|
return 2
|
||||||
|
|
||||||
|
# site.yaml 里"声明了但没接线"的键：与其静默无效，不如明说。
|
||||||
|
# 见 sim_site.unwired_warnings() —— 那里逐条说明了为什么还没接。
|
||||||
|
for _w in unwired_warnings(
|
||||||
|
site,
|
||||||
|
actual_joint_prefix=ACTUAL_JOINT_PREFIX,
|
||||||
|
physics_in_world=physics_enabled_in_world(Path(world)),
|
||||||
|
):
|
||||||
|
print(f"[gazebo_sim_launcher] ⚠ {_w}", file=sys.stderr)
|
||||||
|
|
||||||
if missing:
|
if missing:
|
||||||
# 这一条能省掉一整个下午：world 漏声明基础系统时，仿真看起来完全正常
|
# 这一条能省掉一整个下午：world 漏声明基础系统时，仿真看起来完全正常
|
||||||
# （/clock 在走、create 打印成功），但机械臂从未进入 Gazebo。
|
# （/clock 在走、create 打印成功），但机械臂从未进入 Gazebo。
|
||||||
@@ -135,6 +161,11 @@ def main(argv: Optional[List[str]] = None) -> int:
|
|||||||
print(f" 描述 : {xacro_path}")
|
print(f" 描述 : {xacro_path}")
|
||||||
print(f" 控制器配置 : {controllers}")
|
print(f" 控制器配置 : {controllers}")
|
||||||
print(f" 世界 : {world} (world 名: {wname})")
|
print(f" 世界 : {world} (world 名: {wname})")
|
||||||
|
print(f" 世界来源 : {world_origin}")
|
||||||
|
if site.source is not None:
|
||||||
|
print(f" site.yaml : {site.source}")
|
||||||
|
else:
|
||||||
|
print(" site.yaml : 不存在（用默认值；这不会影响仿真）")
|
||||||
print(f" URDF 归一化 : {report.summary()}")
|
print(f" URDF 归一化 : {report.summary()}")
|
||||||
if missing:
|
if missing:
|
||||||
print(
|
print(
|
||||||
|
|||||||
@@ -20,6 +20,7 @@ devices/ **不是 ROS 包**，所以 $(find ...) 找不到它，必须用文件
|
|||||||
"""
|
"""
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
import os
|
import os
|
||||||
import re
|
import re
|
||||||
import subprocess
|
import subprocess
|
||||||
@@ -107,6 +108,183 @@ def world_name(world_file: Path) -> str:
|
|||||||
return m.group(1)
|
return m.group(1)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 从 world 文件里读模型位姿
|
||||||
|
# =============================================================================
|
||||||
|
# 为什么需要：Gazebo 里的 <model>（尤其是 static 的工装/相机支架）**不会**产生
|
||||||
|
# ROS TF —— TF 只来自 robot_state_publisher。但应用要做 tf2 查询，比如
|
||||||
|
# "把 D435 看到的点从相机系转到 world"。少了这条 TF，查询直接报
|
||||||
|
# "frame does not exist"，而图像本身看起来完全正常。
|
||||||
|
#
|
||||||
|
# 为什么是"解析"而不是"再配置一遍"：位姿的唯一真相就在 world 文件里。
|
||||||
|
# 另配一份等于制造"改了 world 忘了改配置"的机会，而这种不一致不会报错，
|
||||||
|
# 只会让相机看到的东西和它自称的位置对不上 —— 极难查。
|
||||||
|
def _find_block(text: str, tag: str, name: str) -> Optional[str]:
|
||||||
|
"""取出 <tag name="name">...</tag> 的内容（考虑同名标签嵌套）。
|
||||||
|
|
||||||
|
SDF 里 <model> 可以嵌套 <model>（组合体），<link> 里也可能有 <link>（少见）。
|
||||||
|
只找第一个 </tag> 在嵌套时会截断到错误的位置，所以这里数嵌套深度。
|
||||||
|
"""
|
||||||
|
pat = re.compile(
|
||||||
|
r"<%(t)s\b[^>]*\bname\s*=\s*[\"']%(n)s[\"'][^>]*>" % {"t": tag, "n": re.escape(name)}
|
||||||
|
)
|
||||||
|
m = pat.search(text)
|
||||||
|
if not m:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# 自闭合：<link name="l"/> 是完全合法的 SDF（没有子元素）。
|
||||||
|
# 不特判的话会走到下面的"找 </link>"分支、找不到 → 返回 None，
|
||||||
|
# 于是被误报成"这个 link 不存在"，而它明明在。
|
||||||
|
if m.group(0).rstrip().endswith("/>"):
|
||||||
|
return ""
|
||||||
|
|
||||||
|
inner_start = m.end()
|
||||||
|
depth = 1
|
||||||
|
pos = inner_start
|
||||||
|
open_re = re.compile(r"<%(t)s\b" % {"t": tag})
|
||||||
|
close_re = re.compile(r"</%(t)s\s*>" % {"t": tag})
|
||||||
|
while depth > 0:
|
||||||
|
o = open_re.search(text, pos)
|
||||||
|
c = close_re.search(text, pos)
|
||||||
|
if c is None:
|
||||||
|
return None # 标签没闭合 → 文件本身有问题，交给别的检查去报
|
||||||
|
if o is not None and o.start() < c.start():
|
||||||
|
depth += 1
|
||||||
|
pos = o.end()
|
||||||
|
else:
|
||||||
|
depth -= 1
|
||||||
|
if depth == 0:
|
||||||
|
return text[inner_start : c.start()]
|
||||||
|
pos = c.end()
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def _own_pose(block: str, stop_before: Tuple[str, ...]) -> Tuple[float, float, float, float, float, float]:
|
||||||
|
"""取一段块自己的 <pose>（默认全 0）。
|
||||||
|
|
||||||
|
只认**第一个** <pose>，且必须出现在 stop_before 里任一标签之前 ——
|
||||||
|
这样才不会把子元素的 pose 误当成父元素的。例如 <model> 的 pose 必须在
|
||||||
|
第一个 <link>/<model>/<joint> 之前；<link> 的 pose 必须在第一个
|
||||||
|
<visual>/<collision>/<sensor> 之前。
|
||||||
|
"""
|
||||||
|
cut = len(block)
|
||||||
|
for tag in stop_before:
|
||||||
|
i = block.find("<" + tag)
|
||||||
|
if 0 <= i < cut:
|
||||||
|
cut = i
|
||||||
|
m = re.search(r"<pose(?:\s[^>]*)?>([^<]*)</pose>", block[:cut])
|
||||||
|
if not m:
|
||||||
|
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||||
|
vals = m.group(1).split()
|
||||||
|
if len(vals) < 6:
|
||||||
|
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||||
|
try:
|
||||||
|
return tuple(float(v) for v in vals[:6]) # type: ignore[return-value]
|
||||||
|
except ValueError:
|
||||||
|
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||||
|
|
||||||
|
|
||||||
|
def _rpy_to_quat(r: float, p: float, y: float) -> Tuple[float, float, float, float]:
|
||||||
|
cr, sr = math.cos(r / 2), math.sin(r / 2)
|
||||||
|
cp, sp = math.cos(p / 2), math.sin(p / 2)
|
||||||
|
cy, sy = math.cos(y / 2), math.sin(y / 2)
|
||||||
|
return (
|
||||||
|
sr * cp * cy - cr * sp * sy,
|
||||||
|
cr * sp * cy + sr * cp * sy,
|
||||||
|
cr * cp * sy - sr * sp * cy,
|
||||||
|
cr * cp * cy + sr * sp * sy,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _quat_to_rpy(q: Tuple[float, float, float, float]) -> Tuple[float, float, float]:
|
||||||
|
x, y, z, w = q
|
||||||
|
# roll (x 轴)
|
||||||
|
sinr = 2 * (w * x + y * z)
|
||||||
|
cosr = 1 - 2 * (x * x + y * y)
|
||||||
|
roll = math.atan2(sinr, cosr)
|
||||||
|
# pitch (y 轴)：用 asin，并夹到 [-1,1] 免得浮点误差导致 domain error
|
||||||
|
sinp = 2 * (w * y - z * x)
|
||||||
|
sinp = max(-1.0, min(1.0, sinp))
|
||||||
|
pitch = math.asin(sinp)
|
||||||
|
# yaw (z 轴)
|
||||||
|
siny = 2 * (w * z + x * y)
|
||||||
|
cosy = 1 - 2 * (y * y + z * z)
|
||||||
|
yaw = math.atan2(siny, cosy)
|
||||||
|
return (roll, pitch, yaw)
|
||||||
|
|
||||||
|
|
||||||
|
def _quat_mul(a, b):
|
||||||
|
ax, ay, az, aw = a
|
||||||
|
bx, by, bz, bw = b
|
||||||
|
return (
|
||||||
|
aw * bx + ax * bw + ay * bz - az * by,
|
||||||
|
aw * by - ax * bz + ay * bw + az * bx,
|
||||||
|
aw * bz + ax * by - ay * bx + az * bw,
|
||||||
|
aw * bw - ax * bx - ay * by - az * bz,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _rotate(q, v):
|
||||||
|
"""用四元数旋转向量 v（v 在子系里，返回在父系里的表达）。"""
|
||||||
|
x, y, z, w = q
|
||||||
|
vx, vy, vz = v
|
||||||
|
# t = 2 * (q_vec × v)
|
||||||
|
tx = 2 * (y * vz - z * vy)
|
||||||
|
ty = 2 * (z * vx - x * vz)
|
||||||
|
tz = 2 * (x * vy - y * vx)
|
||||||
|
# v' = v + w*t + q_vec × t
|
||||||
|
return (
|
||||||
|
vx + w * tx + (y * tz - z * ty),
|
||||||
|
vy + w * ty + (z * tx - x * tz),
|
||||||
|
vz + w * tz + (x * ty - y * tx),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def model_link_pose(
|
||||||
|
world_file: Path, model: str, link: str = ""
|
||||||
|
) -> Tuple[Tuple[float, float, float], Tuple[float, float, float]]:
|
||||||
|
"""返回 world 文件里 <model name=model> 的 <link name=link> 在**世界系**的位姿。
|
||||||
|
|
||||||
|
返回值是 ((x, y, z), (roll, pitch, yaw))。link 留空表示取模型本身的位姿。
|
||||||
|
两者都会做复合（模型位姿 ∘ link 位姿），因为 SDF 的 <pose> 是相对父元素的。
|
||||||
|
|
||||||
|
找不到模型/link 时抛 SimDescriptionError，并提示这里和 cameras.yaml 的
|
||||||
|
world_model 字段必须对应 —— 配错的话相机不会有 TF，而症状是
|
||||||
|
"tf 查询报 frame does not exist"，与相机本身无关，容易查错方向。
|
||||||
|
"""
|
||||||
|
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||||
|
|
||||||
|
mblock = _find_block(text, "model", model)
|
||||||
|
if mblock is None:
|
||||||
|
raise SimDescriptionError(
|
||||||
|
f"{world_file} 里找不到 <model name=\"{model}\">。\n"
|
||||||
|
f" devices/sim/gazebo/config/cameras.yaml 里若有 world_model: {model}，"
|
||||||
|
f"就必须在 world 里真的有这个模型 —— 否则该相机没有 TF，"
|
||||||
|
f"应用做 tf2 查询会报 frame does not exist。"
|
||||||
|
)
|
||||||
|
|
||||||
|
mp = _own_pose(mblock, ("link", "model", "joint", "plugin"))
|
||||||
|
mq = _rpy_to_quat(mp[3], mp[4], mp[5])
|
||||||
|
|
||||||
|
if not link:
|
||||||
|
return ((mp[0], mp[1], mp[2]), (mp[3], mp[4], mp[5]))
|
||||||
|
|
||||||
|
lblock = _find_block(mblock, "link", link)
|
||||||
|
if lblock is None:
|
||||||
|
raise SimDescriptionError(
|
||||||
|
f"{world_file} 的 <model name=\"{model}\"> 里找不到 <link name=\"{link}\">。"
|
||||||
|
)
|
||||||
|
|
||||||
|
lp = _own_pose(lblock, ("visual", "collision", "sensor", "light"))
|
||||||
|
lq = _rpy_to_quat(lp[3], lp[4], lp[5])
|
||||||
|
|
||||||
|
# 复合：世界系下的位置 = t_model + R_model * t_link
|
||||||
|
t = _rotate(mq, (lp[0], lp[1], lp[2]))
|
||||||
|
pos = (mp[0] + t[0], mp[1] + t[1], mp[2] + t[2])
|
||||||
|
rpy = _quat_to_rpy(_quat_mul(mq, lq))
|
||||||
|
return (pos, rpy)
|
||||||
|
|
||||||
|
|
||||||
# 只要 world 里声明了任何一个 <plugin>，Gazebo 就不再加载 server.config 的默认
|
# 只要 world 里声明了任何一个 <plugin>，Gazebo 就不再加载 server.config 的默认
|
||||||
# 系统列表，这三个基础系统必须由 world 自己声明，否则整个仿真静默失效。
|
# 系统列表，这三个基础系统必须由 world 自己声明，否则整个仿真静默失效。
|
||||||
# 详细后果见 assembly.world 里那段注释，以及这里的 check_base_plugins()。
|
# 详细后果见 assembly.world 里那段注释，以及这里的 check_base_plugins()。
|
||||||
|
|||||||
@@ -0,0 +1,309 @@
|
|||||||
|
"""sim_camera_adapter —— 把桥接过来的 Gazebo 相机，变成应用能直接吃的话题。
|
||||||
|
|
||||||
|
=============================================================================
|
||||||
|
它到底解决什么问题
|
||||||
|
=============================================================================
|
||||||
|
ros_gz_bridge 只做"传输"：把 gz 的消息类型翻成 ROS 的消息类型，话题名照抄。
|
||||||
|
它**改不了**两样东西，而这两样恰恰是仿真里最容易错、也最不报错的：
|
||||||
|
|
||||||
|
1. frame_id
|
||||||
|
gz 盖的是内部路径（实测是 rizon_sim/sim_link7/d405_rgbd），
|
||||||
|
而 TF 里真实存在的坐标系叫 sim_d405_link —— 两者对不上。
|
||||||
|
更麻烦的是：即使改成 sim_d405_link 也还是错的，因为那是"相机机身"系
|
||||||
|
（X 前 / Y 左 / Z 上），而 ROS 的 image/camera_info 必须是光学系
|
||||||
|
（Z 前 / X 右 / Y 下）。tf2 查询两种都能查到、都不报错，
|
||||||
|
但用错的那个把深度反投影成三维点会整体转错。
|
||||||
|
所以这里盖的是 <相机>_color_optical_frame，并同时发布
|
||||||
|
link → optical 的静态 TF。
|
||||||
|
|
||||||
|
2. 深度单位
|
||||||
|
gz 是 32FC1 / 米，真机 realsense 的 aligned_depth 是 16UC1 / 毫米。
|
||||||
|
不转换的话应用按毫米解释会整体差 1000 倍，而且 dtype 不符时
|
||||||
|
numpy 读出来直接是垃圾。
|
||||||
|
|
||||||
|
=============================================================================
|
||||||
|
为什么是"桥接 + 适配器"两层，而不是合成一层
|
||||||
|
=============================================================================
|
||||||
|
桥接器改不了 frame_id，所以无论如何都要有第二层做语义转换。
|
||||||
|
既然必须有，就让第二层把"改名 + 坐标系 + 单位"全都集中处理，
|
||||||
|
而不是让桥接器改一半、适配器补一半 —— 那样出问题时不知道去哪找。
|
||||||
|
副产品：gz 原名的话题（/d405/image 等）在 ROS 图里仍然可见，
|
||||||
|
排查"是桥接断了还是适配器断了"时能一刀切开。
|
||||||
|
|
||||||
|
=============================================================================
|
||||||
|
发布的话题（与真机完全一致，应用无需为仿真改任何代码）
|
||||||
|
=============================================================================
|
||||||
|
/<ros_ns>/color/image_raw rgb8
|
||||||
|
/<ros_ns>/color/camera_info plumb_bob（畸变全 0，gz 不模拟畸变）
|
||||||
|
/<ros_ns>/aligned_depth_to_color/image_raw 16UC1 毫米，0 = 无读数
|
||||||
|
|
||||||
|
QoS 用 sensor data（BEST_EFFORT, keep_last 1），与真机 realsense 发布端、
|
||||||
|
以及应用的订阅端（qos_profile_sensor_data）都对齐。这一点不能想当然：
|
||||||
|
发布端 BEST_EFFORT、订阅端 RELIABLE 是不兼容的，表现是"话题在、echo 不到"。
|
||||||
|
"""
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
from typing import Dict, List, Optional
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import rclpy
|
||||||
|
from geometry_msgs.msg import TransformStamped
|
||||||
|
from rclpy.node import Node
|
||||||
|
from rclpy.qos import (
|
||||||
|
DurabilityPolicy,
|
||||||
|
HistoryPolicy,
|
||||||
|
QoSProfile,
|
||||||
|
ReliabilityPolicy,
|
||||||
|
qos_profile_sensor_data,
|
||||||
|
)
|
||||||
|
from sensor_msgs.msg import CameraInfo, Image
|
||||||
|
from tf2_ros import StaticTransformBroadcaster
|
||||||
|
|
||||||
|
from .sim_cameras import (
|
||||||
|
DEFAULT_CAMERAS_REL,
|
||||||
|
CameraConfigError,
|
||||||
|
CameraSpec,
|
||||||
|
depth_m_to_mm16,
|
||||||
|
intrinsics,
|
||||||
|
load_cameras,
|
||||||
|
optical_rpy,
|
||||||
|
rpy_to_quat,
|
||||||
|
)
|
||||||
|
|
||||||
|
# 图像上盖的坐标系（光学系）相对相机本体系的固定旋转。
|
||||||
|
# 见 sim_cameras.OPTICAL_RPY 的说明 —— 这是"看起来正常但三维全错"的那一项。
|
||||||
|
|
||||||
|
|
||||||
|
class SimCameraAdapter(Node):
|
||||||
|
def __init__(self) -> None:
|
||||||
|
super().__init__("sim_camera_adapter")
|
||||||
|
|
||||||
|
self.declare_parameter("cameras_file", "")
|
||||||
|
path = str(self.get_parameter("cameras_file").value or "").strip()
|
||||||
|
|
||||||
|
try:
|
||||||
|
specs = load_cameras(path or None, root=self._repo_root())
|
||||||
|
except Exception as exc: # 配置错误应当直接说清怎么修，而不是抛栈
|
||||||
|
self.get_logger().error(f"\n仿真相机清单读取失败：\n{exc}")
|
||||||
|
raise
|
||||||
|
|
||||||
|
self._specs: List[CameraSpec] = specs
|
||||||
|
self._subs: List[object] = []
|
||||||
|
self._pubs: Dict[str, object] = {}
|
||||||
|
self._warned: set = set()
|
||||||
|
|
||||||
|
# 相机本体 → 光学系的静态 TF。必须在图像到达**之前**就发出去，
|
||||||
|
# 因为 TF 是 transient_local（迟到的订阅者也能拿到），但若从没发过，
|
||||||
|
# 应用查 tf 会直接报 "frame does not exist"。
|
||||||
|
self._tf = StaticTransformBroadcaster(self)
|
||||||
|
self._publish_optical_tfs()
|
||||||
|
|
||||||
|
for s in specs:
|
||||||
|
self._wire(s)
|
||||||
|
|
||||||
|
names = ", ".join(s.ros_ns for s in specs)
|
||||||
|
self.get_logger().info(
|
||||||
|
f"仿真相机适配器就绪: {names}"
|
||||||
|
+ f"（深度 {sum(1 for s in specs if s.depth)}/台 已做 米→毫米 转换）"
|
||||||
|
)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------ 内部
|
||||||
|
def _repo_root(self):
|
||||||
|
"""复用 robot_description 的仓库根定位（它已经处理了三种情形）。"""
|
||||||
|
from .robot_description import repo_root
|
||||||
|
|
||||||
|
try:
|
||||||
|
return repo_root()
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
def _qos(self) -> QoSProfile:
|
||||||
|
# 深拷贝一份 sensor data QoS：共享的那个对象若被改动会影响其它节点。
|
||||||
|
return QoSProfile(
|
||||||
|
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||||
|
history=HistoryPolicy.KEEP_LAST,
|
||||||
|
depth=qos_profile_sensor_data.depth,
|
||||||
|
)
|
||||||
|
|
||||||
|
def _publish_optical_tfs(self) -> None:
|
||||||
|
"""发布 link → optical 的静态 TF（每台相机一条，平移为零）。"""
|
||||||
|
msgs = []
|
||||||
|
for s in self._specs:
|
||||||
|
t = TransformStamped()
|
||||||
|
t.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
t.header.frame_id = s.link_frame
|
||||||
|
t.child_frame_id = s.optical_frame
|
||||||
|
t.transform.translation.x = 0.0
|
||||||
|
t.transform.translation.y = 0.0
|
||||||
|
t.transform.translation.z = 0.0
|
||||||
|
qx, qy, qz, qw = rpy_to_quat(optical_rpy())
|
||||||
|
t.transform.rotation.x = qx
|
||||||
|
t.transform.rotation.y = qy
|
||||||
|
t.transform.rotation.z = qz
|
||||||
|
t.transform.rotation.w = qw
|
||||||
|
msgs.append(t)
|
||||||
|
self._tf.sendTransform(msgs)
|
||||||
|
|
||||||
|
def _wire(self, s: CameraSpec) -> None:
|
||||||
|
qos = self._qos()
|
||||||
|
|
||||||
|
self._pubs[s.ros_color] = self.create_publisher(Image, s.ros_color, qos)
|
||||||
|
self._pubs[s.ros_info] = self.create_publisher(CameraInfo, s.ros_info, qos)
|
||||||
|
if s.depth:
|
||||||
|
self._pubs[s.ros_depth] = self.create_publisher(Image, s.ros_depth, qos)
|
||||||
|
|
||||||
|
self._subs.append(
|
||||||
|
self.create_subscription(
|
||||||
|
Image, s.gz_image, lambda m, sp=s: self._on_color(sp, m), qos
|
||||||
|
)
|
||||||
|
)
|
||||||
|
self._subs.append(
|
||||||
|
self.create_subscription(
|
||||||
|
CameraInfo, s.gz_info, lambda m, sp=s: self._on_info(sp, m), qos
|
||||||
|
)
|
||||||
|
)
|
||||||
|
if s.depth:
|
||||||
|
self._subs.append(
|
||||||
|
self.create_subscription(
|
||||||
|
Image, s.gz_depth, lambda m, sp=s: self._on_depth(sp, m), qos
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
def _restamp(self, msg, frame_id: str) -> None:
|
||||||
|
"""把坐标系换掉。就地改，避免每帧多一次消息构造与一次大数组拷贝。"""
|
||||||
|
if msg.header.frame_id != frame_id:
|
||||||
|
msg.header.frame_id = frame_id
|
||||||
|
|
||||||
|
def _on_color(self, s: CameraSpec, msg: Image) -> None:
|
||||||
|
self._note_encoding(s, msg)
|
||||||
|
self._restamp(msg, s.optical_frame)
|
||||||
|
self._pubs[s.ros_color].publish(msg)
|
||||||
|
|
||||||
|
def _on_info(self, s: CameraSpec, msg: CameraInfo) -> None:
|
||||||
|
"""重算并覆盖内参 —— gz 发的那份是错的，不能透传。
|
||||||
|
|
||||||
|
本机实测：gz 恒发 fx=fy=277, cx=160, cy=120（等于它的默认 60° 视场 +
|
||||||
|
半分辨率主点），而渲染用的是配置的 87° 视场与 640x480。
|
||||||
|
照抄它做三维反投影会错半米量级，且单目预览里完全看不出来。
|
||||||
|
详见 sim_cameras.intrinsics() 的说明。
|
||||||
|
"""
|
||||||
|
w = int(msg.width) or int(s.width)
|
||||||
|
h = int(msg.height) or int(s.height)
|
||||||
|
try:
|
||||||
|
k, p = intrinsics(s, w, h)
|
||||||
|
except CameraConfigError as exc:
|
||||||
|
key = ("info", s.ros_ns)
|
||||||
|
if key not in self._warned:
|
||||||
|
self._warned.add(key)
|
||||||
|
self.get_logger().error(f"{s.ros_ns}: 内参算不出来，{s.ros_info} 无输出。{exc}")
|
||||||
|
return
|
||||||
|
|
||||||
|
key = ("info", s.ros_ns)
|
||||||
|
if key not in self._warned:
|
||||||
|
self._warned.add(key)
|
||||||
|
gz_k = list(msg.k) if len(msg.k) == 9 else []
|
||||||
|
self.get_logger().info(
|
||||||
|
f"{s.ros_ns}: 内参已按视场 {s.horizontal_fov:.4f} rad "
|
||||||
|
f"({math.degrees(s.horizontal_fov):.1f}°) 与 {w}x{h} 重算 —— "
|
||||||
|
f"fx={k[0]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}"
|
||||||
|
+ (
|
||||||
|
f"（覆盖 gz 的 fx={gz_k[0]:.1f} cx={gz_k[2]:.1f} cy={gz_k[5]:.1f}）"
|
||||||
|
if gz_k
|
||||||
|
else ""
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
out = CameraInfo()
|
||||||
|
out.header = msg.header
|
||||||
|
out.header.frame_id = s.optical_frame
|
||||||
|
out.height = h
|
||||||
|
out.width = w
|
||||||
|
out.distortion_model = "plumb_bob"
|
||||||
|
# gz 不模拟镜头畸变，所以畸变系数全 0、R 为单位阵。
|
||||||
|
# 这意味着"仿真里用畸变矫正调出来的参数"在真机上仍需重调 —— 已在
|
||||||
|
# devices/sim/gazebo/README.md 里说明。
|
||||||
|
out.d = [0.0] * 5
|
||||||
|
out.k = list(k)
|
||||||
|
out.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||||
|
out.p = list(p)
|
||||||
|
self._pubs[s.ros_info].publish(out)
|
||||||
|
|
||||||
|
def _on_depth(self, s: CameraSpec, msg: Image) -> None:
|
||||||
|
"""32FC1 米 → 16UC1 毫米。
|
||||||
|
|
||||||
|
用 frombuffer 而不是 array(...)：后者会把整个 buffer 再拷一遍。
|
||||||
|
这一步每帧都在跑（30Hz × 640×480×4B），能省则省。
|
||||||
|
"""
|
||||||
|
if msg.encoding != "32FC1":
|
||||||
|
# 只提醒一次，避免刷屏
|
||||||
|
key = (s.ros_ns, msg.encoding)
|
||||||
|
if key not in self._warned:
|
||||||
|
self._warned.add(key)
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"{s.gz_depth} 的编码是 {msg.encoding}，不是预期的 32FC1。"
|
||||||
|
f"已跳过本次转换 —— 深度话题会没有输出。"
|
||||||
|
f"若 Gazebo 版本改动了深度编码，需要同步改 sim_cameras."
|
||||||
|
f"depth_m_to_mm16 的调用处。"
|
||||||
|
)
|
||||||
|
return
|
||||||
|
|
||||||
|
arr = np.frombuffer(msg.data, dtype=np.float32).reshape(msg.height, msg.width)
|
||||||
|
mm = depth_m_to_mm16(arr)
|
||||||
|
|
||||||
|
out = Image()
|
||||||
|
out.header = msg.header
|
||||||
|
out.header.frame_id = s.optical_frame
|
||||||
|
out.height = msg.height
|
||||||
|
out.width = msg.width
|
||||||
|
out.encoding = "16UC1"
|
||||||
|
# gz 给的是连续 buffer；转成 uint16 后每像素 2 字节，step 必须同步改，
|
||||||
|
# 否则下游按 RGB 的 stride 解读会错位。
|
||||||
|
out.is_bigendian = 0
|
||||||
|
out.step = msg.width * 2
|
||||||
|
out.data = mm.tobytes()
|
||||||
|
self._pubs[s.ros_depth].publish(out)
|
||||||
|
|
||||||
|
def _note_encoding(self, s: CameraSpec, msg: Image) -> None:
|
||||||
|
"""第一次收到彩色帧时把编码打出来。
|
||||||
|
|
||||||
|
为什么值得单独打印：gz 给的是 rgb8（实测），而真机 realsense 的
|
||||||
|
color/image_raw 也是 rgb8 —— 一致，所以不需要转换。但这一点是实测
|
||||||
|
结论，不是保证；换个 Gazebo 版本或传感器配置可能变成 bgr8。
|
||||||
|
打一行日志，使"颜色不对"时能立刻定位到是不是编码变了。
|
||||||
|
"""
|
||||||
|
key = ("enc", s.ros_ns, msg.encoding)
|
||||||
|
if key not in self._warned:
|
||||||
|
self._warned.add(key)
|
||||||
|
self.get_logger().info(
|
||||||
|
f"{s.ros_ns}: 彩色 {msg.width}x{msg.height} encoding={msg.encoding} "
|
||||||
|
f"→ {s.ros_color} (frame={s.optical_frame})"
|
||||||
|
)
|
||||||
|
if msg.encoding not in ("rgb8", "bgr8"):
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"{s.ros_ns} 的彩色编码是 {msg.encoding}，既不是 rgb8 也不是 "
|
||||||
|
f"bgr8。应用按 rgb8 解释会得到错误的颜色。"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def main(argv: Optional[List[str]] = None) -> int:
|
||||||
|
rclpy.init(args=argv)
|
||||||
|
try:
|
||||||
|
node = SimCameraAdapter()
|
||||||
|
except Exception:
|
||||||
|
# 参数/配置错误时 rclpy 已经打印了原因；这里只需保证退出码非零，
|
||||||
|
# 让 launch 能把它当成失败（否则会留下一个没有相机的"成功"启动）。
|
||||||
|
rclpy.try_shutdown()
|
||||||
|
return 2
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.try_shutdown()
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,394 @@
|
|||||||
|
"""仿真相机的配置解析与格式换算。
|
||||||
|
|
||||||
|
=============================================================================
|
||||||
|
为什么单独一个模块
|
||||||
|
=============================================================================
|
||||||
|
"把 Gazebo 的相机输出变成应用能直接吃的话题"这件事里，真正有内容的是三件事：
|
||||||
|
|
||||||
|
1. 深度单位换算 32FC1 米 → 16UC1 毫米（且 0 表示无读数）
|
||||||
|
2. 坐标系盖章 图像要盖**光学系**，不能盖相机本体系
|
||||||
|
3. 话题改名 /d405/image → /d405/color/image_raw 等
|
||||||
|
|
||||||
|
这三件事全是纯数据变换，不需要 ROS 跑起来就能验。所以实现放在这里、
|
||||||
|
用普通函数表达，节点（sim_camera_adapter.py）只做订阅/发布这层壳。
|
||||||
|
好处是换算逻辑可以被单测钉死 —— 深度单位差 1000 倍这种错误，
|
||||||
|
在单目预览里肉眼看不出任何异常，只能靠测试兜住。
|
||||||
|
|
||||||
|
另外，启动文件里生成 ros_gz_bridge 参数也要用同一份相机清单，
|
||||||
|
所以清单的解析也放在这里，避免"桥接用的清单"和"适配器用的清单"各写一份。
|
||||||
|
"""
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
import re
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Dict, List, Optional, Sequence, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import yaml
|
||||||
|
|
||||||
|
# 默认配置位置（相对仓库根）。与其它 devices/ 下的配置一样，是**物料**而非代码。
|
||||||
|
DEFAULT_CAMERAS_REL = "devices/sim/gazebo/config/cameras.yaml"
|
||||||
|
|
||||||
|
# 相机本体系（X 前 / Y 左 / Z 上）→ 光学系（Z 前 / X 右 / Y 下）的固定旋转。
|
||||||
|
# 这就是真机 realsense 的 <相机>_color_optical_frame 相对其 link 用的一组角，
|
||||||
|
# 且是 ROS 的惯例（REP-103 / REP-105）。
|
||||||
|
#
|
||||||
|
# ⚠️ 这一步很容易被省掉，而且省掉之后**什么都不会报错**：
|
||||||
|
# 应用拿 image 的 frame_id 去做 tf2 查询，本体系是存在的，查询成功；
|
||||||
|
# 但那是"相机机身"的朝向，用它把深度反投影成三维点会整体转错。
|
||||||
|
# 单目预览看起来完全正常，只有把点投回世界坐标时才露馅 —— 所以
|
||||||
|
# 专门有一个端到端测试把"深度反投影后应落在已知平面"作为断言。
|
||||||
|
OPTICAL_RPY: Tuple[float, float, float] = (-1.5708, 0.0, -1.5708)
|
||||||
|
|
||||||
|
# uint16 能表示的最大毫米数（65535 mm = 65.535 m）。
|
||||||
|
# 更远的读数按"无读数"处理（0），而不是饱和到 65535 ——
|
||||||
|
# 饱和会让远处凭空出现一个 65.5m 的平面，比"无效"更有害。
|
||||||
|
MAX_MM = 65535
|
||||||
|
|
||||||
|
|
||||||
|
class CameraConfigError(RuntimeError):
|
||||||
|
"""配置有问题时抛出，消息面向使用者。"""
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CameraSpec:
|
||||||
|
"""一台仿真相机的声明。"""
|
||||||
|
|
||||||
|
gz_ns: str
|
||||||
|
ros_ns: str
|
||||||
|
link_frame: str
|
||||||
|
optical_frame: str
|
||||||
|
depth: bool = True
|
||||||
|
horizontal_fov: float = 0.0
|
||||||
|
world_model: str = ""
|
||||||
|
world_link: str = "link"
|
||||||
|
width: int = 0
|
||||||
|
height: int = 0
|
||||||
|
|
||||||
|
# --- Gazebo 侧（ros_gz_bridge 的输入）-----------------------------------
|
||||||
|
@property
|
||||||
|
def gz_image(self) -> str:
|
||||||
|
return f"{self.gz_ns}/image"
|
||||||
|
|
||||||
|
@property
|
||||||
|
def gz_depth(self) -> str:
|
||||||
|
return f"{self.gz_ns}/depth_image"
|
||||||
|
|
||||||
|
@property
|
||||||
|
def gz_info(self) -> str:
|
||||||
|
return f"{self.gz_ns}/camera_info"
|
||||||
|
|
||||||
|
# --- ROS 侧（应用按这些名字订阅，与真机 realsense 一致）------------------
|
||||||
|
@property
|
||||||
|
def ros_color(self) -> str:
|
||||||
|
return f"/{self.ros_ns}/color/image_raw"
|
||||||
|
|
||||||
|
@property
|
||||||
|
def ros_info(self) -> str:
|
||||||
|
return f"/{self.ros_ns}/color/camera_info"
|
||||||
|
|
||||||
|
@property
|
||||||
|
def ros_depth(self) -> str:
|
||||||
|
return f"/{self.ros_ns}/aligned_depth_to_color/image_raw"
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_world_mounted(self) -> bool:
|
||||||
|
"""挂在世界模型上（而非机器人本体）→ 需要额外补一条静态 TF。"""
|
||||||
|
return bool(self.world_model)
|
||||||
|
|
||||||
|
|
||||||
|
def load_cameras(path: Optional[Path] = None, root: Optional[Path] = None) -> List[CameraSpec]:
|
||||||
|
"""读相机清单。找不到或写错时抛 CameraConfigError，消息里给出修法。"""
|
||||||
|
if path is None:
|
||||||
|
base = root if root is not None else Path.cwd()
|
||||||
|
path = Path(base) / DEFAULT_CAMERAS_REL
|
||||||
|
path = Path(path)
|
||||||
|
|
||||||
|
if not path.is_file():
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"相机清单不存在: {path}\n"
|
||||||
|
f" 它是仿真相机的话题/坐标系唯一来源（桥接与适配器都从它派生）。\n"
|
||||||
|
f" 缺了它仿真里的相机就完全没输出。确认仓库根下存在 "
|
||||||
|
f"{DEFAULT_CAMERAS_REL}。"
|
||||||
|
)
|
||||||
|
|
||||||
|
try:
|
||||||
|
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||||
|
except yaml.YAMLError as exc:
|
||||||
|
raise CameraConfigError(f"相机清单 YAML 解析失败: {path}\n 原因: {exc}") from exc
|
||||||
|
|
||||||
|
raw = doc.get("cameras")
|
||||||
|
if not isinstance(raw, list) or not raw:
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"{path} 里没有 cameras 列表（或为空）。至少写一台相机，否则仿真里没有图像。"
|
||||||
|
)
|
||||||
|
|
||||||
|
specs: List[CameraSpec] = []
|
||||||
|
seen: Dict[str, str] = {}
|
||||||
|
for i, item in enumerate(raw):
|
||||||
|
if not isinstance(item, dict):
|
||||||
|
raise CameraConfigError(f"{path} 的 cameras[{i}] 不是映射（键值对）。")
|
||||||
|
|
||||||
|
def need(key: str) -> str:
|
||||||
|
v = str(item.get(key, "") or "").strip()
|
||||||
|
if not v:
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"{path} 的 cameras[{i}] 缺少 {key}。"
|
||||||
|
f"（gz_ns / ros_ns / link_frame / optical_frame 都必填）"
|
||||||
|
)
|
||||||
|
return v
|
||||||
|
|
||||||
|
# 必填键先查完，再查取值是否合理 —— 否则一份"缺键且键值也不对"的配置
|
||||||
|
# 会先报一个后者的错，让人以为只是值填错，实际键都还没写全。
|
||||||
|
gz_ns = "/" + need("gz_ns").strip("/")
|
||||||
|
ros_ns = need("ros_ns").strip("/")
|
||||||
|
link_frame = need("link_frame")
|
||||||
|
optical_frame = need("optical_frame")
|
||||||
|
|
||||||
|
# 同一个 ROS 命名空间出现两次 → 两台相机会互相覆盖话题，
|
||||||
|
# 表现为"只有一台出图"或图像在两个相机之间跳，很难查。直接拒绝。
|
||||||
|
if ros_ns in seen:
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"{path}: ros_ns '{ros_ns}' 重复（cameras[{i}] 与 {seen[ros_ns]}）。\n"
|
||||||
|
f" 两台相机用同一个 ros_ns 会互相覆盖话题，必须区分开。"
|
||||||
|
)
|
||||||
|
seen[ros_ns] = f"cameras[{i}]"
|
||||||
|
|
||||||
|
try:
|
||||||
|
fov = float(item.get("horizontal_fov", 0.0) or 0.0)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"{path} 的 cameras[{i}].horizontal_fov 不是数字。"
|
||||||
|
) from None
|
||||||
|
# 视场是内参的唯一来源（gz 自己发的 camera_info 是错的，见文件头），
|
||||||
|
# 所以它必须存在且合理。填错的话三维点位会整体错，而且看不出来。
|
||||||
|
if not (0.0 < fov < math.pi):
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"{path} 的 cameras[{i}].horizontal_fov = {fov} 不合理。\n"
|
||||||
|
f" 必须是 (0, π) 弧度之间的值，例如真机 D405 约 87° = 1.5184。\n"
|
||||||
|
f" 它决定内参 fx = fy = W/(2*tan(hfov/2))，填错会让反投影出的"
|
||||||
|
f"三维点整体偏移，而单目预览里完全看不出来。"
|
||||||
|
)
|
||||||
|
|
||||||
|
specs.append(
|
||||||
|
CameraSpec(
|
||||||
|
gz_ns=gz_ns,
|
||||||
|
ros_ns=ros_ns,
|
||||||
|
link_frame=link_frame,
|
||||||
|
optical_frame=optical_frame,
|
||||||
|
depth=bool(item.get("depth", True)),
|
||||||
|
horizontal_fov=fov,
|
||||||
|
world_model=str(item.get("world_model", "") or "").strip(),
|
||||||
|
world_link=str(item.get("world_link", "") or "link").strip() or "link",
|
||||||
|
width=int(item.get("width", 0) or 0),
|
||||||
|
height=int(item.get("height", 0) or 0),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return specs
|
||||||
|
|
||||||
|
|
||||||
|
def intrinsics(spec: CameraSpec, width: int, height: int) -> Tuple[List[float], List[float]]:
|
||||||
|
"""由视场与图像尺寸算出 (K, P)。返回扁平列表，可直接塞进 CameraInfo。
|
||||||
|
|
||||||
|
=========================================================================
|
||||||
|
为什么必须自己算，而不是用 gz 发的那份 camera_info
|
||||||
|
=========================================================================
|
||||||
|
本机实测（Fortress 6.16 / RoboStack）：gz 给 rgbd_camera 发的 camera_info
|
||||||
|
**恒为** fx=fy=277, cx=160, cy=120 —— 无论 SDF 里怎么配。这组值对应 gz 的
|
||||||
|
默认参数：60° 视场 + 320x240 图幅。
|
||||||
|
|
||||||
|
而它**渲染**用的却是 SDF 里配的 87° 视场与 640x480。证据是：用已知地面平面
|
||||||
|
对深度反投影做最小二乘，解出 fy=337.2、cy=239.5，与
|
||||||
|
640 / (2*tan(1.5184/2)) = 337.2 480/2 = 240
|
||||||
|
完全吻合（残差中位 0.2mm）。也就是说渲染是对的，只有 camera_info 是默认值。
|
||||||
|
|
||||||
|
后果有多严重：照抄那份 camera_info 反投影，D435 看到的地面会被算到
|
||||||
|
z ≈ -0.49m（真实是 0），偏差半米量级。而单目预览一切正常 ——
|
||||||
|
这类错误只有在用三维点位时才暴露，恰好是本项目装配任务的核心输入。
|
||||||
|
|
||||||
|
因此这里按"渲染实际用的视场"重算，并覆盖 gz 的 camera_info。
|
||||||
|
像素是方的（gz 由同一视场与宽高比推出两个方向），所以 fx = fy。
|
||||||
|
|
||||||
|
⚠️ 相应地，horizontal_fov 必须与 SDF 里真正渲染用的值一致。
|
||||||
|
它有两个来源，启动时会自动比对并告警（check_fov_consistency）。
|
||||||
|
"""
|
||||||
|
if not (0.0 < spec.horizontal_fov < math.pi):
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"相机 {spec.ros_ns} 的 horizontal_fov={spec.horizontal_fov} 不合理，"
|
||||||
|
f"无法算内参。"
|
||||||
|
)
|
||||||
|
if width <= 0 or height <= 0:
|
||||||
|
raise CameraConfigError(
|
||||||
|
f"相机 {spec.ros_ns} 的图像尺寸非法（{width}x{height}），无法算内参。"
|
||||||
|
)
|
||||||
|
|
||||||
|
fx = width / (2.0 * math.tan(spec.horizontal_fov / 2.0))
|
||||||
|
fy = fx # 方形像素
|
||||||
|
cx = width / 2.0
|
||||||
|
cy = height / 2.0
|
||||||
|
|
||||||
|
k = [fx, 0.0, cx,
|
||||||
|
0.0, fy, cy,
|
||||||
|
0.0, 0.0, 1.0]
|
||||||
|
# P 是 3x4 的投影矩阵。gz 不模拟畸变，所以 R 是单位阵、最后一列为 0。
|
||||||
|
p = [fx, 0.0, cx, 0.0,
|
||||||
|
0.0, fy, cy, 0.0,
|
||||||
|
0.0, 0.0, 1.0, 0.0]
|
||||||
|
return k, p
|
||||||
|
|
||||||
|
|
||||||
|
def check_fov_consistency(
|
||||||
|
specs: Sequence[CameraSpec],
|
||||||
|
world_file: Optional[Path] = None,
|
||||||
|
sensors_xacro: Optional[Path] = None,
|
||||||
|
) -> List[str]:
|
||||||
|
"""比对 cameras.yaml 里声明的视场与 SDF 里真正渲染用的视场。
|
||||||
|
|
||||||
|
为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 算出，
|
||||||
|
而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 ——
|
||||||
|
症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头
|
||||||
|
那一长串说明所警告的那类错误，所以要主动比对。
|
||||||
|
|
||||||
|
返回人类可读的告警列表（空 = 没问题）。
|
||||||
|
|
||||||
|
精度说明（有意为之，不做过度解析）：
|
||||||
|
- 世界相机：按 <sensor> 里的 <topic> 精确匹配到对应相机，读到的是精确值。
|
||||||
|
- 腕上相机：它在 xacro 宏里、值由参数传入，展开前无法可靠地归属到具体
|
||||||
|
相机。所以退化成"文件里出现过这个值就算过"—— 偏宽松，但足以抓住
|
||||||
|
"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。
|
||||||
|
"""
|
||||||
|
warnings: List[str] = []
|
||||||
|
|
||||||
|
if world_file is not None and Path(world_file).is_file():
|
||||||
|
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||||
|
for s in specs:
|
||||||
|
if not s.is_world_mounted:
|
||||||
|
continue
|
||||||
|
# 找到 <topic> 等于本相机 gz_ns 的那个 <sensor> 块
|
||||||
|
fov = None
|
||||||
|
for m in re.finditer(r"<sensor\b[^>]*>(.*?)</sensor>", text, re.S):
|
||||||
|
body = m.group(1)
|
||||||
|
tm = re.search(r"<topic>\s*([^<\s]+)\s*</topic>", body)
|
||||||
|
if not tm or "/" + tm.group(1).strip().lstrip("/") != s.gz_ns:
|
||||||
|
continue
|
||||||
|
fm = re.search(r"<horizontal_fov>\s*([0-9.eE+-]+)\s*</horizontal_fov>", body)
|
||||||
|
if fm:
|
||||||
|
fov = float(fm.group(1))
|
||||||
|
break
|
||||||
|
if fov is None:
|
||||||
|
warnings.append(
|
||||||
|
f"相机 {s.ros_ns}: 在 {Path(world_file).name} 里找不到 topic 为 "
|
||||||
|
f"{s.gz_ns} 的 <sensor> 的 <horizontal_fov>，无法比对。"
|
||||||
|
f"内参将按 cameras.yaml 的 {s.horizontal_fov} 计算 —— "
|
||||||
|
f"若与渲染实际用的视场不符，三维点位会整体错。"
|
||||||
|
)
|
||||||
|
elif abs(fov - s.horizontal_fov) > 1e-4:
|
||||||
|
warnings.append(
|
||||||
|
f"相机 {s.ros_ns}: 视场不一致！cameras.yaml 写 {s.horizontal_fov}，"
|
||||||
|
f"而 {Path(world_file).name} 里渲染用的是 {fov}。\n"
|
||||||
|
f" 内参按前者算、图像按后者渲染 → 三维点位会整体错，"
|
||||||
|
f"而单目预览看不出来。请把两处改成一致。"
|
||||||
|
)
|
||||||
|
|
||||||
|
if sensors_xacro is not None and Path(sensors_xacro).is_file():
|
||||||
|
text = Path(sensors_xacro).read_text(encoding="utf-8", errors="replace")
|
||||||
|
found = [float(v) for v in re.findall(r"<horizontal_fov>\s*([0-9.eE+-]+)", text)]
|
||||||
|
for s in specs:
|
||||||
|
if s.is_world_mounted:
|
||||||
|
continue
|
||||||
|
if not found:
|
||||||
|
warnings.append(
|
||||||
|
f"相机 {s.ros_ns}: 在 {Path(sensors_xacro).name} 里没找到 "
|
||||||
|
f"<horizontal_fov>，无法比对（内参按 cameras.yaml 的 "
|
||||||
|
f"{s.horizontal_fov} 计算）。"
|
||||||
|
)
|
||||||
|
elif not any(abs(v - s.horizontal_fov) <= 1e-4 for v in found):
|
||||||
|
warnings.append(
|
||||||
|
f"相机 {s.ros_ns}: 视场可能不一致。cameras.yaml 写 "
|
||||||
|
f"{s.horizontal_fov}，而 {Path(sensors_xacro).name} 里出现的值是 "
|
||||||
|
f"{found}。若渲染用的不是 {s.horizontal_fov}，三维点位会整体错。"
|
||||||
|
)
|
||||||
|
|
||||||
|
return warnings
|
||||||
|
|
||||||
|
|
||||||
|
def bridge_args(specs: Sequence[CameraSpec]) -> List[str]:
|
||||||
|
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
|
||||||
|
|
||||||
|
只做**传输**：gz 消息类型 → ROS 消息类型，话题名保持 gz 侧的原样。
|
||||||
|
改名与格式换算交给适配器 —— 分开的原因是桥接器改不了 frame_id，
|
||||||
|
而 frame_id 恰恰是最容易错又最不报错的那一项。让适配器统一处理
|
||||||
|
"名字 + 坐标系 + 单位"，逻辑集中在一处。
|
||||||
|
|
||||||
|
注：/d405/image 这类"gz 原名"话题在 ROS 图里也会可见。
|
||||||
|
这是有意保留的：排查"到底是桥接断了还是适配器断了"时，
|
||||||
|
直接 echo 原名话题能一刀切开。
|
||||||
|
"""
|
||||||
|
args: List[str] = []
|
||||||
|
for s in specs:
|
||||||
|
args.append(f"{s.gz_image}@sensor_msgs/msg/Image[gz.msgs.Image")
|
||||||
|
args.append(f"{s.gz_info}@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo")
|
||||||
|
if s.depth:
|
||||||
|
# gz 的深度是 float32（本机实测 4 字节/像素），桥接器支持为 32FC1。
|
||||||
|
# 若哪天桥接器不再支持，这里会静默少一个话题 —— 验收时用
|
||||||
|
# `ros2 topic list | grep aligned_depth` 确认它在。
|
||||||
|
args.append(f"{s.gz_depth}@sensor_msgs/msg/Image[gz.msgs.Image")
|
||||||
|
return args
|
||||||
|
|
||||||
|
|
||||||
|
def depth_m_to_mm16(depth_m: np.ndarray, max_mm: int = MAX_MM) -> np.ndarray:
|
||||||
|
"""深度：32FC1 米 → 16UC1 毫米。0 表示"无读数"（与真机 realsense 一致）。
|
||||||
|
|
||||||
|
规则（每一条都对应一种真机会出现的情况）：
|
||||||
|
非有限值（nan / inf） → 0。gz 在没命中时可能给 inf，
|
||||||
|
真机 realsense 给 0；统一成 0。
|
||||||
|
<= 0 → 0。真机对无效像素就是给 0，
|
||||||
|
负深度没有物理意义。
|
||||||
|
0 < d <= max_mm/1000 → round(d*1000)
|
||||||
|
> max_mm/1000（65.535m） → 0（见 MAX_MM 的说明：不饱和）
|
||||||
|
舍入后为 0 的极小正数 → 0（例如 0.0001m，落不进 uint16）
|
||||||
|
|
||||||
|
返回与输入同形状的 uint16 数组。
|
||||||
|
"""
|
||||||
|
a = np.asarray(depth_m)
|
||||||
|
if a.dtype != np.float32:
|
||||||
|
a = a.astype(np.float32, copy=False)
|
||||||
|
|
||||||
|
out = np.zeros(a.shape, dtype=np.uint16)
|
||||||
|
|
||||||
|
finite = np.isfinite(a)
|
||||||
|
if not finite.any():
|
||||||
|
return out
|
||||||
|
|
||||||
|
# 只对有限且为正的像素做换算；先把它们挑出来，避免对 nan 做比较
|
||||||
|
# （nan 参与的比较全是 False，但显式挑出来意图更清楚，也少一遍遍历）
|
||||||
|
cand = finite & (a > 0.0)
|
||||||
|
if not cand.any():
|
||||||
|
return out
|
||||||
|
|
||||||
|
mm = np.rint(a[cand].astype(np.float64) * 1000.0)
|
||||||
|
ok = (mm > 0.0) & (mm <= float(max_mm))
|
||||||
|
if ok.any():
|
||||||
|
out[cand] = np.where(ok, mm, 0.0).astype(np.uint16)
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def optical_rpy() -> Tuple[float, float, float]:
|
||||||
|
"""本体 → 光学系的固定旋转，供静态 TF 使用。"""
|
||||||
|
return OPTICAL_RPY
|
||||||
|
|
||||||
|
|
||||||
|
def rpy_to_quat(rpy: Sequence[float]) -> Tuple[float, float, float, float]:
|
||||||
|
"""(roll, pitch, yaw) → (x, y, z, w)。只用于发布静态 TF。"""
|
||||||
|
r, p, y = (float(v) for v in rpy)
|
||||||
|
cr, sr = math.cos(r / 2.0), math.sin(r / 2.0)
|
||||||
|
cp, sp = math.cos(p / 2.0), math.sin(p / 2.0)
|
||||||
|
cy, sy = math.cos(y / 2.0), math.sin(y / 2.0)
|
||||||
|
return (
|
||||||
|
sr * cp * cy - cr * sp * sy,
|
||||||
|
cr * sp * cy + sr * cp * sy,
|
||||||
|
cr * cp * sy - sr * sp * cy,
|
||||||
|
cr * cp * cy + sr * sp * sy,
|
||||||
|
)
|
||||||
@@ -0,0 +1,214 @@
|
|||||||
|
"""读 `config/site.yaml` 的 `sim.*` 段。
|
||||||
|
|
||||||
|
=============================================================================
|
||||||
|
为什么需要这个模块
|
||||||
|
=============================================================================
|
||||||
|
`config/site.yaml` 的 `sim` 段（world / physics_enable / joint_prefix）与
|
||||||
|
`devices/sim/gazebo/manifest.yaml` 的 `config_schema` 一直宣称"改这里就生效"，
|
||||||
|
但**没有任何代码读它**：world 路径硬编码在 robot_description._WORLD_REL 里。
|
||||||
|
|
||||||
|
于是产生最难受的一类 bug：用户按文档改了 `site.yaml`，启动后行为毫无变化，
|
||||||
|
也不报错。既然文档与 manifest 都把它当契约，就必须真的读它。
|
||||||
|
|
||||||
|
要读，但也不能假装所有键都接线了 —— 有些键（physics_enable / joint_prefix）
|
||||||
|
目前确实还没接。所以本模块除了读值，还提供 unwired_warnings()：
|
||||||
|
把"声明了、但当前实现并不消费"的差异显式报出来，
|
||||||
|
把静默无效变成一条看得见的告警。等哪天真的接线了，告警自然消失。
|
||||||
|
=============================================================================
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import re
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import List, Optional
|
||||||
|
|
||||||
|
import yaml
|
||||||
|
|
||||||
|
SITE_REL = "config/site.yaml"
|
||||||
|
SITE_EXAMPLE_REL = "config/site.example.yaml"
|
||||||
|
|
||||||
|
# 与 manifest.yaml 的 config_schema 默认值保持一致。
|
||||||
|
# 三处（site.yaml 注释 / manifest / 这里）都写了默认值，改动时必须同步 ——
|
||||||
|
# 所以下面有用例盯着这里和 manifest 的一致性。
|
||||||
|
DEFAULT_WORLD = ""
|
||||||
|
DEFAULT_PHYSICS_ENABLE = True
|
||||||
|
DEFAULT_JOINT_PREFIX = "sim_"
|
||||||
|
|
||||||
|
|
||||||
|
class SimSiteError(RuntimeError):
|
||||||
|
"""site.yaml 读不了或内容不合法。"""
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class SimSite:
|
||||||
|
"""`sim.*` 段的解析结果。"""
|
||||||
|
|
||||||
|
world: str = DEFAULT_WORLD
|
||||||
|
physics_enable: bool = DEFAULT_PHYSICS_ENABLE
|
||||||
|
joint_prefix: str = DEFAULT_JOINT_PREFIX
|
||||||
|
# 这些键在 site.yaml 里**显式**出现过吗？
|
||||||
|
# 用来区分"用户说了要关物理"和"用户没提，所以用默认值"——
|
||||||
|
# 前者值得告警，后者没啥可说。
|
||||||
|
present: set = field(default_factory=set)
|
||||||
|
source: Optional[Path] = None
|
||||||
|
|
||||||
|
|
||||||
|
def load_sim_site(root: Optional[Path] = None) -> SimSite:
|
||||||
|
"""读 `config/site.yaml` 的 sim 段。文件不存在则返回全默认值。
|
||||||
|
|
||||||
|
故意**不**在文件缺失时报错：仿真要能在没有站点的机器上跑起来
|
||||||
|
（site.yaml 里含真实设备 ID，本来就不该提交，见 README 的"要提交什么"）。
|
||||||
|
"""
|
||||||
|
base = Path(root) if root is not None else repo_root()
|
||||||
|
path = base / SITE_REL
|
||||||
|
|
||||||
|
if not path.is_file():
|
||||||
|
return SimSite()
|
||||||
|
|
||||||
|
try:
|
||||||
|
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||||
|
except yaml.YAMLError as exc:
|
||||||
|
raise SimSiteError(
|
||||||
|
f"{path} 解析失败: {exc}\n"
|
||||||
|
f" 这一行会让仿真启动不了。修好它，或临时改名为 site.yaml.bak 忽略。"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
if not isinstance(doc, dict):
|
||||||
|
raise SimSiteError(f"{path} 的顶层不是映射（键值对）。")
|
||||||
|
|
||||||
|
raw = doc.get("sim")
|
||||||
|
if raw is None:
|
||||||
|
return SimSite(source=path)
|
||||||
|
if not isinstance(raw, dict):
|
||||||
|
raise SimSiteError(
|
||||||
|
f"{path} 的 sim 段不是映射（键值对），而是 {type(raw).__name__}。"
|
||||||
|
f"它应该像这样：\n sim:\n world: devices/sim/gazebo/worlds/assembly.world"
|
||||||
|
)
|
||||||
|
|
||||||
|
site = SimSite(source=path)
|
||||||
|
if "world" in raw:
|
||||||
|
site.world = str(raw.get("world") or "").strip()
|
||||||
|
site.present.add("world")
|
||||||
|
if "physics_enable" in raw:
|
||||||
|
site.physics_enable = bool(raw.get("physics_enable"))
|
||||||
|
site.present.add("physics_enable")
|
||||||
|
if "joint_prefix" in raw:
|
||||||
|
site.joint_prefix = str(raw.get("joint_prefix") or "").strip()
|
||||||
|
site.present.add("joint_prefix")
|
||||||
|
return site
|
||||||
|
|
||||||
|
|
||||||
|
def repo_root() -> Path:
|
||||||
|
"""仓库根。本模块在 src/arm_sim_gazebo/arm_sim_gazebo/ 下，向上走四层。"""
|
||||||
|
return Path(__file__).resolve().parents[3]
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_world(
|
||||||
|
cli_world: str = "",
|
||||||
|
root: Optional[Path] = None,
|
||||||
|
fallback: Optional[Path] = None,
|
||||||
|
) -> tuple[Path, str]:
|
||||||
|
"""决定用哪个 world 文件。返回 (路径, 来源说明)。
|
||||||
|
|
||||||
|
优先级：命令行 --world > site.yaml 的 sim.world > 内置默认。
|
||||||
|
|
||||||
|
为什么要有优先级而不是"只认 site.yaml"：site.yaml 是站点级的，而调试时
|
||||||
|
经常要临时换一个世界（比如 in.sdf 里复制一份改着试）。命令行必须能压过它。
|
||||||
|
"""
|
||||||
|
base = Path(root) if root is not None else repo_root()
|
||||||
|
|
||||||
|
if cli_world and cli_world.strip():
|
||||||
|
p = Path(cli_world.strip())
|
||||||
|
# 相对路径按仓库根解析：用户从任意目录启动，写相对路径的直觉都是"相对仓库"。
|
||||||
|
if not p.is_absolute():
|
||||||
|
p = base / p
|
||||||
|
return p, "命令行 --world"
|
||||||
|
|
||||||
|
site = load_sim_site(base)
|
||||||
|
if site.world:
|
||||||
|
p = Path(site.world)
|
||||||
|
if not p.is_absolute():
|
||||||
|
p = base / p
|
||||||
|
return p, f"{SITE_REL} 的 sim.world"
|
||||||
|
|
||||||
|
if fallback is None:
|
||||||
|
raise SimSiteError(
|
||||||
|
"没能确定 world 文件：site.yaml 里 sim.world 为空，也没给内置默认。"
|
||||||
|
)
|
||||||
|
fb = Path(fallback)
|
||||||
|
# 内置默认也要跟前面两条一样解析成绝对路径：调用方（启动器 / launch）
|
||||||
|
# 拿到的必须是绝对路径，否则从别的目录启动时会被解析到错误的位置，
|
||||||
|
# 而那会表现成"找不到 world 文件"，看起来像配置写错了。
|
||||||
|
if not fb.is_absolute():
|
||||||
|
fb = base / fb
|
||||||
|
return fb, "内置默认（site.yaml 未指定 sim.world）"
|
||||||
|
|
||||||
|
|
||||||
|
def unwired_warnings(
|
||||||
|
site: SimSite,
|
||||||
|
actual_joint_prefix: Optional[str] = None,
|
||||||
|
physics_in_world: Optional[bool] = None,
|
||||||
|
) -> List[str]:
|
||||||
|
"""把"site.yaml 里声明了、但当前实现并不消费"的配置报出来。
|
||||||
|
|
||||||
|
这是本模块存在的一半理由：与其让 `physics_enable: false` 静默无效，
|
||||||
|
不如明说"这个键还没接线"。下面两个键目前确实没接：
|
||||||
|
|
||||||
|
physics_enable 世界文件里的重力/步长是写死的，没有按它开关。
|
||||||
|
joint_prefix 仿真关节前缀由 arm.sim.urdf.xacro 的 arm_prefix 决定，
|
||||||
|
且 controllers.yaml 里硬编码了 sim_joint1..7；
|
||||||
|
改 site.yaml 不会传导过去。
|
||||||
|
|
||||||
|
等它们真的接线了，从这里删掉对应分支，告警就自然消失。
|
||||||
|
"""
|
||||||
|
out: List[str] = []
|
||||||
|
|
||||||
|
if "physics_enable" in site.present and physics_in_world is not None:
|
||||||
|
want_on = bool(site.physics_enable)
|
||||||
|
if want_on != bool(physics_in_world):
|
||||||
|
out.append(
|
||||||
|
f"site.yaml 写了 sim.physics_enable={site.physics_enable}，"
|
||||||
|
f"但世界文件当前是物理{'开启' if physics_in_world else '关闭'}状态，"
|
||||||
|
f"而这个键**还没有接线** —— 世界文件里的重力/步长是写死的。\n"
|
||||||
|
f" 想让物理开关生效，得改 world 文件里的 <physics>，"
|
||||||
|
f"或者先把这个键接进启动流程（见 sim_site.py 顶部说明）。"
|
||||||
|
)
|
||||||
|
|
||||||
|
if "joint_prefix" in site.present and actual_joint_prefix is not None:
|
||||||
|
want = site.joint_prefix
|
||||||
|
if want and want != actual_joint_prefix:
|
||||||
|
out.append(
|
||||||
|
f"site.yaml 写了 sim.joint_prefix={want!r}，"
|
||||||
|
f"但实际用的前缀是 {actual_joint_prefix!r}，"
|
||||||
|
f"而这个键**还没有接线**。\n"
|
||||||
|
f" 前缀来自 arm.sim.urdf.xacro 的 arm_prefix，"
|
||||||
|
f"且 config/controllers.yaml 里硬编码了 {actual_joint_prefix}joint1..7 —— "
|
||||||
|
f"三处必须一致，改一处不够。"
|
||||||
|
)
|
||||||
|
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def physics_enabled_in_world(world_file: Path) -> Optional[bool]:
|
||||||
|
"""世界文件里的物理到底开没开。判据是重力是不是零向量。
|
||||||
|
|
||||||
|
gz 里"关物理"的常见做法就是把 <gravity> 置零（比删掉整个 <physics>
|
||||||
|
更常用，因为 <physics> 还带着步长等其它必需项）。没有 <physics> /
|
||||||
|
<gravity> 时按"开着"算 —— gz 的默认就是开。
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||||
|
except OSError:
|
||||||
|
return None
|
||||||
|
m = re.search(r"<gravity>\s*([^<]+)\s*</gravity>", text)
|
||||||
|
if not m:
|
||||||
|
return True
|
||||||
|
try:
|
||||||
|
vals = [float(x) for x in m.group(1).split()]
|
||||||
|
except ValueError:
|
||||||
|
return None
|
||||||
|
if len(vals) != 3:
|
||||||
|
return None
|
||||||
|
return any(abs(v) > 1e-12 for v in vals)
|
||||||
@@ -76,10 +76,12 @@ def _prepare(context, *args, **kwargs):
|
|||||||
default_controllers,
|
default_controllers,
|
||||||
default_world,
|
default_world,
|
||||||
default_xacro,
|
default_xacro,
|
||||||
|
model_link_pose,
|
||||||
prepare_urdf,
|
prepare_urdf,
|
||||||
repo_root,
|
repo_root,
|
||||||
world_name,
|
world_name,
|
||||||
)
|
)
|
||||||
|
from arm_sim_gazebo.sim_site import SimSiteError, resolve_world
|
||||||
|
|
||||||
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()
|
||||||
@@ -89,7 +91,10 @@ def _prepare(context, *args, **kwargs):
|
|||||||
|
|
||||||
try:
|
try:
|
||||||
root = repo_root()
|
root = repo_root()
|
||||||
world_path = world or str(default_world(root))
|
# 与 gazebo_sim_launcher 用同一套优先级，直接 `ros2 launch` 的人也不会
|
||||||
|
# 遇到"改了 site.yaml 没反应"：--world > site.yaml 的 sim.world > 内置默认。
|
||||||
|
world_path, _world_origin = resolve_world(world, root, fallback=default_world(root))
|
||||||
|
world_path = str(world_path)
|
||||||
|
|
||||||
urdf, report = prepare_urdf(
|
urdf, report = prepare_urdf(
|
||||||
xacro_path=default_xacro(root),
|
xacro_path=default_xacro(root),
|
||||||
@@ -99,7 +104,7 @@ def _prepare(context, *args, **kwargs):
|
|||||||
)
|
)
|
||||||
wname = world_name(world_path)
|
wname = world_name(world_path)
|
||||||
missing_base = check_base_plugins(Path(world_path))
|
missing_base = check_base_plugins(Path(world_path))
|
||||||
except SimDescriptionError as exc:
|
except (SimDescriptionError, SimSiteError) as exc:
|
||||||
# 描述准备失败是最常见的上手障碍（环境没 source、路径不对）。
|
# 描述准备失败是最常见的上手障碍（环境没 source、路径不对）。
|
||||||
# 直接把可读原因打出来，而不是让它变成一个看不懂的 launch traceback。
|
# 直接把可读原因打出来，而不是让它变成一个看不懂的 launch traceback。
|
||||||
raise RuntimeError(f"\n[gazebo_sim] 仿真描述准备失败：\n{exc}\n") from exc
|
raise RuntimeError(f"\n[gazebo_sim] 仿真描述准备失败：\n{exc}\n") from exc
|
||||||
@@ -179,7 +184,123 @@ def _prepare(context, *args, **kwargs):
|
|||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
# --- 4) spawn 机器人 -----------------------------------------------------
|
# --- 4) 相机：桥接 → 适配器 → （世界相机）补 TF --------------------------
|
||||||
|
# 只在挂了传感器时才做。--no-sensors 时跳过是必要的：适配器会去订阅
|
||||||
|
# 永远不存在的 gz 话题，它**不会报错**，只是静默没输出 —— 而"相机没图"
|
||||||
|
# 正是最需要快速定位的问题，不该被一个假故障混淆。
|
||||||
|
if with_sensors != "false":
|
||||||
|
from arm_sim_gazebo.sim_cameras import (
|
||||||
|
DEFAULT_CAMERAS_REL,
|
||||||
|
CameraConfigError,
|
||||||
|
bridge_args,
|
||||||
|
check_fov_consistency,
|
||||||
|
load_cameras,
|
||||||
|
rpy_to_quat,
|
||||||
|
)
|
||||||
|
|
||||||
|
try:
|
||||||
|
specs = load_cameras(root=root)
|
||||||
|
except CameraConfigError as exc:
|
||||||
|
raise RuntimeError(f"\n[gazebo_sim] 仿真相机清单有问题：\n{exc}\n") from exc
|
||||||
|
|
||||||
|
# 视场一致性：内参由 cameras.yaml 的 horizontal_fov 算，而**渲染**用的是
|
||||||
|
# SDF 里的值。两者不一致时图像与内参对不上，三维点位会整体错 ——
|
||||||
|
# 而单目预览完全看不出来（gz 自己发的 camera_info 本来就是错的，
|
||||||
|
# 所以这份内参是"唯一真相"，更要保证它与渲染一致）。
|
||||||
|
# 这里只告警不拦截：真有偏差时，仍应能起仿真去现场比对。
|
||||||
|
_fov_issues = check_fov_consistency(
|
||||||
|
specs,
|
||||||
|
world_file=Path(world_path),
|
||||||
|
sensors_xacro=Path(root)
|
||||||
|
/ "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
|
||||||
|
)
|
||||||
|
# ⚠ 这里用 LogInfo 而不是 LogWarn：Humble 的 launch.actions 里没有
|
||||||
|
# LogWarn（Iron 才加），照抄新版本会直接 ImportError 让整个 launch
|
||||||
|
# 加载失败 —— 一个"只是想说句话"的动作把仿真起不来，代价太大。
|
||||||
|
# 前缀里带 ⚠ 保证它在满屏日志里仍然显眼。
|
||||||
|
for _w in _fov_issues:
|
||||||
|
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ {_w}"))
|
||||||
|
|
||||||
|
# 4a) 世界相机的 TF 兜底
|
||||||
|
#
|
||||||
|
# Gazebo **不会**为 world 里的 <model> 产生 ROS TF —— TF 只来自
|
||||||
|
# robot_state_publisher（也就是只覆盖 URDF 里的东西）。
|
||||||
|
# D435 正属于这一类：它的物理归属是"世界"（固定支架），不在 URDF 里
|
||||||
|
# （早期挂在 URDF 的 world link 上时，被 sdformat 连整棵子树静默丢掉）。
|
||||||
|
# 少了这条 TF，应用 tf2 查询直接报 "frame does not exist" ——
|
||||||
|
# 而图像本身完全正常，很容易误判成相机坏了。
|
||||||
|
#
|
||||||
|
# 位姿从 world 文件**解析**，不在这里重复配置：唯一真相就是 world 文件。
|
||||||
|
# 另配一份等于制造"改了 world 忘了改这里"，而症状是相机看到的东西
|
||||||
|
# 与它自称的位置对不上，极难查。
|
||||||
|
world_tf = []
|
||||||
|
for s in specs:
|
||||||
|
if not s.is_world_mounted:
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
(px, py, pz), (rr, rp, ry) = model_link_pose(
|
||||||
|
Path(world_path), s.world_model, s.world_link
|
||||||
|
)
|
||||||
|
except SimDescriptionError as exc:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"\n[gazebo_sim] 世界相机 {s.ros_ns} 的 TF 补不出来：\n{exc}\n"
|
||||||
|
) from exc
|
||||||
|
qx, qy, qz, qw = rpy_to_quat((rr, rp, ry))
|
||||||
|
world_tf.append(
|
||||||
|
Node(
|
||||||
|
package="tf2_ros",
|
||||||
|
executable="static_transform_publisher",
|
||||||
|
name=f"tf_{s.ros_ns}",
|
||||||
|
output="screen",
|
||||||
|
arguments=[
|
||||||
|
"--x", f"{px}", "--y", f"{py}", "--z", f"{pz}",
|
||||||
|
"--qx", f"{qx}", "--qy", f"{qy}", "--qz", f"{qz}", "--qw", f"{qw}",
|
||||||
|
"--frame-id", "world",
|
||||||
|
"--child-frame-id", s.link_frame,
|
||||||
|
],
|
||||||
|
parameters=[{"use_sim_time": True}],
|
||||||
|
)
|
||||||
|
)
|
||||||
|
actions.append(
|
||||||
|
LogInfo(
|
||||||
|
msg=f"[gazebo_sim] 世界相机 {s.ros_ns}: world → {s.link_frame} "
|
||||||
|
f"@ ({px:.3f}, {py:.3f}, {pz:.3f}) 由 world 文件解析得到"
|
||||||
|
)
|
||||||
|
)
|
||||||
|
if world_tf:
|
||||||
|
# 延后一点再发：/clock 还没起来时发布静态 TF 可能被 tf2 判成
|
||||||
|
# "未来时间戳"，而这一类问题只在很后面才以 extrapolation 报出来。
|
||||||
|
actions.append(TimerAction(period=2.0, actions=world_tf))
|
||||||
|
|
||||||
|
# 4b) 桥接：只做传输（gz 消息类型 → ROS 消息类型，话题名照抄）。
|
||||||
|
# 改名、坐标系、深度单位都交给 4c 的适配器 —— 桥接器改不了 frame_id，
|
||||||
|
# 而 frame_id 恰恰是最容易错又最不报错的那一项。
|
||||||
|
actions.append(
|
||||||
|
Node(
|
||||||
|
package="ros_gz_bridge",
|
||||||
|
executable="parameter_bridge",
|
||||||
|
name="camera_bridge",
|
||||||
|
output="screen",
|
||||||
|
arguments=bridge_args(specs),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
# 4c) 适配器：盖光学系 frame_id + 深度 32FC1 米 → 16UC1 毫米 + 改成
|
||||||
|
# 应用期望的话题名。详见 sim_camera_adapter.py 顶部。
|
||||||
|
actions.append(
|
||||||
|
Node(
|
||||||
|
package="arm_sim_gazebo",
|
||||||
|
executable="sim_camera_adapter",
|
||||||
|
name="sim_camera_adapter",
|
||||||
|
output="screen",
|
||||||
|
parameters=[
|
||||||
|
{"cameras_file": str(Path(root) / DEFAULT_CAMERAS_REL)},
|
||||||
|
{"use_sim_time": True},
|
||||||
|
],
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
# --- 5) spawn 机器人 -----------------------------------------------------
|
||||||
# -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
|
# -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
|
||||||
# 表现成"launch 一切正常但 RViz 里什么都没有"。
|
# 表现成"launch 一切正常但 RViz 里什么都没有"。
|
||||||
actions.append(
|
actions.append(
|
||||||
@@ -201,7 +322,7 @@ def _prepare(context, *args, **kwargs):
|
|||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
# --- 5) 控制器 -----------------------------------------------------------
|
# --- 6) 控制器 -----------------------------------------------------------
|
||||||
# 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
|
# 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
|
||||||
# rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
|
# rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
|
||||||
controllers = [
|
controllers = [
|
||||||
@@ -236,8 +357,18 @@ def _prepare(context, *args, **kwargs):
|
|||||||
" ros2 control list_controllers # 两个控制器 active\n"
|
" ros2 control list_controllers # 两个控制器 active\n"
|
||||||
" 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"
|
||||||
|
" python3 scripts/check_sim_cameras.py # ★ 相机的真正验收\n"
|
||||||
" 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
|
" 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
|
||||||
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。"
|
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n"
|
||||||
|
" ★ 别用 'ros2 topic hz' 或肉眼预览来验收相机 —— 那些只能证明'有图像'。\n"
|
||||||
|
" 仿真相机的错误几乎全是静默的（深度单位差 1000 倍、frame_id 盖成\n"
|
||||||
|
" 本体系、gz 发的内参是错的、位姿 rpy 写错导致相机背对目标），\n"
|
||||||
|
" 它们全都能出图、能查到 TF。必须用 check_sim_cameras.py 反投影\n"
|
||||||
|
" 到 world、检查是否命中地面/台面，才算通过。\n"
|
||||||
|
" ⚠ 深度话题是 16UC1 毫米（已从 gz 的 32FC1 米转换过）。\n"
|
||||||
|
" 内参也不是 gz 发的那份：gz 恒发 fx=277/cx=160/cy=120（它的默认\n"
|
||||||
|
" 60° 视场 + 半分辨率主点），适配器已按 cameras.yaml 的视场重算覆盖。\n"
|
||||||
|
" 所以改相机视场要同时改 cameras.yaml 和 SDF，启动时会自动比对告警。"
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -38,6 +38,10 @@ setup(
|
|||||||
# 它只做"准备描述 + 转发给 launch 文件"，逻辑在
|
# 它只做"准备描述 + 转发给 launch 文件"，逻辑在
|
||||||
# arm_sim_gazebo/gazebo_sim_launcher.py。
|
# arm_sim_gazebo/gazebo_sim_launcher.py。
|
||||||
'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
|
'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
|
||||||
|
# 仿真相机适配器：把桥接过来的 gz 相机变成应用期望的 realsense
|
||||||
|
# 话题（改名 + 光学系 frame_id + 深度 米→毫米）。
|
||||||
|
# 由 gazebo_sim.launch.py 拉起，一般不手跑；单独调试时可以用。
|
||||||
|
'sim_camera_adapter = arm_sim_gazebo.sim_camera_adapter:main',
|
||||||
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
|
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
|
||||||
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
|
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
|
||||||
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
|
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
|
||||||
|
|||||||
@@ -21,6 +21,7 @@ from arm_sim_gazebo.robot_description import ( # noqa: E402
|
|||||||
check_base_plugins,
|
check_base_plugins,
|
||||||
default_world,
|
default_world,
|
||||||
expand_xacro,
|
expand_xacro,
|
||||||
|
model_link_pose,
|
||||||
prepare_urdf,
|
prepare_urdf,
|
||||||
repo_root,
|
repo_root,
|
||||||
world_name,
|
world_name,
|
||||||
@@ -262,3 +263,218 @@ def test_prepare_urdf_missing_controllers_is_actionable(tmp_path):
|
|||||||
with pytest.raises(SimDescriptionError) as ei:
|
with pytest.raises(SimDescriptionError) as ei:
|
||||||
prepare_urdf(controllers_file=tmp_path / "nope.yaml")
|
prepare_urdf(controllers_file=tmp_path / "nope.yaml")
|
||||||
assert "控制器配置不存在" in str(ei.value)
|
assert "控制器配置不存在" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# 从 world 文件解析模型位姿（世界相机的 TF 靠它）
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# 这组测试存在的理由：Gazebo 不为 world 里的 <model> 产生 ROS TF，
|
||||||
|
# 而 D435 这类固定相机就属于这种情况。位姿靠解析而不是重复配置，
|
||||||
|
# 因此"解析对不对"直接决定了相机看到的东西和它自称的位置是否一致。
|
||||||
|
# 解析错了不会报错，只会让三维点整体偏移或转错。
|
||||||
|
|
||||||
|
|
||||||
|
def _world_file(tmp_path, body: str) -> Path:
|
||||||
|
p = tmp_path / "w.world"
|
||||||
|
p.write_text(f'<?xml version="1.0"?>\n<sdf version="1.9">\n{body}\n</sdf>\n', encoding="utf-8")
|
||||||
|
return p
|
||||||
|
|
||||||
|
|
||||||
|
def test_model_pose_identity_link(tmp_path):
|
||||||
|
"""link 没有自己的 pose 时，位姿就是模型位姿。"""
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name="w">
|
||||||
|
<model name="d435">
|
||||||
|
<static>true</static>
|
||||||
|
<pose>-0.45 0.0 1.35 0.0 0.872665 3.14159</pose>
|
||||||
|
<link name="link">
|
||||||
|
<sensor name="cam" type="rgbd_camera">
|
||||||
|
<pose>0.1 0 0 0 0 0</pose>
|
||||||
|
</sensor>
|
||||||
|
</link>
|
||||||
|
</model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
(pos, rpy) = model_link_pose(w, "d435", "link")
|
||||||
|
assert pos == pytest.approx((-0.45, 0.0, 1.35))
|
||||||
|
assert rpy == pytest.approx((0.0, 0.872665, 3.14159))
|
||||||
|
|
||||||
|
|
||||||
|
def test_link_own_pose_is_composed(tmp_path):
|
||||||
|
"""SDF 的 <pose> 是相对父元素的，模型位姿与 link 位姿必须复合。"""
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name="w">
|
||||||
|
<model name="m">
|
||||||
|
<pose>1 0 0 0 0 0</pose>
|
||||||
|
<link name="l">
|
||||||
|
<pose>0 1 0 0 0 0</pose>
|
||||||
|
<visual name="v"/>
|
||||||
|
</link>
|
||||||
|
</model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
pos, _ = model_link_pose(w, "m", "l")
|
||||||
|
assert pos == pytest.approx((1.0, 1.0, 0.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_link_translation_is_rotated_by_model_rotation(tmp_path):
|
||||||
|
"""模型绕 Z 转 90° 时，link 的 (1,0,0) 应落到世界的 (0,1,0)。"""
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name="w">
|
||||||
|
<model name="m">
|
||||||
|
<pose>0 0 0 0 0 1.5707963</pose>
|
||||||
|
<link name="l">
|
||||||
|
<pose>1 0 0 0 0 0</pose>
|
||||||
|
<visual name="v"/>
|
||||||
|
</link>
|
||||||
|
</model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
pos, rpy = model_link_pose(w, "m", "l")
|
||||||
|
assert pos == pytest.approx((0.0, 1.0, 0.0), abs=1e-6)
|
||||||
|
assert rpy[2] == pytest.approx(1.5707963, abs=1e-6)
|
||||||
|
|
||||||
|
|
||||||
|
def test_link_own_pose_ignores_child_visual_pose(tmp_path):
|
||||||
|
"""link 自己的 pose 必须在第一个 <visual>/<collision>/<sensor> 之前取，
|
||||||
|
否则会把子元素的 pose 误当成 link 的。"""
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name="w">
|
||||||
|
<model name="m">
|
||||||
|
<pose>2 2 2 0 0 0</pose>
|
||||||
|
<link name="l">
|
||||||
|
<pose>1 1 1 0 0 0</pose>
|
||||||
|
<visual name="v">
|
||||||
|
<pose>9 9 9 0 0 0</pose>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
</model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
pos, _ = model_link_pose(w, "m", "l")
|
||||||
|
assert pos == pytest.approx((3.0, 3.0, 3.0)) # 不是 2+9
|
||||||
|
|
||||||
|
|
||||||
|
def test_missing_pose_defaults_to_identity(tmp_path):
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name="w">
|
||||||
|
<model name="m">
|
||||||
|
<link name="l"><visual name="v"/></link>
|
||||||
|
</model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
pos, rpy = model_link_pose(w, "m", "l")
|
||||||
|
assert pos == pytest.approx((0.0, 0.0, 0.0))
|
||||||
|
assert rpy == pytest.approx((0.0, 0.0, 0.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_model_only_query(tmp_path):
|
||||||
|
"""link 留空 = 取模型本身位姿。"""
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name="w">
|
||||||
|
<model name="m"><pose>5 6 7 0 0 0</pose><link name="l"/></model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
pos, _ = model_link_pose(w, "m")
|
||||||
|
assert pos == pytest.approx((5.0, 6.0, 7.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_single_quotes_accepted(tmp_path):
|
||||||
|
"""手写 world 时单引号也会出现，两种都要认。"""
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name='w'>
|
||||||
|
<model name='m'><pose>1 2 3 0 0 0</pose><link name='l'/></model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
pos, _ = model_link_pose(w, "m", "l")
|
||||||
|
assert pos == pytest.approx((1.0, 2.0, 3.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_nested_model_does_not_break_extraction(tmp_path):
|
||||||
|
"""<model> 可以嵌套 <model>（组合体）。找第一个 </model> 会截断错位，
|
||||||
|
所以 _find_block 要数嵌套深度。"""
|
||||||
|
w = _world_file(
|
||||||
|
tmp_path,
|
||||||
|
"""
|
||||||
|
<world name="w">
|
||||||
|
<model name="outer">
|
||||||
|
<pose>0 0 0 0 0 0</pose>
|
||||||
|
<model name="inner">
|
||||||
|
<pose>9 9 9 0 0 0</pose>
|
||||||
|
<link name="l"><visual name="v"/></link>
|
||||||
|
</model>
|
||||||
|
<link name="outer_link"><pose>1 0 0 0 0 0</pose><visual name="v"/></link>
|
||||||
|
</model>
|
||||||
|
</world>
|
||||||
|
""",
|
||||||
|
)
|
||||||
|
pos, _ = model_link_pose(w, "outer", "outer_link")
|
||||||
|
assert pos == pytest.approx((1.0, 0.0, 0.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_missing_model_error_mentions_cameras_yaml(tmp_path):
|
||||||
|
"""报错要指向 cameras.yaml 的 world_model 字段 —— 配错时症状是
|
||||||
|
'tf 查询报 frame does not exist'，与相机本身无关，容易查错方向。"""
|
||||||
|
w = _world_file(tmp_path, '<world name="w"><model name="other"/></world>')
|
||||||
|
with pytest.raises(SimDescriptionError) as ei:
|
||||||
|
model_link_pose(w, "d435", "link")
|
||||||
|
assert "d435" in str(ei.value)
|
||||||
|
assert "cameras.yaml" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_missing_link_error_is_clear(tmp_path):
|
||||||
|
w = _world_file(tmp_path, '<world name="w"><model name="m"><link name="other"/></model></world>')
|
||||||
|
with pytest.raises(SimDescriptionError) as ei:
|
||||||
|
model_link_pose(w, "m", "link")
|
||||||
|
assert "link" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_real_assembly_world_d435_pose():
|
||||||
|
"""仓库里那份 assembly.world 的 d435 位姿必须与文件里写的一致。
|
||||||
|
|
||||||
|
这条是"防回归"的：d435 的 TF 完全依赖这个解析结果，
|
||||||
|
位姿解析悄悄出错不会报任何错，只会让相机看到的东西与自称位置不符。
|
||||||
|
"""
|
||||||
|
root = None
|
||||||
|
for cand in [Path.cwd(), Path.cwd().parent, Path(__file__).resolve().parent]:
|
||||||
|
cur = cand.resolve()
|
||||||
|
for _ in range(8):
|
||||||
|
if (cur / "devices/sim/gazebo/urdf/arm.sim.urdf.xacro").is_file():
|
||||||
|
root = cur
|
||||||
|
break
|
||||||
|
cur = cur.parent
|
||||||
|
if root:
|
||||||
|
break
|
||||||
|
if root is None:
|
||||||
|
pytest.skip("找不到仓库根")
|
||||||
|
|
||||||
|
world = root / "devices/sim/gazebo/worlds/assembly.world"
|
||||||
|
if not world.is_file():
|
||||||
|
pytest.skip("assembly.world 不存在")
|
||||||
|
|
||||||
|
pos, rpy = model_link_pose(world, "d435", "link")
|
||||||
|
assert pos == pytest.approx((-0.45, 0.0, 1.35), abs=1e-6)
|
||||||
|
# yaw 必须是 0：曾经写成 3.14159，把视线从"+X 俯视"翻成"-X 俯视"，
|
||||||
|
# 相机于是背对台面、只看得见地面。这条断言就是防止它被改回去。
|
||||||
|
assert rpy == pytest.approx((0.0, 0.872665, 0.0), abs=1e-6)
|
||||||
|
|||||||
@@ -0,0 +1,407 @@
|
|||||||
|
"""sim_cameras 的单测。
|
||||||
|
|
||||||
|
重点在深度换算：米 → 毫米差 1000 倍，而单目预览里**看不出任何异常**，
|
||||||
|
只能靠测试兜住。另一类是配置错误（ros_ns 重复、缺必填键），
|
||||||
|
它们会让两台相机互相覆盖话题，症状是"只有一台出图"，也很难查。
|
||||||
|
"""
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_sim_gazebo.sim_cameras import (
|
||||||
|
MAX_MM,
|
||||||
|
CameraConfigError,
|
||||||
|
CameraSpec,
|
||||||
|
bridge_args,
|
||||||
|
depth_m_to_mm16,
|
||||||
|
load_cameras,
|
||||||
|
rpy_to_quat,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------------------- 辅助
|
||||||
|
def _write(tmp_path, text):
|
||||||
|
p = tmp_path / "cameras.yaml"
|
||||||
|
p.write_text(text, encoding="utf-8")
|
||||||
|
return p
|
||||||
|
|
||||||
|
|
||||||
|
MINIMAL = """
|
||||||
|
cameras:
|
||||||
|
- gz_ns: /d405
|
||||||
|
ros_ns: d405
|
||||||
|
link_frame: sim_d405_link
|
||||||
|
optical_frame: sim_d405_color_optical_frame
|
||||||
|
horizontal_fov: 1.5184
|
||||||
|
"""
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------- 话题名派生
|
||||||
|
def test_topic_names_follow_realsense_convention():
|
||||||
|
"""话题名必须与真机 realsense 完全一致，否则应用要为仿真改代码。"""
|
||||||
|
s = CameraSpec(
|
||||||
|
gz_ns="/d405",
|
||||||
|
ros_ns="d405",
|
||||||
|
link_frame="sim_d405_link",
|
||||||
|
optical_frame="sim_d405_color_optical_frame",
|
||||||
|
)
|
||||||
|
assert s.ros_color == "/d405/color/image_raw"
|
||||||
|
assert s.ros_info == "/d405/color/camera_info"
|
||||||
|
assert s.ros_depth == "/d405/aligned_depth_to_color/image_raw"
|
||||||
|
assert s.gz_image == "/d405/image"
|
||||||
|
assert s.gz_depth == "/d405/depth_image"
|
||||||
|
assert s.gz_info == "/d405/camera_info"
|
||||||
|
|
||||||
|
|
||||||
|
def test_gz_ns_normalized_without_double_slash():
|
||||||
|
s = CameraSpec(gz_ns="/d405", ros_ns="d405", link_frame="l", optical_frame="o")
|
||||||
|
assert "//" not in s.gz_image
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------- 配置加载
|
||||||
|
def test_load_minimal_config(tmp_path):
|
||||||
|
specs = load_cameras(_write(tmp_path, MINIMAL))
|
||||||
|
assert len(specs) == 1
|
||||||
|
assert specs[0].ros_ns == "d405"
|
||||||
|
assert specs[0].depth is True # 默认开深度
|
||||||
|
assert specs[0].is_world_mounted is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_load_missing_file_raises_with_hint(tmp_path):
|
||||||
|
with pytest.raises(CameraConfigError) as e:
|
||||||
|
load_cameras(tmp_path / "nope.yaml")
|
||||||
|
# 报错必须给出修法，而不是一句 FileNotFoundError
|
||||||
|
assert "cameras.yaml" in str(e.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_load_empty_list_raises(tmp_path):
|
||||||
|
with pytest.raises(CameraConfigError) as e:
|
||||||
|
load_cameras(_write(tmp_path, "cameras: []\n"))
|
||||||
|
assert "cameras" in str(e.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_load_missing_required_key_raises(tmp_path):
|
||||||
|
bad = """
|
||||||
|
cameras:
|
||||||
|
- gz_ns: /d405
|
||||||
|
ros_ns: d405
|
||||||
|
link_frame: sim_d405_link
|
||||||
|
"""
|
||||||
|
with pytest.raises(CameraConfigError) as e:
|
||||||
|
load_cameras(_write(tmp_path, bad))
|
||||||
|
assert "optical_frame" in str(e.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_duplicate_ros_ns_rejected(tmp_path):
|
||||||
|
"""两台相机用同一个 ros_ns 会互相覆盖话题，表现为'只有一台出图'。"""
|
||||||
|
dup = """
|
||||||
|
cameras:
|
||||||
|
- gz_ns: /d405
|
||||||
|
ros_ns: d405
|
||||||
|
link_frame: a
|
||||||
|
optical_frame: b
|
||||||
|
horizontal_fov: 1.5184
|
||||||
|
- gz_ns: /d406
|
||||||
|
ros_ns: d405
|
||||||
|
link_frame: c
|
||||||
|
optical_frame: d
|
||||||
|
horizontal_fov: 1.5184
|
||||||
|
"""
|
||||||
|
with pytest.raises(CameraConfigError) as e:
|
||||||
|
load_cameras(_write(tmp_path, dup))
|
||||||
|
assert "重复" in str(e.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_world_mounted_flag(tmp_path):
|
||||||
|
cfg = """
|
||||||
|
cameras:
|
||||||
|
- gz_ns: /d435
|
||||||
|
ros_ns: d435
|
||||||
|
link_frame: d435_link
|
||||||
|
optical_frame: d435_color_optical_frame
|
||||||
|
horizontal_fov: 1.5184
|
||||||
|
world_model: d435
|
||||||
|
world_link: link
|
||||||
|
"""
|
||||||
|
specs = load_cameras(_write(tmp_path, cfg))
|
||||||
|
assert specs[0].is_world_mounted is True
|
||||||
|
assert specs[0].world_link == "link"
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------- 深度换算
|
||||||
|
def test_depth_basic_conversion():
|
||||||
|
"""1.5m → 1500mm。这是最容易错的那一项（差 1000 倍）。"""
|
||||||
|
d = np.array([[1.5, 0.25]], dtype=np.float32)
|
||||||
|
mm = depth_m_to_mm16(d)
|
||||||
|
assert mm.dtype == np.uint16
|
||||||
|
assert mm.tolist() == [[1500, 250]]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_rounding():
|
||||||
|
d = np.array([1.2344, 1.2346], dtype=np.float32)
|
||||||
|
mm = depth_m_to_mm16(d)
|
||||||
|
assert mm.tolist() == [1234, 1235]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_non_finite_becomes_zero():
|
||||||
|
"""gz 在没命中时可能给 inf，真机 realsense 给 0 —— 统一成 0。"""
|
||||||
|
d = np.array([np.inf, -np.inf, np.nan], dtype=np.float32)
|
||||||
|
assert depth_m_to_mm16(d).tolist() == [0, 0, 0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_zero_and_negative_become_zero():
|
||||||
|
d = np.array([0.0, -0.5], dtype=np.float32)
|
||||||
|
assert depth_m_to_mm16(d).tolist() == [0, 0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_too_small_rounds_to_zero():
|
||||||
|
"""0.0001m 舍入后是 0mm —— 必须落到'无读数'，不能变成 1 或负数回绕。"""
|
||||||
|
d = np.array([0.0001, 0.0004, 0.0006], dtype=np.float32)
|
||||||
|
assert depth_m_to_mm16(d).tolist() == [0, 0, 1]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_beyond_uint16_becomes_zero_not_saturated():
|
||||||
|
"""超出 65.535m 按'无读数'处理。
|
||||||
|
|
||||||
|
饱和到 65535 会让远处凭空出现一个 65.5m 的平面，比'无效'更有害 ——
|
||||||
|
下游会把它当成真实几何去用。
|
||||||
|
"""
|
||||||
|
d = np.array([70.0, 100.0, 0.0], dtype=np.float32)
|
||||||
|
assert depth_m_to_mm16(d).tolist() == [0, 0, 0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_exact_uint16_max_is_kept():
|
||||||
|
d = np.array([MAX_MM / 1000.0], dtype=np.float32)
|
||||||
|
assert depth_m_to_mm16(d).tolist() == [MAX_MM]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_shape_preserved():
|
||||||
|
d = np.full((480, 640), 1.0, dtype=np.float32)
|
||||||
|
mm = depth_m_to_mm16(d)
|
||||||
|
assert mm.shape == (480, 640)
|
||||||
|
assert mm.dtype == np.uint16
|
||||||
|
assert (mm == 1000).all()
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_accepts_float64_input():
|
||||||
|
"""上游若给了 float64 也不能崩（gz 是 float32，但不做假设）。"""
|
||||||
|
d = np.array([2.0], dtype=np.float64)
|
||||||
|
assert depth_m_to_mm16(d).tolist() == [2000]
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------- 桥接参数
|
||||||
|
def test_bridge_args_include_depth_when_enabled():
|
||||||
|
specs = [
|
||||||
|
CameraSpec(gz_ns="/d405", ros_ns="d405", link_frame="l", optical_frame="o", depth=True)
|
||||||
|
]
|
||||||
|
args = bridge_args(specs)
|
||||||
|
assert "/d405/image@sensor_msgs/msg/Image[gz.msgs.Image" in args
|
||||||
|
assert "/d405/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo" in args
|
||||||
|
assert "/d405/depth_image@sensor_msgs/msg/Image[gz.msgs.Image" in args
|
||||||
|
|
||||||
|
|
||||||
|
def test_bridge_args_skip_depth_when_disabled():
|
||||||
|
specs = [
|
||||||
|
CameraSpec(gz_ns="/d405", ros_ns="d405", link_frame="l", optical_frame="o", depth=False)
|
||||||
|
]
|
||||||
|
args = bridge_args(specs)
|
||||||
|
assert not any("depth_image" in a for a in args)
|
||||||
|
assert len(args) == 2
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------- 旋转工具
|
||||||
|
def test_rpy_to_quat_identity():
|
||||||
|
x, y, z, w = rpy_to_quat((0.0, 0.0, 0.0))
|
||||||
|
assert (x, y, z, w) == pytest.approx((0.0, 0.0, 0.0, 1.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_rpy_to_quat_is_unit_norm():
|
||||||
|
for rpy in [(-1.5708, 0.0, -1.5708), (0.0, 0.872665, 3.14159), (0.3, -0.7, 1.2)]:
|
||||||
|
x, y, z, w = rpy_to_quat(rpy)
|
||||||
|
assert np.isclose(x * x + y * y + z * z + w * w, 1.0, atol=1e-9)
|
||||||
|
|
||||||
|
|
||||||
|
def test_optical_rpy_is_the_documented_value():
|
||||||
|
"""光学系的旋转值本身也是契约：写错这一项三维点会整体转错。"""
|
||||||
|
from arm_sim_gazebo.sim_cameras import OPTICAL_RPY
|
||||||
|
|
||||||
|
assert OPTICAL_RPY == pytest.approx((-1.5708, 0.0, -1.5708))
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------------- 内参重算
|
||||||
|
#
|
||||||
|
# 这一组是"仿真能不能用"的地基：gz 自己发的 camera_info 是错的，内参全靠
|
||||||
|
# 这里算。算错的话仿真里所有三维点位都偏，而单目预览完全正常。
|
||||||
|
def _spec(fov=1.5184, **kw):
|
||||||
|
return CameraSpec(
|
||||||
|
gz_ns="/c",
|
||||||
|
ros_ns="c",
|
||||||
|
link_frame="l",
|
||||||
|
optical_frame="o",
|
||||||
|
horizontal_fov=fov,
|
||||||
|
**kw,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_intrinsics_match_the_measured_ground_truth():
|
||||||
|
"""用逆解实测出来的值反验：87° 视场、640x480 → fx=337.2, cx=320, cy=240。
|
||||||
|
|
||||||
|
这组数是本机拿已知地面平面做最小二乘解出来的（残差中位 0.2mm），
|
||||||
|
不是从公式推的 —— 所以它能同时抓住"公式写错"和"视场理解错"。
|
||||||
|
"""
|
||||||
|
from arm_sim_gazebo.sim_cameras import intrinsics
|
||||||
|
|
||||||
|
k, p = intrinsics(_spec(), 640, 480)
|
||||||
|
assert k[0] == pytest.approx(337.2, abs=0.1) # fx
|
||||||
|
assert k[4] == pytest.approx(337.2, abs=0.1) # fy
|
||||||
|
assert k[2] == pytest.approx(320.0) # cx
|
||||||
|
assert k[5] == pytest.approx(240.0) # cy
|
||||||
|
assert p[:4] == pytest.approx([k[0], 0.0, k[2], 0.0])
|
||||||
|
assert k[1] == k[3] == k[6] == k[7] == 0.0 and k[8] == 1.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_intrinsics_scales_with_fov_and_size():
|
||||||
|
"""视场变大 → 焦距变小；分辨率翻倍 → 焦距与主点同步翻倍。"""
|
||||||
|
from arm_sim_gazebo.sim_cameras import intrinsics
|
||||||
|
|
||||||
|
k_narrow, _ = intrinsics(_spec(fov=1.0), 640, 480)
|
||||||
|
k_wide, _ = intrinsics(_spec(fov=2.0), 640, 480)
|
||||||
|
assert k_narrow[0] > k_wide[0]
|
||||||
|
|
||||||
|
k_1x, _ = intrinsics(_spec(), 640, 480)
|
||||||
|
k_2x, _ = intrinsics(_spec(), 1280, 960)
|
||||||
|
assert k_2x[0] == pytest.approx(2 * k_1x[0])
|
||||||
|
assert k_2x[2] == pytest.approx(2 * k_1x[2])
|
||||||
|
assert k_2x[5] == pytest.approx(2 * k_1x[5])
|
||||||
|
|
||||||
|
|
||||||
|
def test_intrinsics_rejects_bad_fov():
|
||||||
|
from arm_sim_gazebo.sim_cameras import intrinsics
|
||||||
|
|
||||||
|
for bad in (0.0, -1.0, math.pi, 4.0):
|
||||||
|
with pytest.raises(CameraConfigError):
|
||||||
|
intrinsics(_spec(fov=bad), 640, 480)
|
||||||
|
|
||||||
|
|
||||||
|
def test_intrinsics_rejects_bad_size():
|
||||||
|
from arm_sim_gazebo.sim_cameras import intrinsics
|
||||||
|
|
||||||
|
for w, h in ((0, 480), (640, 0), (-1, 480)):
|
||||||
|
with pytest.raises(CameraConfigError):
|
||||||
|
intrinsics(_spec(), w, h)
|
||||||
|
|
||||||
|
|
||||||
|
def test_load_cameras_requires_horizontal_fov(tmp_path):
|
||||||
|
"""缺 horizontal_fov 必须报错，不能默认成某个值 —— 默认值错了看不出来。"""
|
||||||
|
p = _write(tmp_path, """
|
||||||
|
cameras:
|
||||||
|
- gz_ns: /d405
|
||||||
|
ros_ns: d405
|
||||||
|
link_frame: sim_d405_link
|
||||||
|
optical_frame: sim_d405_color_optical_frame
|
||||||
|
""")
|
||||||
|
with pytest.raises(CameraConfigError) as ei:
|
||||||
|
load_cameras(p)
|
||||||
|
assert "horizontal_fov" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_load_cameras_rejects_absurd_fov(tmp_path):
|
||||||
|
p = _write(tmp_path, """
|
||||||
|
cameras:
|
||||||
|
- gz_ns: /d405
|
||||||
|
ros_ns: d405
|
||||||
|
link_frame: sim_d405_link
|
||||||
|
optical_frame: sim_d405_color_optical_frame
|
||||||
|
horizontal_fov: 0
|
||||||
|
""")
|
||||||
|
with pytest.raises(CameraConfigError) as ei:
|
||||||
|
load_cameras(p)
|
||||||
|
assert "horizontal_fov" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
# ----------------------------------------------------------- 视场一致性比对
|
||||||
|
def test_fov_check_flags_world_camera_mismatch(tmp_path):
|
||||||
|
"""世界相机：按 <topic> 精确匹配，能抓到 cameras.yaml 与 world 不一致。"""
|
||||||
|
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||||
|
|
||||||
|
world = tmp_path / "assembly.world"
|
||||||
|
world.write_text(
|
||||||
|
"""
|
||||||
|
<sdf><world name="w">
|
||||||
|
<model name="d435"><link name="link"><sensor name="s" type="rgbd_camera">
|
||||||
|
<topic>/d435</topic>
|
||||||
|
<camera><horizontal_fov>1.5184</horizontal_fov></camera>
|
||||||
|
</sensor></link></model>
|
||||||
|
</world></sdf>
|
||||||
|
""",
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
spec = CameraSpec(
|
||||||
|
gz_ns="/d435",
|
||||||
|
ros_ns="d435",
|
||||||
|
link_frame="d435_link",
|
||||||
|
optical_frame="d435_color_optical_frame",
|
||||||
|
horizontal_fov=1.5184,
|
||||||
|
world_model="d435",
|
||||||
|
world_link="link",
|
||||||
|
)
|
||||||
|
assert check_fov_consistency([spec], world_file=world) == []
|
||||||
|
|
||||||
|
bad = CameraSpec(**{**spec.__dict__, "horizontal_fov": 1.2})
|
||||||
|
issues = check_fov_consistency([bad], world_file=world)
|
||||||
|
assert len(issues) == 1 and "1.5184" in issues[0] and "1.2" in issues[0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_fov_check_flags_urdf_camera_when_value_absent(tmp_path):
|
||||||
|
"""腕上相机：视场在 xacro 里，只能"文件里出现过才算过"，但要能抓到缺失。"""
|
||||||
|
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||||
|
|
||||||
|
xacro = tmp_path / "arm.sim.sensors.xacro"
|
||||||
|
xacro.write_text("<horizontal_fov>1.5184</horizontal_fov>", encoding="utf-8")
|
||||||
|
spec = _spec()
|
||||||
|
assert check_fov_consistency([spec], sensors_xacro=xacro) == []
|
||||||
|
|
||||||
|
xacro.write_text("<horizontal_fov>1.2</horizontal_fov>", encoding="utf-8")
|
||||||
|
issues = check_fov_consistency([spec], sensors_xacro=xacro)
|
||||||
|
assert len(issues) == 1 and "1.2" in issues[0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_fov_check_warns_when_world_camera_sensor_missing(tmp_path):
|
||||||
|
"""world 里找不到对应 <sensor> 时要告警，而不是静默放过。"""
|
||||||
|
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||||
|
|
||||||
|
world = tmp_path / "assembly.world"
|
||||||
|
world.write_text("<sdf><world name='w'/></sdf>", encoding="utf-8")
|
||||||
|
spec = CameraSpec(
|
||||||
|
gz_ns="/d435",
|
||||||
|
ros_ns="d435",
|
||||||
|
link_frame="d435_link",
|
||||||
|
optical_frame="d435_color_optical_frame",
|
||||||
|
horizontal_fov=1.5184,
|
||||||
|
world_model="d435",
|
||||||
|
)
|
||||||
|
issues = check_fov_consistency([spec], world_file=world)
|
||||||
|
assert len(issues) == 1 and "找不到" in issues[0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_fov_check_against_real_repo_files():
|
||||||
|
"""对仓库里真实的两份 SDF 做一次回归：现在应当一致。
|
||||||
|
|
||||||
|
这条测试的价值在于：以后谁改了 arm.sim.sensors.xacro 或 assembly.world 的
|
||||||
|
视场却忘了同步 cameras.yaml，这里就会红。内参不一致是本模块开头
|
||||||
|
警告的那类"看不出来"的错误，必须有个自动化兜底。
|
||||||
|
"""
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from arm_sim_gazebo.sim_cameras import check_fov_consistency
|
||||||
|
|
||||||
|
root = Path(__file__).resolve().parents[3]
|
||||||
|
specs = load_cameras(root=root)
|
||||||
|
issues = check_fov_consistency(
|
||||||
|
specs,
|
||||||
|
world_file=root / "devices/sim/gazebo/worlds/assembly.world",
|
||||||
|
sensors_xacro=root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
|
||||||
|
)
|
||||||
|
assert issues == [], "视场与 SDF 不一致：\n" + "\n".join(issues)
|
||||||
@@ -0,0 +1,172 @@
|
|||||||
|
"""sim_site 的单测。
|
||||||
|
|
||||||
|
这个模块存在的理由就是"别再让 site.yaml 静默无效"，所以用例的重点是：
|
||||||
|
- 优先级真的按 --world > site.yaml > 内置默认 走；
|
||||||
|
- site.yaml 缺失/写坏时的行为是可预期的（不崩、但有清晰报错）；
|
||||||
|
- 声明了却没接线的键**必须**告警，而不是安静地忽略。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_sim_gazebo.sim_site import (
|
||||||
|
DEFAULT_JOINT_PREFIX,
|
||||||
|
SimSite,
|
||||||
|
SimSiteError,
|
||||||
|
load_sim_site,
|
||||||
|
physics_enabled_in_world,
|
||||||
|
resolve_world,
|
||||||
|
unwired_warnings,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _site(tmp_path: Path, body: str) -> Path:
|
||||||
|
(tmp_path / "config").mkdir(parents=True, exist_ok=True)
|
||||||
|
(tmp_path / "config" / "site.yaml").write_text(body, encoding="utf-8")
|
||||||
|
return tmp_path
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------ 读取
|
||||||
|
def test_missing_site_yaml_falls_back_to_defaults(tmp_path):
|
||||||
|
"""site.yaml 不存在不算错 —— 它含真实设备 ID，本就不该提交。"""
|
||||||
|
site = load_sim_site(tmp_path)
|
||||||
|
assert site.world == ""
|
||||||
|
assert site.physics_enable is True
|
||||||
|
assert site.joint_prefix == DEFAULT_JOINT_PREFIX
|
||||||
|
assert site.source is None
|
||||||
|
assert site.present == set()
|
||||||
|
|
||||||
|
|
||||||
|
def test_reads_sim_section(tmp_path):
|
||||||
|
root = _site(
|
||||||
|
tmp_path,
|
||||||
|
"sim:\n world: devices/sim/gazebo/worlds/assembly.world\n"
|
||||||
|
" physics_enable: false\n joint_prefix: 'sim_'\n",
|
||||||
|
)
|
||||||
|
site = load_sim_site(root)
|
||||||
|
assert site.world == "devices/sim/gazebo/worlds/assembly.world"
|
||||||
|
assert site.physics_enable is False
|
||||||
|
assert site.joint_prefix == "sim_"
|
||||||
|
assert site.present == {"world", "physics_enable", "joint_prefix"}
|
||||||
|
|
||||||
|
|
||||||
|
def test_no_sim_section_is_fine(tmp_path):
|
||||||
|
site = load_sim_site(_site(tmp_path, "robot:\n sn: X\n"))
|
||||||
|
assert site.world == ""
|
||||||
|
assert site.present == set()
|
||||||
|
|
||||||
|
|
||||||
|
def test_malformed_yaml_raises_with_hint(tmp_path):
|
||||||
|
with pytest.raises(SimSiteError) as ei:
|
||||||
|
load_sim_site(_site(tmp_path, "sim: [unclosed\n"))
|
||||||
|
assert "site.yaml" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_non_mapping_sim_section_raises(tmp_path):
|
||||||
|
"""sim 段写成列表属于典型手误（缩进错了），要报出来而不是当空处理。"""
|
||||||
|
with pytest.raises(SimSiteError):
|
||||||
|
load_sim_site(_site(tmp_path, "sim:\n - world\n"))
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------- 优先级
|
||||||
|
def test_cli_world_beats_site_yaml(tmp_path):
|
||||||
|
root = _site(tmp_path, "sim:\n world: from_site.world\n")
|
||||||
|
p, origin = resolve_world("from_cli.world", root, fallback=Path("fallback.world"))
|
||||||
|
assert p == root / "from_cli.world"
|
||||||
|
assert "命令行" in origin
|
||||||
|
|
||||||
|
|
||||||
|
def test_site_yaml_beats_builtin_default(tmp_path):
|
||||||
|
root = _site(tmp_path, "sim:\n world: from_site.world\n")
|
||||||
|
p, origin = resolve_world("", root, fallback=Path("fallback.world"))
|
||||||
|
assert p == root / "from_site.world"
|
||||||
|
assert "sim.world" in origin
|
||||||
|
|
||||||
|
|
||||||
|
def test_builtin_default_used_when_site_silent(tmp_path):
|
||||||
|
root = _site(tmp_path, "other: 1\n")
|
||||||
|
p, origin = resolve_world("", root, fallback=Path("devices/sim/gazebo/worlds/x.world"))
|
||||||
|
assert p == root / "devices/sim/gazebo/worlds/x.world"
|
||||||
|
assert "内置默认" in origin
|
||||||
|
|
||||||
|
|
||||||
|
def test_absolute_paths_are_not_rewritten(tmp_path):
|
||||||
|
root = _site(tmp_path, "sim:\n world: from_site.world\n")
|
||||||
|
p, _ = resolve_world("/tmp/abs.world", root, fallback=Path("f.world"))
|
||||||
|
assert p == Path("/tmp/abs.world")
|
||||||
|
p2, _ = resolve_world("", tmp_path, fallback=Path("f.world"))
|
||||||
|
assert p2.is_absolute()
|
||||||
|
|
||||||
|
|
||||||
|
def test_raises_when_nothing_available(tmp_path):
|
||||||
|
with pytest.raises(SimSiteError):
|
||||||
|
resolve_world("", tmp_path, fallback=None)
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------------------------------- 未接线键的告警
|
||||||
|
def test_unwired_joint_prefix_warns(tmp_path):
|
||||||
|
"""site.yaml 写了一个跟实际不同的前缀 → 必须告警，不能静默。"""
|
||||||
|
site = load_sim_site(_site(tmp_path, "sim:\n joint_prefix: 'robot_'\n"))
|
||||||
|
msgs = unwired_warnings(site, actual_joint_prefix="sim_")
|
||||||
|
assert len(msgs) == 1
|
||||||
|
assert "'robot_'" in msgs[0] and "'sim_'" in msgs[0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_matching_joint_prefix_does_not_warn(tmp_path):
|
||||||
|
site = load_sim_site(_site(tmp_path, "sim:\n joint_prefix: 'sim_'\n"))
|
||||||
|
assert unwired_warnings(site, actual_joint_prefix="sim_") == []
|
||||||
|
|
||||||
|
|
||||||
|
def test_unset_keys_never_warn(tmp_path):
|
||||||
|
"""site.yaml 没提这些键时不该有任何告警 —— 默认值不需要解释。"""
|
||||||
|
site = load_sim_site(_site(tmp_path, "sim:\n world: w.world\n"))
|
||||||
|
assert unwired_warnings(site, actual_joint_prefix="sim_", physics_in_world=True) == []
|
||||||
|
|
||||||
|
|
||||||
|
def test_unwired_physics_warns_only_on_conflict(tmp_path):
|
||||||
|
site = load_sim_site(_site(tmp_path, "sim:\n physics_enable: false\n"))
|
||||||
|
# 世界文件里物理其实是开的 → 用户的要求没被满足，必须说
|
||||||
|
assert len(unwired_warnings(site, physics_in_world=True)) == 1
|
||||||
|
# 世界文件里物理本来就关着 → 结果与用户要求一致，没什么可说
|
||||||
|
assert unwired_warnings(site, physics_in_world=False) == []
|
||||||
|
|
||||||
|
|
||||||
|
def test_unwired_warnings_are_skipped_without_actuals():
|
||||||
|
"""拿不到"实际值"时宁可不说，也不要凭猜测告警。"""
|
||||||
|
site = SimSite(joint_prefix="robot_", physics_enable=False)
|
||||||
|
site.present = {"joint_prefix", "physics_enable"}
|
||||||
|
assert unwired_warnings(site) == []
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------------------------- 从世界里判断物理是否开启
|
||||||
|
def test_physics_enabled_in_world(tmp_path):
|
||||||
|
on = tmp_path / "on.world"
|
||||||
|
on.write_text("<sdf><world><physics><gravity>0 0 -9.8</gravity></physics></world></sdf>")
|
||||||
|
assert physics_enabled_in_world(on) is True
|
||||||
|
|
||||||
|
off = tmp_path / "off.world"
|
||||||
|
off.write_text("<sdf><world><physics><gravity>0 0 0</gravity></physics></world></sdf>")
|
||||||
|
assert physics_enabled_in_world(off) is False
|
||||||
|
|
||||||
|
# 没写 gravity → gz 默认是开
|
||||||
|
none = tmp_path / "none.world"
|
||||||
|
none.write_text("<sdf><world/></sdf>")
|
||||||
|
assert physics_enabled_in_world(none) is True
|
||||||
|
|
||||||
|
missing = tmp_path / "nope.world"
|
||||||
|
assert physics_enabled_in_world(missing) is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_real_repo_site_yaml_is_consistent():
|
||||||
|
"""对仓库真实的 site.yaml 做回归：声明的 world 必须真的存在。
|
||||||
|
|
||||||
|
这条能抓住"site.yaml 指向一个不存在的 world"——启动方会报错，
|
||||||
|
但错误发生在几十秒的 Gazebo 初始化之后，不如在这里就红。
|
||||||
|
"""
|
||||||
|
root = Path(__file__).resolve().parents[3]
|
||||||
|
site = load_sim_site(root)
|
||||||
|
if site.source is None:
|
||||||
|
pytest.skip("本机没有 config/site.yaml（属于正常情况）")
|
||||||
|
if site.world:
|
||||||
|
assert (root / site.world).is_file(), f"site.yaml 的 sim.world 不存在: {site.world}"
|
||||||
Reference in New Issue
Block a user