diff --git a/config/site.example.yaml b/config/site.example.yaml
index 327844c..e168114 100644
--- a/config/site.example.yaml
+++ b/config/site.example.yaml
@@ -480,13 +480,19 @@ network:
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
sim:
- # ⚠️ 这一段目前**没有代码读取**（gazebo_sim_launcher 还没实现，见
- # devices/sim/gazebo/manifest.yaml 的进度段）。现在跑 `run_sim.sh` 得到的
- # 仍是 mock 驱动（假数据 + RViz），不是物理仿真。
+ # 这一段由 arm_sim_gazebo.sim_site 真正读取（此前是"声明了但没人读"，
+ # 改了毫无反应也不报错）。三个键的接线状况**不一样**，务必看清：
#
- # 之所以先把 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"
world: "devices/sim/gazebo/worlds/assembly.world"
physics_enable: true
diff --git a/devices/sim/gazebo/README.md b/devices/sim/gazebo/README.md
index 9de997f..29c0622 100644
--- a/devices/sim/gazebo/README.md
+++ b/devices/sim/gazebo/README.md
@@ -1,95 +1,148 @@
# devices/sim/gazebo/ —— Gazebo 仿真平台
-## 先读这段：当前仿真是假的
-
-**现在项目里的「仿真」并不是 Gazebo。** 实际情况是：
-
-- `src/arm_description/urdf/arm.gazebo.xacro` 是零引用的死文件；
-- `arm_bringup/stack.launch.py` 的 `isaac` 分支只有一行日志，没有任何实际动作；
-- `run_sim.sh` 走的是 `sim.launch.py`，底层是 **mock 驱动**（`mock_arm_driver.py`），
- 即用假数据模拟机器人状态，不是物理仿真；
-- Gazebo 从未真正接线。
-
-所以这份文档分两部分：**第一部分是怎么把 Gazebo 真正跑起来**（当前必须做的前置工作），
-**第二部分才是日常怎么用**。不要跳过第一部分。
+> 这份文档记录的是**已经跑通的事实**和**实测踩到的坑**。仿真里绝大多数错误都是
+> 静默的（不报错、能出图、能查到 TF，结果却是错的），所以下面每条坑都写了
+> **怎么验**。改仿真之前先把"坑"那一节看完。
---
-## 第一部分：把 Gazebo 真正接起来（前置工作）
+## 先读这段：哪个"仿真"是真的
-要让 Gazebo 跑起来，缺的是这几块，按顺序补齐：
+仓库里有两个都叫"仿真"的东西，很容易混：
-### 1. URDF 加 Gazebo 标签
+| 入口 | 底层 | 有物理吗 | 用途 |
+|------|------|----------|------|
+| `scripts/run_sim.sh` | `arm_bringup` 的 **mock 驱动**（`mock_arm_driver.py`） | ✘ 假数据 + RViz | 验证话题/流程/UI，不耗资源 |
+| `scripts/run_sim_gz.sh` | **Gazebo Fortress**（真物理、真接触） | ✔ | 验证接触/力控/几何 |
-现有 `arm_description/urdf/arm.gazebo.xacro` 是死文件，需要重写。每个 link 必须补：
+**要验证装配的接触与力判据，只能用后者。** 前者永远不会产生接触力。
-```xml
-
-
- Gazebo/Grey
-
-```
+---
-并在 `arm.ros2_control.xacro` 里把 `ros2_control` 标签的 hardware 插件从
-`flexiv_hardware/FlexivHardware` 换成 `gazebo_ros2_control/GazeboSystem`。
+## 第一部分：现状（截至 P5c-4，2026-09-23）
-### 2. 关节传动与控制器
+### 已就绪
-Gazebo 需要 `transmission` 标签把关节映射到 `ros2_control`。控制器配置可以直接复用
-`arm_control/config/controllers.yaml` 里的 `rizon_arm_controller`，只需把关节名前缀
-从真实 SN（如 `Rizon4-063989_`）换成仿真专用的前缀（如 `sim_`）。
+| 部分 | 位置 | 说明 |
+|------|------|------|
+| 运行环境 | `scripts/install_sim_env.sh` / `sim_env.sh` | 独立 conda 环境 `ros_gz_sim`（Gazebo Fortress 6.16 + ROS 2 Humble），与真机环境 `ros_flexiv` 隔离 |
+| 世界 | `worlds/assembly.world` | 地面 + 工作台 + D435 + 通止规标记位 |
+| 机器人描述 | `urdf/*.xacro` | 复用官方 `flexiv_description`，启动时**归一化**后喂 Gazebo |
+| 控制器 | `config/controllers.yaml` | 控制器名与真机一致（`rizon_arm_controller` + `joint_state_broadcaster`） |
+| 启动器 | `src/arm_sim_gazebo` 的 `gazebo_sim_launcher` | 起 Gazebo → 发归一化 URDF → spawn → 拉起控制器 |
+| 相机 | `config/cameras.yaml` | 两台相机的**唯一真相**：话题、坐标系、视场 |
-### 3. 传感器插件
+### 还没做
-| 仿真对象 | 插件 | 对应真实设备 |
-|----------|------|--------------|
-| 腕部相机 | `libgazebo_ros_camera.so` | D405 |
-| 手外相机 | `libgazebo_ros_camera.so` | D435 |
-| 深度 | `libgazebo_ros_depth_camera.so` | 对齐深度流 |
-| 触觉力 | **需要自己写插件** | Orisys |
+| 部分 | 缺什么 | 影响 |
+|------|--------|------|
+| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 |
+| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） |
-**最关键的一条：Orisys 视触觉没有现成的 Gazebo 插件。**
-3 路切向力 + 1 路法向力 + 深度图 + 力矢量图 这套数据需要自定义 Gazebo 插件来模拟。
-在它做出来之前，仿真里**无法验证通止规装配的力控部分**（S07 接触、S08 拧紧判定）。
-建议的自定义插件做法：订阅 Gazebo 的接触传感器数据，按接触深度和相对滑动速度合成
-出一组「类触觉」读数，再按真实传感器的量纲（任意单位增量）发布到同样的内部话题上。
-
-### 4. 工件与通止规模型
-
-需要给工件（含 k0 中心孔、k1/k2 螺纹孔）和通止规建简化网格，并保证：
-
-- 工件的 k0/k1/k2 三个点在模型坐标系里的位置与实际一致 —— 否则推理出的坐标映射到仿真里会对不上；
-- 螺纹孔要能表达「拧入」的几何阻力，否则 S08 的力反馈无法触发。
+> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 +
+> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据，
+> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到
+> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。
+> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。
---
## 第二部分：日常怎么用
+### 启动
+
+```bash
+bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个）
+bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI
+bash scripts/run_sim_gz.sh --check # 只自检描述，不启动任何东西
+bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源，只验运动学
+```
+
+首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。
+
+### 验收（★ 必做，别只看"有没有图像"）
+
+```bash
+ros2 topic echo /clock --once # 仿真时间在走
+ros2 control list_controllers # 两个控制器 active
+ros2 topic echo /joint_states --once # 7 个 sim_joint
+python3 scripts/check_sim_cameras.py # ★ 相机的真正验收
+```
+
+**`check_sim_cameras.py` 才是相机的验收标准。** 它把深度反投影到 world，
+检查落点是否命中已知平面（地面 z=0、台面 z=0.40）。"有图像"证明不了什么 ——
+详见下面的坑 4 与坑 5。
+
### 放置格式与命名要求
| 类型 | 放哪 | 格式 | 命名要求 |
|------|------|------|----------|
-| 世界文件 | `worlds/` | `.world` 或 `.sdf` | `<场景名>.world`，如 `assembly.world` |
-| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 必须以 `sim_` 为关节前缀，与真实臂区分 |
+| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` |
+| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） |
| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
-| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | 单位必须是米。用毫米建模的必须先缩放到米 |
+| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米**。用毫米建模的必须先缩放到米 |
+| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用启动时的 `camera_name` |
-### 导入一个新场景
+### 改配置：改哪里才会真正生效
-1. 把 `.world` 放进 `worlds/`，网格放进 `models/<模型名>/`
-2. 改 `config/site.yaml` 的 `sim.world` 指向该文件
-3. 改 `devices/registry.yaml` 的 `sim: gazebo`（通常已是）
-4. 启动 `scripts/run_sim.sh`
+| 想改什么 | 改哪里 | 真的生效吗 |
+|----------|--------|------------|
+| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 已接线（优先级：`--world` > `site.yaml` > 内置默认） |
+| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致会告警 |
+| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 里的 `` |
+| 关节前缀 | `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 轴本体缺 `` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN | 只有 Gazebo 拒绝，RViz/MoveIt 完全正常 | 启动时**归一化**（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check` |
+| 2 | world 里只要声明了**任何一个** ``，就会挤掉默认的 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`（相机验收）。
diff --git a/devices/sim/gazebo/config/cameras.yaml b/devices/sim/gazebo/config/cameras.yaml
new file mode 100644
index 0000000..a8f0730
--- /dev/null
+++ b/devices/sim/gazebo/config/cameras.yaml
@@ -0,0 +1,86 @@
+# =============================================================================
+# devices/sim/gazebo/config/cameras.yaml
+#
+# 仿真相机的**唯一数据源**：gazebo_sim.launch.py 里的 ros_gz_bridge 参数、
+# sim_camera_adapter 节点、以及为世界相机补的静态 TF，全部从这份文件派生。
+# 想加一台仿真相机，改这里一处即可（前提是它在 URDF / world 里已经有 ）。
+#
+# 与 site.yaml 的分工：
+# 这里只描述"Gazebo 里有什么、要桥到 ROS 的哪个名字"。
+# site.yaml 描述"真机上有哪些相机、序列号是多少"。仿真不读序列号 ——
+# Gazebo 里没有 USB，序列号无从绑定。
+# 两者的**话题命名约定必须一致**，这是应用层能不改代码跑仿真的前提。
+#
+# 话题命名约定（与真机 realsense 完全对齐，见 arm_application/handeye_realsense.py
+# 的 REALSENSE_TOPIC_DEFAULTS 与 realsense_color_pub.py）：
+# //color/image_raw rgb8
+# //color/camera_info plumb_bob
+# //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 里该 的 horizontal_fov
+# =============================================================================
+
+cameras:
+ # --- 腕上相机（eye-in-hand），挂在机械臂法兰上 ------------------------------
+ - gz_ns: /d405 # Gazebo 传感器话题前缀（see arm.sim.sensors.xacro 的 ）
+ 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 文件里的 ，world_link 是模型里的 。
+ world_model: d435
+ world_link: link
+ note: height 也与真机不同（真机 1280x720），同 D405 的说明
diff --git a/devices/sim/gazebo/manifest.yaml b/devices/sim/gazebo/manifest.yaml
index d5e9caf..5517ceb 100644
--- a/devices/sim/gazebo/manifest.yaml
+++ b/devices/sim/gazebo/manifest.yaml
@@ -3,7 +3,7 @@
#
# Gazebo 仿真平台清单。
#
-# 进度（截至 P5c-3，如实记录，不要凭印象改）：
+# 进度（截至 P5c-4，如实记录，不要凭印象改）：
# ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
# 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh
# ✔ 世界文件 worlds/assembly.world（sdformat 校验通过）
@@ -16,12 +16,17 @@
# 与 build_sim.sh 的提示语引用，照做会报文件不存在）
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做）
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做）
-# ✘ 相机出图 D405/D435 传感器已在 Gazebo 里成功注册并发布话题，
-# 但还没接 ros_gz_bridge 到 ROS，也还没验证图像内容
+# ✔ 相机出图 D405/D435 已注册并桥接到 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 轴本体缺
# 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。两者都只有
# Gazebo 会暴露，RViz/MoveIt 完全正常。已在启动时归一化解决。
@@ -29,11 +34,21 @@
# Physics/UserCommands/SceneBroadcaster。少了 UserCommands，
# /world/<名>/create 服务不存在 → 模型根本没进 Gazebo，而 create 仍打印
# "OK creation of entity"。所以这三个系统必须在 world 里显式声明。
-# 3. gz 的插件搜索路径不含 $CONDA_PREFIX/lib（ros_gz_sim 只拼 LD_LIBRARY_PATH，
-# 而 conda 激活不往那里加），于是 gz_ros2_control 报 "couldn't find shared
-# library" 而库其实就在那儿。必须在 ros2 launch 之前设
+# 3. gz 的插件搜索路径不含 $CONDA_PREFIX/lib（ros_gz_sim 的 launch 只拼
+# LD_LIBRARY_PATH，而 conda 激活不往那里加），于是 gz_ros2_control 报
+# "couldn't find shared library" 而库其实就在那儿。必须在 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° 视场。
+# 照抄它做三维反投影，点会偏半米量级，而单目预览完全看不出来。
+# 已改为由 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
@@ -47,8 +62,10 @@ version: "Fortress 6.16 (ignition-gazebo6, ros_gz + gz_ros2_control 0.7.15)"
description: >
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
+ D405/D435 已桥接到 ROS，且几何正确性有自动验收（scripts/check_sim_cameras.py）。
场景模型（工件/通止规）与 Orisys 触觉插件尚未实现；
- 相机传感器已在 Gazebo 内注册并出话题，但还没接到 ROS 侧（P5c-4）。
+ sim.world 已真正从 config/site.yaml 读取（physics_enable / joint_prefix 尚未接线，
+ 写了不一致的值会告警而非静默忽略，见文件头 config_schema）。
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
@@ -100,28 +117,44 @@ driver:
# --- 配置项声明 ------------------------------------------------------------
# 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），
# 避免「仿真调好的值搬到真机不生效」。只有物理引擎自身的参数放在 sim.* 下。
+#
+# ⚠️ 每个键都必须如实标注"是否真的接线"（wired）。声明了却没接线的键是最难查的
+# 一类问题：用户按文档改了 site.yaml，行为毫无变化，也不报错。
+# 目前由 arm_sim_gazebo.sim_site 读取的是 world（已接线）；
+# physics_enable / joint_prefix 尚未接线，若其值与世界/描述的实际状态冲突，
+# 启动时会打印告警 —— 见 sim_site.unwired_warnings()。
config_schema:
- key: world
site_path: sim.world
type: str
required: false
default: ""
+ wired: true
doc: >
- 仿真世界文件路径。留空表示使用 Gazebo 默认空场景。
- Gazebo 接线完成后填真实路径，如 devices/sim/gazebo/worlds/assembly.world。
+ 仿真世界文件路径。留空 = 用内置默认（devices/sim/gazebo/worlds/assembly.world）。
+ 相对路径按仓库根解析。优先级：命令行 --world > 本项 > 内置默认。
+ 实现在 arm_sim_gazebo/sim_site.resolve_world()。
- key: physics_enable
site_path: sim.physics_enable
type: bool
required: false
default: true
- doc: 是否启用物理引擎。关闭可用于纯运动学验证。
+ wired: false
+ doc: >
+ 是否启用物理引擎。**尚未接线**：世界文件里的 （重力/步长）是写死的，
+ 本项不会传导过去。若这里写的值与世界实际状态不符，启动时告警而不是静默忽略。
+ 要真正生效，需改 world 文件里的 ，或把本项接进启动流程。
- key: joint_prefix
site_path: sim.joint_prefix
type: str
required: false
default: sim_
+ wired: false
doc: >
仿真关节名前缀。必须与真实臂的前缀（Rizon4-xxxxxx_）不同，
否则仿真与真机的 TF 会互相污染。
+ **尚未接线**：实际前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix 决定，
+ 且 config/controllers.yaml 里硬编码了 sim_joint1..7。写了个不同的值会告警。
+ 要真改，这三处必须一起改。
diff --git a/devices/sim/gazebo/worlds/assembly.world b/devices/sim/gazebo/worlds/assembly.world
index e3c4863..7c445f5 100644
--- a/devices/sim/gazebo/worlds/assembly.world
+++ b/devices/sim/gazebo/worlds/assembly.world
@@ -229,15 +229,36 @@
（早期把它挂在 URDF 的 world link 上，结果转 SDF 时整台相机消失 ——
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 # 有数
- 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 放上工件后要用上面那条命令复核"工件是否完整可见"。
========================================================================= -->
true
- -0.45 0.0 1.35 0.0 0.872665 3.14159
+ -0.45 0.0 1.35 0.0 0.872665 0.0
0.09 0.025 0.025
diff --git a/scripts/README.md b/scripts/README.md
index 81d53b7..6cad567 100644
--- a/scripts/README.md
+++ b/scripts/README.md
@@ -14,6 +14,12 @@
- 体检：`weights.sh doctor --check-engines`（能查出 engine 与 TensorRT 版本不匹配这类静默失效）
- 规则写在 [models/README.md](../models/README.md)。`export_yolo_trt.sh` 现在是它的兼容转发壳。
**配置：** `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`
**RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh` · `rdk_drag_bridge.py`
**搁置未用：** `fetch_isaac_flexiv.sh`（拉 Flexiv 的 Isaac Sim / RDK 源码到 `thirdparty/`；Isaac 路线目前搁置，Gazebo 是选定平台）
diff --git a/scripts/check_sim_cameras.py b/scripts/check_sim_cameras.py
new file mode 100755
index 0000000..1f5b5a7
--- /dev/null
+++ b/scripts/check_sim_cameras.py
@@ -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())
diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/gazebo_sim_launcher.py b/src/arm_sim_gazebo/arm_sim_gazebo/gazebo_sim_launcher.py
index 58b5024..c50cf74 100644
--- a/src/arm_sim_gazebo/arm_sim_gazebo/gazebo_sim_launcher.py
+++ b/src/arm_sim_gazebo/arm_sim_gazebo/gazebo_sim_launcher.py
@@ -33,10 +33,23 @@ from .robot_description import (
repo_root,
world_name,
)
+from .sim_site import (
+ SimSiteError,
+ load_sim_site,
+ physics_enabled_in_world,
+ resolve_world,
+ unwired_warnings,
+)
LAUNCH_PKG = "arm_sim_gazebo"
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]:
# 用 python -m 调 ros2 而不是 exec "ros2"：这样在 ros2 CLI 不在 PATH 的
@@ -95,8 +108,12 @@ def main(argv: Optional[List[str]] = None) -> int:
root = repo_root()
xacro_path = default_xacro(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(
xacro_path=xacro_path,
controllers_file=controllers,
@@ -105,10 +122,19 @@ def main(argv: Optional[List[str]] = None) -> int:
)
wname = world_name(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)
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:
# 这一条能省掉一整个下午：world 漏声明基础系统时，仿真看起来完全正常
# （/clock 在走、create 打印成功），但机械臂从未进入 Gazebo。
@@ -135,6 +161,11 @@ def main(argv: Optional[List[str]] = None) -> int:
print(f" 描述 : {xacro_path}")
print(f" 控制器配置 : {controllers}")
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()}")
if missing:
print(
diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py b/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py
index 42917df..f6d8679 100644
--- a/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py
+++ b/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py
@@ -20,6 +20,7 @@ devices/ **不是 ROS 包**，所以 $(find ...) 找不到它，必须用文件
"""
from __future__ import annotations
+import math
import os
import re
import subprocess
@@ -107,6 +108,183 @@ def world_name(world_file: Path) -> str:
return m.group(1)
+# =============================================================================
+# 从 world 文件里读模型位姿
+# =============================================================================
+# 为什么需要：Gazebo 里的 （尤其是 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]:
+ """取出 ... 的内容（考虑同名标签嵌套）。
+
+ SDF 里 可以嵌套 （组合体）， 里也可能有 （少见）。
+ 只找第一个 在嵌套时会截断到错误的位置，所以这里数嵌套深度。
+ """
+ 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
+
+ # 自闭合： 是完全合法的 SDF（没有子元素）。
+ # 不特判的话会走到下面的"找 "分支、找不到 → 返回 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": 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]:
+ """取一段块自己的 （默认全 0）。
+
+ 只认**第一个** ，且必须出现在 stop_before 里任一标签之前 ——
+ 这样才不会把子元素的 pose 误当成父元素的。例如 的 pose 必须在
+ 第一个 // 之前； 的 pose 必须在第一个
+ // 之前。
+ """
+ cut = len(block)
+ for tag in stop_before:
+ i = block.find("<" + tag)
+ if 0 <= i < cut:
+ cut = i
+ m = re.search(r"]*)?>([^<]*)", 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 文件里 的 在**世界系**的位姿。
+
+ 返回值是 ((x, y, z), (roll, pitch, yaw))。link 留空表示取模型本身的位姿。
+ 两者都会做复合（模型位姿 ∘ link 位姿），因为 SDF 的 是相对父元素的。
+
+ 找不到模型/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} 里找不到 。\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} 的 里找不到 。"
+ )
+
+ 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 里声明了任何一个 ，Gazebo 就不再加载 server.config 的默认
# 系统列表，这三个基础系统必须由 world 自己声明，否则整个仿真静默失效。
# 详细后果见 assembly.world 里那段注释，以及这里的 check_base_plugins()。
diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_camera_adapter.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_camera_adapter.py
new file mode 100644
index 0000000..0e56bce
--- /dev/null
+++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_camera_adapter.py
@@ -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 图里仍然可见，
+排查"是桥接断了还是适配器断了"时能一刀切开。
+
+=============================================================================
+发布的话题（与真机完全一致，应用无需为仿真改任何代码）
+=============================================================================
+ //color/image_raw rgb8
+ //color/camera_info plumb_bob（畸变全 0，gz 不模拟畸变）
+ //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())
diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py
new file mode 100644
index 0000000..d09606e
--- /dev/null
+++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py
@@ -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 里的值。两者一旦不一致，图像与内参就对不上了 ——
+ 症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头
+ 那一长串说明所警告的那类错误，所以要主动比对。
+
+ 返回人类可读的告警列表（空 = 没问题）。
+
+ 精度说明（有意为之，不做过度解析）：
+ - 世界相机：按 里的 精确匹配到对应相机，读到的是精确值。
+ - 腕上相机：它在 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
+ # 找到 等于本相机 gz_ns 的那个 块
+ fov = None
+ for m in re.finditer(r"]*>(.*?)", text, re.S):
+ body = m.group(1)
+ tm = re.search(r"\s*([^<\s]+)\s*", body)
+ if not tm or "/" + tm.group(1).strip().lstrip("/") != s.gz_ns:
+ continue
+ fm = re.search(r"\s*([0-9.eE+-]+)\s*", 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} 的 的 ，无法比对。"
+ 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"\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"，无法比对（内参按 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,
+ )
diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py
new file mode 100644
index 0000000..9cfca18
--- /dev/null
+++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py
@@ -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 文件里的 ，"
+ 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 里"关物理"的常见做法就是把 置零（比删掉整个
+ 更常用，因为 还带着步长等其它必需项）。没有 /
+ 时按"开着"算 —— gz 的默认就是开。
+ """
+ try:
+ text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+ except OSError:
+ return None
+ m = re.search(r"\s*([^<]+)\s*", 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)
diff --git a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py
index e372d01..1819b63 100644
--- a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py
+++ b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py
@@ -76,10 +76,12 @@ def _prepare(context, *args, **kwargs):
default_controllers,
default_world,
default_xacro,
+ model_link_pose,
prepare_urdf,
repo_root,
world_name,
)
+ from arm_sim_gazebo.sim_site import SimSiteError, resolve_world
world = LaunchConfiguration("world").perform(context).strip()
load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower()
@@ -89,7 +91,10 @@ def _prepare(context, *args, **kwargs):
try:
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(
xacro_path=default_xacro(root),
@@ -99,7 +104,7 @@ def _prepare(context, *args, **kwargs):
)
wname = world_name(world_path)
missing_base = check_base_plugins(Path(world_path))
- except SimDescriptionError as exc:
+ except (SimDescriptionError, SimSiteError) as exc:
# 描述准备失败是最常见的上手障碍（环境没 source、路径不对）。
# 直接把可读原因打出来，而不是让它变成一个看不懂的 launch traceback。
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 里的 产生 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 **不报错**，只是实体没被创建，
# 表现成"launch 一切正常但 RViz 里什么都没有"。
actions.append(
@@ -201,7 +322,7 @@ def _prepare(context, *args, **kwargs):
)
)
- # --- 5) 控制器 -----------------------------------------------------------
+ # --- 6) 控制器 -----------------------------------------------------------
# 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
# rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
controllers = [
@@ -236,8 +357,18 @@ def _prepare(context, *args, **kwargs):
" ros2 control list_controllers # 两个控制器 active\n"
" ros2 topic echo /joint_states --once # 7 个 sim_joint\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，启动时会自动比对告警。"
)
)
)
diff --git a/src/arm_sim_gazebo/setup.py b/src/arm_sim_gazebo/setup.py
index c321dc3..ffb456e 100644
--- a/src/arm_sim_gazebo/setup.py
+++ b/src/arm_sim_gazebo/setup.py
@@ -34,13 +34,17 @@ setup(
license='Apache-2.0',
entry_points={
'console_scripts': [
- # devices/sim/gazebo/manifest.yaml 里声明的那个可执行名，别改。
- # 它只做"准备描述 + 转发给 launch 文件"，逻辑在
- # arm_sim_gazebo/gazebo_sim_launcher.py。
- 'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
- # 独立小工具：只看归一化做了什么，不启动 Gazebo。
- # 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
- 'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
+ # devices/sim/gazebo/manifest.yaml 里声明的那个可执行名，别改。
+ # 它只做"准备描述 + 转发给 launch 文件"，逻辑在
+ # arm_sim_gazebo/gazebo_sim_launcher.py。
+ '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 说我的 link 不存在"时非常省时间。
+ 'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
],
},
)
diff --git a/src/arm_sim_gazebo/test/test_robot_description.py b/src/arm_sim_gazebo/test/test_robot_description.py
index 5ce0c19..64e8b8b 100644
--- a/src/arm_sim_gazebo/test/test_robot_description.py
+++ b/src/arm_sim_gazebo/test/test_robot_description.py
@@ -21,6 +21,7 @@ from arm_sim_gazebo.robot_description import ( # noqa: E402
check_base_plugins,
default_world,
expand_xacro,
+ model_link_pose,
prepare_urdf,
repo_root,
world_name,
@@ -262,3 +263,218 @@ def test_prepare_urdf_missing_controllers_is_actionable(tmp_path):
with pytest.raises(SimDescriptionError) as ei:
prepare_urdf(controllers_file=tmp_path / "nope.yaml")
assert "控制器配置不存在" in str(ei.value)
+
+
+# ---------------------------------------------------------------------------
+# 从 world 文件解析模型位姿（世界相机的 TF 靠它）
+# ---------------------------------------------------------------------------
+# 这组测试存在的理由：Gazebo 不为 world 里的 产生 ROS TF，
+# 而 D435 这类固定相机就属于这种情况。位姿靠解析而不是重复配置，
+# 因此"解析对不对"直接决定了相机看到的东西和它自称的位置是否一致。
+# 解析错了不会报错，只会让三维点整体偏移或转错。
+
+
+def _world_file(tmp_path, body: str) -> Path:
+ p = tmp_path / "w.world"
+ p.write_text(f'\n\n{body}\n\n', encoding="utf-8")
+ return p
+
+
+def test_model_pose_identity_link(tmp_path):
+ """link 没有自己的 pose 时，位姿就是模型位姿。"""
+ w = _world_file(
+ tmp_path,
+ """
+
+
+ true
+ -0.45 0.0 1.35 0.0 0.872665 3.14159
+
+
+ 0.1 0 0 0 0 0
+
+
+
+
+ """,
+ )
+ (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 的 是相对父元素的，模型位姿与 link 位姿必须复合。"""
+ w = _world_file(
+ tmp_path,
+ """
+
+
+ 1 0 0 0 0 0
+
+ 0 1 0 0 0 0
+
+
+
+
+ """,
+ )
+ 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,
+ """
+
+
+ 0 0 0 0 0 1.5707963
+
+ 1 0 0 0 0 0
+
+
+
+
+ """,
+ )
+ 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 必须在第一个 // 之前取，
+ 否则会把子元素的 pose 误当成 link 的。"""
+ w = _world_file(
+ tmp_path,
+ """
+
+
+ 2 2 2 0 0 0
+
+ 1 1 1 0 0 0
+
+ 9 9 9 0 0 0
+
+
+
+
+ """,
+ )
+ 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,
+ """
+
+
+
+
+
+ """,
+ )
+ 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,
+ """
+
+ 5 6 7 0 0 0
+
+ """,
+ )
+ 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,
+ """
+
+ 1 2 3 0 0 0
+
+ """,
+ )
+ 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):
+ """ 可以嵌套 （组合体）。找第一个 会截断错位，
+ 所以 _find_block 要数嵌套深度。"""
+ w = _world_file(
+ tmp_path,
+ """
+
+
+ 0 0 0 0 0 0
+
+ 9 9 9 0 0 0
+
+
+ 1 0 0 0 0 0
+
+
+ """,
+ )
+ 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, '')
+ 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, '')
+ 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)
diff --git a/src/arm_sim_gazebo/test/test_sim_cameras.py b/src/arm_sim_gazebo/test/test_sim_cameras.py
new file mode 100644
index 0000000..c3e3887
--- /dev/null
+++ b/src/arm_sim_gazebo/test/test_sim_cameras.py
@@ -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):
+ """世界相机：按 精确匹配，能抓到 cameras.yaml 与 world 不一致。"""
+ from arm_sim_gazebo.sim_cameras import check_fov_consistency
+
+ world = tmp_path / "assembly.world"
+ world.write_text(
+ """
+
+
+ /d435
+ 1.5184
+
+
+""",
+ 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("1.5184", encoding="utf-8")
+ spec = _spec()
+ assert check_fov_consistency([spec], sensors_xacro=xacro) == []
+
+ xacro.write_text("1.2", 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 里找不到对应 时要告警，而不是静默放过。"""
+ from arm_sim_gazebo.sim_cameras import check_fov_consistency
+
+ world = tmp_path / "assembly.world"
+ world.write_text("", 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)
diff --git a/src/arm_sim_gazebo/test/test_sim_site.py b/src/arm_sim_gazebo/test/test_sim_site.py
new file mode 100644
index 0000000..6cf5b37
--- /dev/null
+++ b/src/arm_sim_gazebo/test/test_sim_site.py
@@ -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("0 0 -9.8")
+ assert physics_enabled_in_world(on) is True
+
+ off = tmp_path / "off.world"
+ off.write_text("0 0 0")
+ assert physics_enabled_in_world(off) is False
+
+ # 没写 gravity → gz 默认是开
+ none = tmp_path / "none.world"
+ none.write_text("")
+ 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}"