P5c-6 场景物料落地 + Gazebo 中文手册：工件/通止规模型、场景校验、两处静默失效修复

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

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

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

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

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

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

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

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

devices/sim/gazebo/config/cameras.yaml

@@ -36,7 +36,7 @@
 #     因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。
 #
 # 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。
-# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_fov_consistency）：
+# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_camera_consistency）：
 #     腕上相机 → devices/sim/gazebo/urdf/arm.sim.sensors.xacro 的 horizontal_fov
 #     世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 <sensor> 的 horizontal_fov
 # =============================================================================
@@ -59,7 +59,13 @@ cameras:
     # 仿真的传感器也按这个建，这样两边的像素尺度接近。
     # 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。
     horizontal_fov: 1.5184
-    width: 640
+    # 图像尺寸必须与真机一致：真机 D405 跑 848x480@30（site.yaml 的 camera.width/height）。
+    # 这**不是**显示偏好 —— 阶段二的孔检测在原始分辨率图上跑 HoughCircles，
+    # hole_detect.yaml 的像素门槛（hough_min_radius / hole_distance_min）只在这个
+    # 尺度下才对。曾经用 640x480（fx 337 而非真机 447），58mm 的两孔间距
+    # 成像只剩 85px，低于 hole_distance_min=90 → 孔检测在仿真里根本不可能通过。
+    # 启动时会比对这里与 xacro 宏默认值，不一致会告警（sim_cameras）。
+    width: 848
     height: 480
     note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供
 

devices/sim/gazebo/config/scene.yaml

