P5c-4 相机出图联调：桥接 d405/d435 到 ROS，并修掉两处静默的几何错误

相机链路（devices/sim/gazebo/config/cameras.yaml 是唯一真相）
  - sim_cameras.py        纯逻辑：话题映射 / 深度换算 / 内参 / 视场一致性比对
  - sim_camera_adapter.py 改光学系 frame_id、32FC1 米 → 16UC1 毫米、覆盖内参
  - gazebo_sim.launch.py  桥接 + 适配器 + 世界相机的静态 TF（位姿从 world 解析）

本轮修掉两个**静默**的几何错误（都能出图、都能查到 TF，但点位是错的）

  1. gz 发的 camera_info 是错的：恒为 fx=fy=277, cx=160, cy=120
     （等于它的默认 60° 视场 + 半分辨率主点），而渲染用的是配置的 87° 视场。
     照抄它做三维反投影，点会偏半米量级；单目预览完全正常。
     用已知地面平面做最小二乘反解，得 fy=337.2 / cy=239.5，与
     640/(2·tan(87°/2))=337.2、480/2=240 吻合（残差中位 0.2mm）。
     改为由 sim_cameras.intrinsics() 按 cameras.yaml 的视场重算并覆盖。
     代价：cameras.yaml 的 horizontal_fov 必须与 SDF 一致，启动时自动比对告警。

  2. assembly.world 里 D435 的 rpy 写成 (0, 50°, 180°)，
     而 URDF 相机的视线是**本体 X 轴**、rpy 复合顺序又是 Rz·Ry·Rx，
     于是"先俯 50° 再偏航 180°"把视线翻到了背面：相机背对台面、只看地面。
     它不报错，而且几何自洽（点确实落在真实地面上），只查数值发现不了。
     改为 (0, 50°, 0)，并加回归测试锁住。

新增验收工具 + 测试
  - scripts/check_sim_cameras.py  反投影深度、要求命中地面/台面才算通过。
    "有图像"不是验收标准；实测 d435 台面 11.9%/地面 75.3%、d405 台面 37.8%。
  - 97 项单测（新增 sim_cameras 22 项、sim_site 16 项）

顺带修掉一处"改了配置静默无效"
  - sim.world 此前硬编码在 robot_description._WORLD_REL，而 site.yaml 与
    manifest.yaml 都宣称改它就生效。现在真的读了：
    优先级 --world > site.yaml:sim.world > 内置默认（相对路径按仓库根解析）。
  - physics_enable / joint_prefix 仍未接线，但若其值与实际状态冲突，
    启动时会告警（sim_site.unwired_warnings），不再静默忽略。

文档
  - devices/sim/gazebo/README.md 重写：原文还写着"仿真不是 Gazebo"、
    相机插件用 libgazebo_ros_camera.so，与实现早已不符。现分三部分：
    哪个"仿真"是真的 / 现状与缺什么 / 五条静默失效的坑及怎么验。

Co-authored-by: Cursor <cursoragent@cursor.com>

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 reference="${prefix}link1">
-  <material>Gazebo/Grey</material>
-</gazebo>
-```
+---
 
-并在 `arm.ros2_control.xacro` 里把 `ros2_control` 标签的 hardware 插件从
-`flexiv_hardware/FlexivHardware` 换成 `gazebo_ros2_control/GazeboSystem`。
+## 第一部分：现状（截至 P5c-4，2026-09-23）
 
-### 2. 关节传动与控制器
+### 已就绪
 
-Gazebo 需要 `transmission` 标签把关节映射到 `ros2_control`。控制器配置可以直接复用
-`arm_control/config/controllers.yaml` 里的 `rizon_arm_controller`，只需把关节名前缀
-从真实 SN（如 `Rizon4-063989_`）换成仿真专用的前缀（如 `sim_`）。
+| 部分 | 位置 | 说明 |
+|------|------|------|
+| 运行环境 | `scripts/install_sim_env.sh` / `sim_env.sh` | 独立 conda 环境 `ros_gz_sim`（Gazebo Fortress 6.16 + ROS 2 Humble），与真机环境 `ros_flexiv` 隔离 |
+| 世界 | `worlds/assembly.world` | 地面 + 工作台 + D435 + 通止规标记位 |
+| 机器人描述 | `urdf/*.xacro` | 复用官方 `flexiv_description`，启动时**归一化**后喂 Gazebo |
+| 控制器 | `config/controllers.yaml` | 控制器名与真机一致（`rizon_arm_controller` + `joint_state_broadcaster`） |
+| 启动器 | `src/arm_sim_gazebo` 的 `gazebo_sim_launcher` | 起 Gazebo → 发归一化 URDF → spawn → 拉起控制器 |
+| 相机 | `config/cameras.yaml` | 两台相机的**唯一真相**：话题、坐标系、视场 |
 
-### 3. 传感器插件
+### 还没做
 
-| 仿真对象 | 插件 | 对应真实设备 |
-|----------|------|--------------|
-| 腕部相机 | `libgazebo_ros_camera.so` | D405 |
-| 手外相机 | `libgazebo_ros_camera.so` | D435 |
-| 深度 | `libgazebo_ros_depth_camera.so` | 对齐深度流 |
-| 触觉力 | **需要自己写插件** | Orisys |
+| 部分 | 缺什么 | 影响 |
+|------|--------|------|
+| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 |
+| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） |
 
-**最关键的一条：Orisys 视触觉没有现成的 Gazebo 插件。**
-3 路切向力 + 1 路法向力 + 深度图 + 力矢量图 这套数据需要自定义 Gazebo 插件来模拟。
-在它做出来之前，仿真里**无法验证通止规装配的力控部分**（S07 接触、S08 拧紧判定）。
-建议的自定义插件做法：订阅 Gazebo 的接触传感器数据，按接触深度和相对滑动速度合成
-出一组「类触觉」读数，再按真实传感器的量纲（任意单位增量）发布到同样的内部话题上。
-
-### 4. 工件与通止规模型
-
-需要给工件（含 k0 中心孔、k1/k2 螺纹孔）和通止规建简化网格，并保证：
-
-- 工件的 k0/k1/k2 三个点在模型坐标系里的位置与实际一致 —— 否则推理出的坐标映射到仿真里会对不上；
-- 螺纹孔要能表达「拧入」的几何阻力，否则 S08 的力反馈无法触发。
+> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 +
+> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据，
+> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到
+> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。
+> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。
 
 ---
 
 ## 第二部分：日常怎么用
 
+### 启动
+
+```bash
+bash scripts/run_sim_gz.sh              # 无头（无显示器环境用这个）
+bash scripts/run_sim_gz.sh --gui        # 带 Gazebo GUI
+bash scripts/run_sim_gz.sh --check      # 只自检描述，不启动任何东西
+bash scripts/run_sim_gz.sh --no-gripper --no-sensors   # 最省资源，只验运动学
+```
+
+首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。
+
+### 验收（★ 必做，别只看"有没有图像"）
+
+```bash
+ros2 topic echo /clock --once          # 仿真时间在走
+ros2 control list_controllers          # 两个控制器 active
+ros2 topic echo /joint_states --once   # 7 个 sim_joint
+python3 scripts/check_sim_cameras.py   # ★ 相机的真正验收
+```
+
+**`check_sim_cameras.py` 才是相机的验收标准。** 它把深度反投影到 world，
+检查落点是否命中已知平面（地面 z=0、台面 z=0.40）。"有图像"证明不了什么 ——
+详见下面的坑 4 与坑 5。
+
 ### 放置格式与命名要求
 
 | 类型 | 放哪 | 格式 | 命名要求 |
 |------|------|------|----------|
-| 世界文件 | `worlds/` | `.world` 或 `.sdf` | `<场景名>.world`，如 `assembly.world` |
-| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 必须以 `sim_` 为关节前缀，与真实臂区分 |
+| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` |
+| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） |
 | 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` |
 | 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 |
-| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | 单位必须是米。用毫米建模的必须先缩放到米 |
+| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米**。用毫米建模的必须先缩放到米 |
+| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用启动时的 `camera_name` |
 
-### 导入一个新场景
+### 改配置：改哪里才会真正生效
 
-1. 把 `.world` 放进 `worlds/`，网格放进 `models/<模型名>/`
-2. 改 `config/site.yaml` 的 `sim.world` 指向该文件
-3. 改 `devices/registry.yaml` 的 `sim: gazebo`（通常已是）
-4. 启动 `scripts/run_sim.sh`
+| 想改什么 | 改哪里 | 真的生效吗 |
+|----------|--------|------------|
+| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 已接线（优先级：`--world` > `site.yaml` > 内置默认） |
+| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致会告警 |
+| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 里的 `<gravity>` |
+| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。前缀由 `urdf/arm.sim.urdf.xacro` 的 `arm_prefix` 决定，且 `config/controllers.yaml` 硬编码了 `sim_joint1..7` —— 三处要一起改 |
+| 仿真里的悬高/速度 | **复用真机那套参数**（`servo.*` / `hole_detect.stage2.*`） | ✔ |
 
-### 修改哪个配置才会生效
+> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何"仿真专用参数"都是
+> 未来"仿真调好的值搬到真机不生效"的隐患。只允许物理引擎自身的参数（重力、步长）
+> 放在 `sim.*` 下。
+>
+> ✘ 标记的两个键由 `arm_sim_gazebo.sim_site.unwired_warnings()` 守着：只要你写的值
+> 和实际状态不一致，启动时就会告警。**不要让它们重回静默。**
 
-| 想改什么 | 改哪里 |
-|----------|--------|
-| 用哪个场景 | `site.yaml` → `sim.world` |
-| 仿真是否启用物理 | `site.yaml` → `sim.physics_enable` |
-| 仿真里的悬高/速度 | **复用真实的那套参数**（`servo.*` / `hole_detect.stage2.*`），不要另建一套，否则仿真调好的值搬到真机不生效 |
-| 关节前缀 | `site.yaml` → `sim.joint_prefix`（默认 `sim_`） |
+---
 
-> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何「仿真专用参数」都是未来
-> 仿真调参搬到真机失效的隐患。只允许物理引擎本身的参数（如重力、步长）放在 `sim.*` 下。
+## 第三部分：实测踩到的坑（都属于"静默失效"）
+
+这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，
+但结果是错的。所以每条都配了"怎么发现"。
+
+| # | 坑 | 症状 | 怎么发现 / 怎么修 |
+|---|----|------|-------------------|
+| 1 | 官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉：7 轴本体缺 `<inertial>` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN | 只有 Gazebo 拒绝，RViz/MoveIt 完全正常 | 启动时**归一化**（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check` |
+| 2 | world 里只要声明了**任何一个** `<plugin>`，就会挤掉默认的 Physics / UserCommands / SceneBroadcaster | 少了 UserCommands → `/world/<名>/create` 服务不存在 → 模型根本没进 Gazebo，而 `create` 仍打印 `OK creation of entity`；控制器永远起不来 | 启动时会检查并告警。修法：在 world 里显式声明这三个系统 |
+| 3 | gz 的插件搜索路径不含 `$CONDA_PREFIX/lib`（`ros_gz_sim` 的 launch 只拼 `LD_LIBRARY_PATH`，而 conda 激活不往那里加） | `gz_ros2_control` 报 `couldn't find shared library`，而库其实就在那儿（`ldd` 全齐） | 必须在 `ros2 launch` **之前**设 `GZ_SIM_SYSTEM_PLUGIN_PATH`；资源路径同理（否则 mesh 全加载不出来）。都由 `scripts/sim_env.sh` 负责 |
+| 4 | **gz 发的 `camera_info` 是错的**：恒为 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分辨率主点），而**渲染**用的是配置的 87° 视场 | 照抄它做三维反投影，点会偏**半米量级**；单目预览完全正常 | 用已知地面平面做最小二乘反解内参，得到 `fy=337.2 / cy=239.5`，与 `640/(2·tan(87°/2))=337.2`、`480/2=240` 吻合（残差中位 0.2mm）。已改为由 `sim_cameras.intrinsics()` 按 `cameras.yaml` 的视场**重算并覆盖** |
+| 5 | **相机位姿的 `rpy` 顺序**：URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；而 `rpy` 复合顺序是 `Rz·Ry·Rx`，"先俯 50° 再偏航 180°"会把视线翻到**背面** | D435 背对台面、只看得见地面。什么都不报，**而且几何自洽**（点确实落在真实地面上），只查数值发现不了 | 用 `scripts/check_sim_cameras.py` 检查"该看见的平面有没有看见"。已修正 + 加回归测试 |
+
+关于坑 4：代价是 `cameras.yaml` 的 `horizontal_fov` 必须与 SDF 真正渲染用的值一致。
+启动时会自动比对并告警（`sim_cameras.check_fov_consistency`）。
+
+关于相机还有两条**规格差异**，是有意保留的（不是 bug，但会让人困惑）：
+
+- **畸变**：gz 不模拟镜头畸变（`d` 全 0，`R` 单位阵）。所以"仿真里用畸变矫正
+  调出来的参数"搬到真机仍需重调。
+- **分辨率**：真机 D405 是 848x480、D435 是 1280x720，仿真建模用 640x480。
+  对"算法能不能跑通"没影响（内参从 `cameras.yaml` 自洽推导），
+  但"仿真里调好的像素阈值搬到真机"会因像素尺度不同而需要重调。
+
+---
+
+## 相关文件一览
+
+```
+devices/sim/gazebo/
+├── manifest.yaml            # 设备清单：进度、配置声明、放置要求
+├── README.md                # 本文档
+├── config/
+│   ├── cameras.yaml         # ★ 两台相机的唯一真相（话题/坐标系/视场）
+│   └── controllers.yaml     # 控制器配置（名字与真机一致）
+├── urdf/                    # 仿真用机器人描述（含 ros2_control 与传感器）
+├── worlds/assembly.world    # 场景
+├── models/                  # ✘ 待做：工件/通止规/夹具
+└── sensor_plugins/          # ✘ 待做：Orisys 触觉插件
+```
+
+配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器）。
+配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/check_sim_cameras.py`（相机验收）。

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 里已经有 <sensor>）。
+#
+# 与 site.yaml 的分工：
+#     这里只描述"Gazebo 里有什么、要桥到 ROS 的哪个名字"。
+#     site.yaml 描述"真机上有哪些相机、序列号是多少"。仿真不读序列号 ——
+#     Gazebo 里没有 USB，序列号无从绑定。
+#     两者的**话题命名约定必须一致**，这是应用层能不改代码跑仿真的前提。
+#
+# 话题命名约定（与真机 realsense 完全对齐，见 arm_application/handeye_realsense.py
+# 的 REALSENSE_TOPIC_DEFAULTS 与 realsense_color_pub.py）：
+#     /<ros_ns>/color/image_raw                    rgb8
+#     /<ros_ns>/color/camera_info                  plumb_bob
+#     /<ros_ns>/aligned_depth_to_color/image_raw   16UC1，单位毫米，0 = 无读数
+#
+# ⚠️ 单位与编码是关键差异，不能只做话题改名就完事：
+#     Gazebo 的深度相机发的是 **32FC1、单位米**（本机实测 step=2560 即 4 字节/像素），
+#     而真机 realsense 的 aligned_depth 是 **16UC1、单位毫米**。
+#     直接把 32FC1 换个名字发出去，应用侧按毫米解释会整体差 1000 倍，
+#     且 numpy 的 frombuffer 会因 dtype 不符报错或读出垃圾。
+#     所以深度必须**真正转换**，由 sim_camera_adapter 负责。
+#
+# ⚠️⚠️ 更要紧的一条：**gz 发布的 camera_info 是错的，不能直接用。**
+#     本机实测（Fortress 6.16 / RoboStack）：无论怎么配，它都给
+#         fx=fy=277, cx=160, cy=120
+#     这组值对应的是 **gz 的默认值** —— 60° 视场 + 半分辨率图幅(320x240)，
+#     而实际渲染用的是配置里的 87° 视场与 640x480。
+#     用已知地面平面反解真实内参得到 fy=337.2、cy=239.5，
+#     与 640/2/tan(1.5184/2)=337.2、480/2=240 完全吻合（残差中位 0.2mm）。
+#     照抄 gz 的 camera_info 去做三维反投影，点位会错 0.5 米量级 ——
+#     而单目预览里**完全看不出来**，只会让"仿真里调好的参数搬到真机"整个失效。
+#     因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。
+#
+# 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。
+# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_fov_consistency）：
+#     腕上相机 → devices/sim/gazebo/urdf/arm.sim.sensors.xacro 的 horizontal_fov
+#     世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 <sensor> 的 horizontal_fov
+# =============================================================================
+
+cameras:
+  # --- 腕上相机（eye-in-hand），挂在机械臂法兰上 ------------------------------
+  - gz_ns: /d405          # Gazebo 传感器话题前缀（see arm.sim.sensors.xacro 的 <topic>）
+    ros_ns: d405          # ROS 侧命名空间。应用按 camera_name 参数用这个名字取话题，
+                          # 所以它必须等于启动应用时给的 camera_name（默认 d405）
+    link_frame: sim_d405_link
+    # 图像上盖的坐标系必须是**光学系**，不能是本体系。
+    # gz 与 URDF 里的相机本体遵循"X 朝前 / Y 朝左 / Z 朝上"（相机机身约定），
+    # 而 ROS 的 image / camera_info 必须用光学约定"Z 朝前 / X 朝右 / Y 朝下"
+    # （REP-103/105）。两者差一个 rpy=(-90°, 0, -90°)。
+    # 若直接把 frame_id 盖成本体系，应用做 tf2 查询能查到、不报错，但
+    # 深度反投影出的三维点会整体转错 —— 这类错误在单目预览里完全看不出来。
+    optical_frame: sim_d405_color_optical_frame
+    depth: true
+    # 水平视场（弧度）。真机 D405 彩色的水平视场约 87° = 1.5184 rad，
+    # 仿真的传感器也按这个建，这样两边的像素尺度接近。
+    # 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。
+    horizontal_fov: 1.5184
+    width: 640
+    height: 480
+    note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供
+
+  # --- 手外相机（eye-to-hand），固定在世界里看工作台 -------------------------
+  # ⚠️ 这台相机在 world 文件里，不在 URDF 里 —— 因为它的正确物理归属就是"世界"，
+  #    而不是机器人本体。（曾经把它挂在 URDF 的 world link 上，结果 sdformat 把
+  #    无质量的根 link 连同整棵子树静默丢掉，相机与它的话题一起消失。详见
+  #    devices/sim/gazebo/README.md。）
+  #    代价是：gz 不会为世界模型发布 TF，所以必须由启动方补一条静态 TF
+  #    world → d435_link。位姿从 world 文件里**解析**得到（不在这里重复配置），
+  #    避免"改了一处忘了另一处"。
+  - gz_ns: /d435
+    ros_ns: d435
+    link_frame: d435_link
+    optical_frame: d435_color_optical_frame
+    depth: true
+    horizontal_fov: 1.5184
+    width: 640
+    height: 480
+    # 非空 = 这台相机挂在世界模型上，需要补静态 TF。
+    # world_model 是 world 文件里的 <model name>，world_link 是模型里的 <link name>。
+    world_model: d435
+    world_link: link
+    note: height 也与真机不同（真机 1280x720），同 D405 的说明

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 轴本体缺
 #      <inertial> 包裹；夹爪有零质量 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: >
+      是否启用物理引擎。**尚未接线**：世界文件里的 <physics>（重力/步长）是写死的，
+      本项不会传导过去。若这里写的值与世界实际状态不符，启动时告警而不是静默忽略。
+      要真正生效，需改 world 文件里的 <gravity>，或把本项接进启动流程。
 
   - 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。写了个不同的值会告警。
+      要真改，这三处必须一起改。

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 放上工件后要用上面那条命令复核"工件是否完整可见"。
          ========================================================================= -->
     <model name="d435">
       <static>true</static>
-      <pose>-0.45 0.0 1.35 0.0 0.872665 3.14159</pose>
+      <pose>-0.45 0.0 1.35 0.0 0.872665 0.0</pose>
       <link name="link">
         <visual name="body">
           <geometry><box><size>0.09 0.025 0.025</size></box></geometry>