@@ -0,0 +1,146 @@
+# =============================================================================
+# devices/sim/gazebo/config/scene.yaml
+#
+# 仿真场景：**谁摆在台面上的哪里**。
+#
+# 为什么单独一份文件，而不是把位姿写进模型或 world：
+#     模型描述"这个东西长什么样"，场景描述"它现在在哪"。
+#     分开之后，同一个模型可以在场景里出现多次、可以在不改模型的情况下换位置，
+#     也才能在真机现场对照着调整（"现场工件是摆在偏左还是偏右？"）。
+#     world 文件里只放**固定不动**的东西（地面、台子、眼在手外的相机）。
+#
+# ⚠️ 位姿是**绝对**坐标（世界系、米），不是相对台面的偏移。
+#    仿真世界没有"台面坐标系"这种东西，写相对量只会多一层换算、多一个出错点。
+#    下面的 z 值都是"物体的原点"。本框架约定：模型的局部原点 = 它**底面的中心**
+#    （工件的板底、通止规的规尖），所以"放在台面上"就是 z = 台面高度。
+#    这条约定让场景文件读起来最直观，代价是建模时必须把原点设在底部 ——
+#    用 SolidWorks/Blender 导出网格前先把原点挪到底面中心。
+# =============================================================================
+
+# 场景模型所在目录（相对仓库根）。每个子目录一个模型：<名字>/model.sdf + model.config
+models_dir: devices/sim/gazebo/models
+
+# ---------------------------------------------------------------------------
+# 台面几何 —— 用于校验"模型确实摆在台面上、没悬空也没穿模"
+# ---------------------------------------------------------------------------
+# 这些值必须与 worlds/assembly.world 里的 work_table 一致。
+# 不一致时脚本会报错（scripts/check_sim_scene.py），而不是让你在现场猜。
+# 台面：pose(0.55,0,0) + link top 在 z=0.375 + box 厚 0.05 → 顶面 0.40
+#       box 尺寸 0.8(x) × 1.2(y)
+table_top_z: 0.40
+table_bounds:
+  x: [0.15, 0.95]
+  y: [-0.60, 0.60]
+
+# ---------------------------------------------------------------------------
+# 作业条件 —— 校验"仿真里的成像几何与真机一致"
+# ---------------------------------------------------------------------------
+# 二阶段的孔检测是**像素门槛**（hough_min_radius / hole_distance_min …），
+# 它们只在"相机到工件表面距离"合适时才有意义。所以这个距离必须在仿真里
+# 显式复现，不能靠"看起来差不多"。
+#
+# ⚠️ 真机与仿真的**悬停高度不一样**，别照抄：
+#     真机 site.yaml：robots[].servo.hole_hover_z_m = 0.23（法兰相对基座）
+#     仿真需要：约 0.56（法兰相对世界，算下来见下）
+#   原因是两者的"基座与台面的相对关系"不同：真机是机器人装在台面同一高度上
+#   （法兰比台面高 23cm 就等于离工件 23cm），而仿真里机械臂站在地面、
+#   台面在 0.40m 高。**相机到工件的距离**才是决定成像的东西，绝对高度不是。
+#   这个差异目前是**已知且未解决**的（要让两套参数完全通用，得把仿真的台面
+#   降到与真机一致，那会连带改 D435 的位姿和相机验收基线）——
+#   详见 README 的"仿真 ≠ 真机"一节，那里写了决策点与影响面。
+operating:
+  # 用哪台相机做孔检测（决定用哪组内参算像素尺度）
+  camera: d405
+  # 哪个物体是"工件" —— 相机到工件的距离以**它的上表面**为基准。
+  # 这里写的是物体的 name（不是模型名）。
+  part_object: workpiece
+  # 相机光心到工件**上表面**的距离 [m]。
+  # 这个值不是量出来的，是**两条约束夹出来的**：
+  #
+  #   (a) 像素门槛（来自 hole_detect.yaml，见下面 gates_px）
+  #         亮环 φ14（半径 7mm）要落在 10~30px      → 距离 ∈ [0.104, 0.313]
+  #         两孔间距 58mm 要 ≥ 90px                 → 距离 ≤ 0.288
+  #       取中段 ⇒ 约 0.17~0.25 都行。
+  #
+  #   (b) 机械臂够得着（site.yaml 的 servo.workspace_xyz_max 是 z=0.65）
+  #         仿真里法兰高度 = 工件上表面(0.412) + 本值 + 相机伸出量(0.05)
+  #         本值 0.17 → 法兰 0.632，离上界还有 18mm 余量。
+  #         本值 0.22 → 法兰 0.682，**超出工作空间**，二阶段会直接拒绝执行。
+  #
+  # 两条约束交出来的取值，恰好也解释了真机的数：
+  #   真机 hole_hover_z_m=0.23（法兰相对基座）、相机比法兰靠下 0.05、
+  #   台面与基座同一水平面、板上表面 0.012 ⇒ 相机到工件 = 0.23-0.05-0.012
+  #   = 0.168 ≈ 0.17。所以 0.17 不是凑出来的，而是与真机的悬停高度自洽的。
+  camera_to_part_m: 0.17
+  # 相机相对法兰沿工具轴的伸出量 [m]。arm.sim.sensors.xacro 里 d405_z=0.05。
+  # 竖直朝下装配时工具轴朝下，所以相机比法兰更靠近工件这么多。
+  camera_offset_below_flange_m: 0.05
+
+# ---------------------------------------------------------------------------
+# 孔检测的像素门槛 —— 用于在**启动时**就算出"仿真里这些特征能否被检测到"
+# ---------------------------------------------------------------------------
+# ⚠️ 这里的值必须与真机的检测配置一致：
+#       src/arm_application/config/hole_detect.yaml
+#       config/site.yaml 里 hole_detect.* 对它的覆盖（site.yaml 优先）
+#    抄一份的原因：那些门槛是"像素"量，只有在仿真侧算出像素值才能提前发现
+#    "孔间距不够/环太小"，不必等上真机。代价是两份数字可能漂移 ——
+#    所以 scripts/check_sim_scene.py 会读那两个文件做**交叉核对**，
+#    不一致直接报错。改门槛时改真机那份，然后同步到这里（脚本会提醒）。
+#
+# 键名必须与模型里 visual 的名字约定对应（见 sim_scene.features_from_workpiece）：
+#     insert_ring_radius  嵌件亮环半径（模型里 insert_*_ring 的 radius）
+#     hole_spacing        两孔中心距（两个 insert_*_ring 的 pose 距离）
+#
+# 半径区间取 [6, 30]：这是 config/site.yaml **实际生效**的值
+# （site.yaml 覆盖了包内 hole_detect.yaml 的 10/30；生效规则是 site 优先，
+#  见 hole_detector.config_from_site）。脚本会读那两个文件做交叉核对，
+# 不一致直接失败 —— 所以这里永远等于真机在用的门槛，不需要人工同步。
+gates_px:
+  insert_ring_radius: [6, 30]
+  hole_spacing: [90, 700]
+
+# ---------------------------------------------------------------------------
+# 场景物体
+# ---------------------------------------------------------------------------
+# name   = 进 Gazebo 后的实体名（也是 `ign model -m <name>` 用的名字）。
+# model  = models_dir 下的子目录名 / model.config 里的 <name>。
+# yaw    = 绕世界 Z 的旋转 [rad]。工件的 yaw 决定两个孔的连线方向，
+#          而两孔连线方向会直接影响 stage2 的 k1/k2 判定（k1_label: left/right）
+#          —— 换 yaw 后要重新确认哪边是"画面里的左孔"。
+objects:
+  - name: workpiece
+    model: workpiece
+    # 摆在工作台长度方向的中间偏左（靠近机械臂一侧）：
+    #   太靠外（x 大）→ 手臂要伸直，接近工作空间边缘，柔顺下压时余量小
+    #   太靠里（x 小）→ 会被手臂本体挡住 D435 的视线（粗定位就瞎了）
+    # x=0.52 是两者折中；校验脚本会把工件四角反投影到 D435 画面里确认没被裁掉。
+    pose: {x: 0.52, y: 0.00, z: 0.40, yaw: 0.0}
+    note: >
+      工件。z=0.40 = 台面顶面，板底贴台面。
+      工件**上表面**在 0.40+0.012=0.412，二阶段的对孔基准面就是它。
+
+  - name: gauge_stand
+    model: gauge_stand
+    # 与规**同心**：规座就在规的下面，规插在它的方孔里。
+    # 座高 32mm，所以规的 z = 台面 + 32 - 30（通端长 30）= 0.402，
+    # 也就是"台肩正好坐在座顶面上、规尖离台面 2mm"。
+    # 这个 30mm 不是随手写的：它来自 gauge/model.sdf 里通端的长度，
+    # 改模型时必须一起改（改错了的表现是规穿进台面或悬在座里没坐实）。
+    pose: {x: 0.30, y: 0.28, z: 0.40, yaw: 0.0}
+    note: >
+      规座（工装，静态）。**必须在规之前 spawn**（objects 里的顺序就是 spawn 顺序）：
+      否则规先落到台面上、规座再出现在它身体里 —— 两个刚体互相嵌入，
+      求解器会把规弹飞，而现象看起来像"规自己会跳"。
+
+  - name: gauge
+    model: gauge
+    # 立在规座里、工件旁边偏机械臂一侧，方便夹爪从上往下夹住夹持段。
+    # y=0.28 是避开工件与 D435 视线的一侧（工件在 y=0 附近）。
+    # z 见 gauge_stand 的说明：0.402 = 座顶(0.432) - 通端长(0.030)。
+    pose: {x: 0.30, y: 0.28, z: 0.402, yaw: 0.0}
+    note: >
+      通止规。规尖在 z=0.402（原点在规尖），夹持段在 0.435~0.480。
+      本阶段它只是"插在规座里立着"；把它夹起来是 P6 的活（P6 会用
+      GripperExecute 指令夹爪开到 ~35mm、伸到夹持段、再闭合到 ~29mm）。
+      真机上规是**人工放进夹爪**的（P6 里有提示交互），仿真里先立在座上，
+      便于确认夹爪能不能够到、开合范围够不够。