P5c-3 仿真启动器跑通：Gazebo + spawn + 两个控制器 active

端到端验收（headless，本机无显示器）：
    ign 加载 Physics/UserCommands/SceneBroadcaster/Sensors + gz_ros2_control
    机械臂 spawn 进 assembly 世界，7 个 sim_joint 全在 /joint_states
    joint_state_broadcaster 与 rizon_arm_controller 双双 active
    world → sim_tcp 可查，且末端位姿 3 秒内不变（重力下站住）
    网格解析报错 0 条

新增：
  scripts/run_sim_gz.sh                  此前**不存在**，却被 install_sim_env.sh
                                         与 build_sim.sh 的提示语引用
  arm_sim_gazebo/robot_description.py    展开 + 归一化，launch 与 CLI 共用一份
  arm_sim_gazebo/gazebo_sim_launcher.py  薄壳：准备描述后转发给 launch 文件
  launch/gazebo_sim.launch.py            起 gz → 发归一化 URDF → spawn → 拉控制器
  test/test_robot_description.py         含"仓库里这份 world 必须过守卫"的测试

接线过程中实测踩到并修掉的三个坑，共同特征是**每一步看起来都成功**：

1) urdf_normalize 在 Python 路径下静默失效（我自己引入的，最严重）
   xacro 的输出格式在不同环境里不一样：真机环境的 CLI 缩进成多行，而仿真环境
   走 xacro.process_file().toxml() 时全挤在一行。归一化的正则要求标签之间必须有
   \n，于是**仿真路径下它什么都没做**（报告"包裹 0 处"），而 CLI 路径下正常 ——
   之前那次"已修好"的验证走的就是 CLI，所以看起来过了。
   后果是 Gazebo 会丢掉整条机械臂，正是这个归一化本来要防的事。
   已改成对空白格式零假设，并加回归测试锁住。

2) world 里声明了 <plugin> 会把默认系统挤掉
   只要 world 声明任何一个 <plugin>，Gazebo 就不再加载 server.config 的
   Physics/UserCommands/SceneBroadcaster。缺 UserCommands →
   /world/<名>/create 服务不存在 → ros_gz_sim create 调服务超时后回退发 topic、
   没人收 → **模型从未进入 Gazebo**，而 create 节点仍打印
   "OK creation of entity"，控制器则永远卡在 "waiting for service"。
   已在这三个系统在 assembly.world 里显式声明，并加纯文本守卫
   check_base_plugins()，让"加了插件忘了补基础系统"在启动前就被指出。

3) gz 的插件与资源搜索路径缺 $CONDA_PREFIX/lib
   gz_ros2_control 报 "Failed to load system plugin
   [libgz_ros2_control-system.so] : couldn't find shared library"，而 ldd 全齐、
   库就在 $CONDA_PREFIX/lib 下。原因是 gz 的 SystemLoader 先在搜索路径里找文件，
   找不到就直接放弃，不去 dlopen 裸文件名（所以 conda 二进制的 RPATH 兜不住）；
   而 ros_gz_sim 只把 LD_LIBRARY_PATH 拼进搜索路径，conda 激活并不往那里加。
   同一原因导致 mesh 的 model:// 也解析不了（视觉报错很吵，但更要紧的是
   **碰撞网格也加载不出来**，会让后续装配的接触判据失效）。
   由于 gz_sim.launch.py 用 `from os import environ` 在生成 launch 描述时就读
   进程环境，这些变量**必须在 ros2 launch 之前**设好，因此放在 sim_env.sh ——
   在 launch 文件里用 SetEnvironmentVariable 补是无效的，会被它自己算的那份
   字典覆盖。

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

devices/sim/gazebo/manifest.yaml

@@ -3,26 +3,37 @@
 #
 # Gazebo 仿真平台清单。
 #
-# 进度（截至 P5c-2，如实记录，不要凭印象改）：
+# 进度（截至 P5c-3，如实记录，不要凭印象改）：
 #   ✔ 环境       独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
 #                安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh
 #   ✔ 世界文件   worlds/assembly.world（sdformat 校验通过）
 #   ✔ 机器人描述 urdf/（复用官方 flexiv_description，已通过 sdformat 校验）
 #   ✔ 控制器     config/controllers.yaml（控制器名与真机一致）
-#   ✔ 描述归一化 arm_sim_gazebo 包的 urdf_normalize（16 项单测）
+#   ✔ 描述归一化 arm_sim_gazebo 包的 urdf_normalize（37 项单测）
-#   ✘ 启动器     driver.executable 声明的 gazebo_sim_launcher 尚未实现
+#   ✔ 启动器     gazebo_sim_launcher 已实现并端到端跑通
+#                起 Gazebo → 发归一化 URDF → spawn → 拉起两个控制器（active）
+#                入口脚本 scripts/run_sim_gz.sh（此前缺失，被 install_sim_env.sh
+#                与 build_sim.sh 的提示语引用，照做会报文件不存在）
 #   ✘ 场景模型   models/ 还是空的（工件/通止规模型未做）
 #   ✘ 触觉插件   sensor_plugins/ 还是空的（Orisys 三向力插件未做）
-#   ✘ 相机出图   D405 已建模，但话题出图未联调验证
+#   ✘ 相机出图   D405/D435 传感器已在 Gazebo 里成功注册并发布话题，
+#                但还没接 ros_gz_bridge 到 ROS，也还没验证图像内容
 #
-# 所以：现在**还不能**靠 run_sim.sh 跑起完整仿真，run_sim.sh 走的仍是
+# 所以：D405/D435 与触觉还要等 P5c-4 / P5c-5，装配状态机（P6）依赖它们。
-# arm_bringup 的 mock 驱动（假数据）。接线步骤见本目录 README.md。
 #
-# 一个重要结论（接线过程中实测得出，值得记住）：
+# 三个实测踩到、且都属于"静默失效"的坑（后续改仿真时务必先看这三条）：
-#   官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉，有两处不兼容
+#   1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
-#   （7 轴本体缺 <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN）。
+#      <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。两者都只有
-#   两者都只有 Gazebo 会暴露，RViz/MoveIt 完全正常，所以此前从未被发现。
+#      Gazebo 会暴露，RViz/MoveIt 完全正常。已在启动时归一化解决。
-#   解决方式是在启动时归一化，见 arm_sim_gazebo/urdf_normalize.py 的长注释。
+#   2. world 文件里只要声明了**任何一个** <plugin>，Gazebo 就不再加载默认的
+#      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 之前设
+#      GZ_SIM_SYSTEM_PLUGIN_PATH；资源路径同理（否则 mesh 全加载不出来）。
+#      两者都由 scripts/sim_env.sh 负责。详见该文件里的长注释。
 # =============================================================================
 
 id: gazebo
@@ -34,9 +45,10 @@ vendor: openrobotics
 version: "Fortress 6.16 (ignition-gazebo6, ros_gz + gz_ros2_control 0.7.15)"
 
 description: >
-  物理仿真平台。环境已装好、世界与机器人描述已就绪并通过 sdformat 校验；
+  物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
-  启动器、场景模型（工件/通止规）与 Orisys 触觉插件尚未实现，
+  （起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
-  因此仿真目前还不能替代 run_sim.sh 的 mock 路径。
+  场景模型（工件/通止规）与 Orisys 触觉插件尚未实现；
+  相机传感器已在 Gazebo 内注册并出话题，但还没接到 ROS 侧（P5c-4）。
   注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
   仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
 
@@ -78,10 +90,12 @@ driver:
   executable: gazebo_sim_launcher
   build: colcon
   notes: >
-    包已建（含经单测的 URDF 归一化），但 gazebo_sim_launcher 本身**尚未实现**，
+    已实现并在 setup.py 注册。它只做"准备描述（展开 + 归一化）+ 转发给
-    因此 setup.py 里暂未注册这个入口点 —— 注册了模块不存在，构建能过、
+    launch/gazebo_sim.launch.py"，启动逻辑不在可执行文件里 —— ros2 launch 的
-    一运行就 ImportError。P5c-3 补上实现后再注册。
+    日志/退出码/中断处理都更成熟，自己重造一份只会更难调试。
-    在它可用之前，run_sim.sh 仍走 arm_bringup 的 mock 驱动路径。
+    入口建议走 scripts/run_sim_gz.sh（它会先 source scripts/sim_env.sh，
+    而 Gazebo 的插件/资源搜索路径必须在 ros2 launch 之前设好，见 sim_env.sh）。
+    单独排查描述用：gazebo_sim_launcher --check / --print-urdf。
 
 # --- 配置项声明 ------------------------------------------------------------
 # 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），

devices/sim/gazebo/worlds/assembly.world

@@ -169,6 +169,41 @@
       </link>
     </model>
 
+    <!-- =========================================================================
+         基础系统插件 —— 必须显式声明，否则整个仿真静默失效
+         =========================================================================
+         ⚠️ 实测（本机 RoboStack/conda 的 Fortress 6.16）：
+
+            world 文件里**只要声明了任何一个 <plugin>**，Gazebo 就不再加载
+            server.config 里的默认系统列表（Physics / UserCommands /
+            SceneBroadcaster）。声明一个 Sensors，就会把这三个挤掉。
+
+         后果极其隐蔽，因为每一步看起来都"成功"：
+            - 仿真照常启动、/clock 照常以 ~1000Hz 推进、物理也在算
+            - 但没有 UserCommands → /world/assembly/create 服务不存在
+              → ros_gz_sim create 调服务超时，回退到发 topic，**没人收**
+              → 机械臂其实从未进入 Gazebo
+            - create 节点仍然打印 "OK creation of entity"，**看起来成功了**
+            - 模型不存在 → gz_ros2_control 插件不会加载 → 没有 controller_manager
+              → 两个 spawner 永远卡在 "waiting for service"
+            - 而且没有 SceneBroadcaster → /world/assembly/state 也没有
+              → `ign model -l` 也会超时
+
+         验证办法（应当同时看到 4 个系统：
+         Physics / UserCommands / SceneBroadcaster / Sensors）：
+             ign gazebo -r -s -v 4 <world> 2>&1 | grep "Loaded system"
+         或直接看服务在不在：
+             ign service -l | grep -E "world/assembly/(create|state)"
+
+         ⚠️ 因此：以后往这个 world 里加任何 <plugin>，都不要删掉下面这三个。
+         ========================================================================= -->
+    <plugin filename="libignition-gazebo6-physics-system.so"
+            name="ignition::gazebo::systems::Physics"/>
+    <plugin filename="libignition-gazebo6-user-commands-system.so"
+            name="ignition::gazebo::systems::UserCommands"/>
+    <plugin filename="libignition-gazebo6-scene-broadcaster-system.so"
+            name="ignition::gazebo::systems::SceneBroadcaster"/>
+
     <!-- =========================================================================
          传感器渲染系统插件（必须，且必须在 world 级）
          =========================================================================

scripts/run_sim_gz.sh

@@ -0,0 +1,81 @@
+#!/usr/bin/env bash
+# =============================================================================
+# 启动 Gazebo 仿真。
+#
+# 用法:
+#     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 --print-urdf     # 打印归一化后的 URDF
+#     bash scripts/run_sim_gz.sh --dry-run        # 只打印将执行的命令
+#     bash scripts/run_sim_gz.sh --verbose        # 打开 Gazebo 详细日志
+#     bash scripts/run_sim_gz.sh --world /abs/other.world
+#     bash scripts/run_sim_gz.sh --no-gripper --no-sensors   # 最省资源的运动学验证
+#
+# 与 scripts/run_sim.sh 的区别（两个名字容易混，这里说清）：
+#     run_sim.sh     ← 走 arm_bringup 的 **mock** 驱动：假数据 + RViz，无物理
+#     run_sim_gz.sh  ← 走 Gazebo：真物理、真接触
+#   两者互不影响，也都不需要真机。要验证力控/接触类逻辑只能用后者。
+#
+# ⚠️ 这个脚本此前**不存在**，却被 install_sim_env.sh（两处）与 build_sim.sh 的
+#    提示语引用 —— 照做会直接报"文件不存在"。现在补上。
+# =============================================================================
+set -euo pipefail
+
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
+cd "${ROOT}"
+
+# 显式映射到 gazebo_sim_launcher 的选项，不做原样透传。
+# 透传会把拼错的选项静默吞掉（ros2 launch 对未知的 key:=value 不报错），
+# 而"我明明设了 --gui 却没生效"这类问题极难查。
+LAUNCH_ARGS=()
+PASS_ONLY=0   # 1 = 只准备描述，不启动（--check / --print-urdf / --dry-run）
+
+while [[ $# -gt 0 ]]; do
+  case "$1" in
+    --gui)         LAUNCH_ARGS+=(--headless false); shift ;;
+    --headless)    LAUNCH_ARGS+=(--headless true);  shift ;;
+    --verbose|-v)  LAUNCH_ARGS+=(--verbose);        shift ;;
+    --no-gripper)  LAUNCH_ARGS+=(--no-gripper);     shift ;;
+    --no-sensors)  LAUNCH_ARGS+=(--no-sensors);     shift ;;
+    --check|--print-urdf|--dry-run)
+                   LAUNCH_ARGS+=("$1"); PASS_ONLY=1; shift ;;
+    --world)
+                   [[ -n "${2:-}" ]] || { echo "--world 需要跟一个路径" >&2; exit 2; }
+                   LAUNCH_ARGS+=(--world "$2"); shift 2 ;;
+    -h|--help)     sed -n '2,18p' "$0"; exit 0 ;;
+    *)             echo "未知参数: $1（用 --help 看用法）" >&2; exit 2 ;;
+  esac
+done
+
+if [[ ! -f "${ROOT}/install_sim/setup.bash" ]]; then
+  echo "install_sim/ 还不存在。先跑:" >&2
+  echo "    bash scripts/build_sim.sh" >&2
+  exit 1
+fi
+
+# sim_env.sh 会先退到 base 再激活目标环境，避免同一 shell 里来回切造成的
+# PATH/PYTHONPATH 残留（详见 scripts/lib/conda_switch.sh 顶部的实测说明）。
+# shellcheck disable=SC1091
+source "${ROOT}/scripts/sim_env.sh"
+
+if ! command -v ign >/dev/null 2>&1; then
+  echo "找不到 ign（Gazebo Fortress 的 CLI）。先跑:" >&2
+  echo "    bash scripts/install_sim_env.sh" >&2
+  exit 1
+fi
+
+if [[ "${PASS_ONLY}" -eq 1 ]]; then
+  # 这些选项不启动任何东西，只准备描述，直接转发。
+  exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
+fi
+
+echo "=== 启动 Gazebo 仿真 ==="
+echo "  环境      : ${CONDA_DEFAULT_ENV:-?}   ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-?}"
+echo "  退出方式  : Ctrl-C"
+echo
+echo "  首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。"
+echo "  验收:  ros2 control list_controllers   应看到两个 active"
+echo
+
+exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"

scripts/sim_env.sh

@@ -63,6 +63,47 @@ export ROS2_WS="${ROOT}"
 # 真机用的是默认 domain，两者天然隔离。
 export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-77}"
 
+# =============================================================================
+# Gazebo 的插件与资源搜索路径 —— 必须在这里设，不能只在 launch 里设
+# =============================================================================
+# ros_gz_sim 的 gz_sim.launch.py 用的是 `from os import environ`，即在**生成
+# launch 描述时**读进程环境，然后把它算出来的值作为 additional_env 传给 gz。
+# 所以用 launch 的 SetEnvironmentVariable 补是没用的：那个值会被 gz_sim.launch.py
+# 自己算的那份字典覆盖掉。必须在 ros2 launch 之前就进到进程环境里。
+#
+# 1) 插件搜索路径
+#    缺了它，Gazebo 会在加载 gz_ros2_control 时报：
+#        [Err] [SystemLoader.cc:94] Failed to load system plugin
+#        [libgz_ros2_control-system.so] : couldn't find shared library.
+#    而库其实就在 $CONDA_PREFIX/lib 下（`ldd` 也全齐）。原因是 gz 的
+#    SystemLoader 先在搜索路径里找文件，找不到就直接放弃，**不会**去
+#    dlopen 一个裸文件名（所以 conda 二进制的 RPATH 兜不住）。
+#    又因为 gz_sim.launch.py 只把 LD_LIBRARY_PATH 拼进搜索路径，而 conda 激活
+#    并不往 LD_LIBRARY_PATH 里加 $CONDA_PREFIX/lib，于是这条路径永远缺失。
+#
+# 2) 资源搜索路径
+#    gz 的 URDF 解析器会把 mesh 的 package:// 改写成 model://，
+#    而 model://<名> 是去"资源路径下的子目录"里找的。因此这里放的必须是
+#    **各包 share 目录的上一层**（该层的子目录名就是包名），不是 share 目录本身。
+#    缺了它的表现是一堆非致命但很吵的报错（视觉网格加载不出来），
+#    并且**碰撞网格同样加载不出来** —— 后者会让装配任务的接触判据失效。
+if [[ -n "${CONDA_PREFIX:-}" ]]; then
+  export GZ_SIM_SYSTEM_PLUGIN_PATH="${CONDA_PREFIX}/lib${GZ_SIM_SYSTEM_PLUGIN_PATH:+:${GZ_SIM_SYSTEM_PLUGIN_PATH}}"
+  # IGN_* 是 Fortress（改名之前）用的名字，两个都设，兼容新旧插件查找逻辑
+  export IGN_GAZEBO_SYSTEM_PLUGIN_PATH="${CONDA_PREFIX}/lib${IGN_GAZEBO_SYSTEM_PLUGIN_PATH:+:${IGN_GAZEBO_SYSTEM_PLUGIN_PATH}}"
+fi
+
+_res_paths=()
+for _d in "${ROOT}"/install_sim/*/share; do
+  [[ -d "${_d}" ]] && _res_paths+=("${_d}")
+done
+_res_paths+=("${ROOT}/devices/sim/gazebo/models")   # 以后往这里放自定义模型
+_res_joined="$(IFS=:; echo "${_res_paths[*]}")"
+
+export GZ_SIM_RESOURCE_PATH="${_res_joined}${GZ_SIM_RESOURCE_PATH:+:${GZ_SIM_RESOURCE_PATH}}"
+export IGN_GAZEBO_RESOURCE_PATH="${_res_joined}${IGN_GAZEBO_RESOURCE_PATH:+:${IGN_GAZEBO_RESOURCE_PATH}}"
+unset _res_paths _res_joined _d
+
 # 仿真与真机共用同一份 site.yaml —— 这样"仿真里调好的参数搬到真机也生效"。
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true

src/arm_sim_gazebo/arm_sim_gazebo/gazebo_sim_launcher.py

@@ -0,0 +1,190 @@
+"""gazebo_sim_launcher —— devices/sim/gazebo/manifest.yaml 声明的那个可执行。
+
+职责刻意做得很薄：它只负责"准备描述"这件事，然后交给常规的 launch 文件去跑。
+不把整套启动逻辑塞进可执行文件里，是因为 `ros2 launch` 的日志/退出码/中断处理
+都更成熟，自己重造一份只会更难调试。
+
+用法：
+    gazebo_sim_launcher                      # 准备描述并启动仿真
+    gazebo_sim_launcher --print-urdf         # 只看展开+归一化后的 URDF（排查用）
+    gazebo_sim_launcher --check              # 只做自检，不启动任何东西
+    gazebo_sim_launcher --headless false     # 带 GUI
+    gazebo_sim_launcher --dry-run            # 打印将要执行的命令，不执行
+
+为什么 --print-urdf / --check 值得单独做：仿真跑不起来时，绝大多数问题都在
+"描述对不对"这一层（link 被丢、关节名对不上）。能把描述单独打出来看，
+比对着整套仿真的日志找要快一个量级。这两个选项都不需要 Gazebo 在跑。
+"""
+from __future__ import annotations
+
+import argparse
+import os
+import sys
+from pathlib import Path
+from typing import List, Optional
+
+from .robot_description import (
+    SimDescriptionError,
+    check_base_plugins,
+    default_controllers,
+    default_world,
+    default_xacro,
+    prepare_urdf,
+    repo_root,
+    world_name,
+)
+
+LAUNCH_PKG = "arm_sim_gazebo"
+LAUNCH_FILE = "gazebo_sim.launch.py"
+
+
+def _build_launch_cmd(extra: List[str]) -> List[str]:
+    # 用 python -m 调 ros2 而不是 exec "ros2"：这样在 ros2 CLI 不在 PATH 的
+    # 环境里也能给出清晰报错，而不是一句 "No such file or directory"。
+    return ["ros2", "launch", LAUNCH_PKG, LAUNCH_FILE, *extra]
+
+
+def main(argv: Optional[List[str]] = None) -> int:
+    parser = argparse.ArgumentParser(
+        prog="gazebo_sim_launcher",
+        description="准备仿真描述并启动 Gazebo（arm_sim_gazebo）。",
+    )
+    parser.add_argument(
+        "--print-urdf",
+        action="store_true",
+        help="把展开并归一化后的 URDF 打到 stdout，不启动仿真",
+    )
+    parser.add_argument(
+        "--check",
+        action="store_true",
+        help="只做自检（能否找到仓库根、描述能否展开、归一化了什么），不启动",
+    )
+    parser.add_argument(
+        "--dry-run",
+        action="store_true",
+        help="打印将要执行的 ros2 launch 命令，不执行",
+    )
+    parser.add_argument(
+        "--no-gripper",
+        action="store_true",
+        help="不挂夹爪（纯运动学验证时更快）",
+    )
+    parser.add_argument(
+        "--no-sensors",
+        action="store_true",
+        help="不挂相机与触觉",
+    )
+    parser.add_argument(
+        "--world", default="", help="世界文件路径，覆盖默认的 assembly.world"
+    )
+    parser.add_argument(
+        "--headless",
+        default="true",
+        choices=["true", "false"],
+        help="true=只开 Gazebo 服务端（无显示器环境用）；false=带 GUI",
+    )
+    parser.add_argument(
+        "--verbose",
+        action="store_true",
+        help="打开 Gazebo 详细日志（排查插件加载失败时开）",
+    )
+    args = parser.parse_args(argv)
+
+    # ---- 1) 准备描述（这一步是真正有内容的，其余都是转发）------------------
+    try:
+        root = repo_root()
+        xacro_path = default_xacro(root)
+        controllers = default_controllers(root)
+        world = args.world or str(default_world(root))
+
+        urdf, report = prepare_urdf(
+            xacro_path=xacro_path,
+            controllers_file=controllers,
+            load_gripper=not args.no_gripper,
+            with_sensors=not args.no_sensors,
+        )
+        wname = world_name(world)
+        missing = check_base_plugins(Path(world))
+    except SimDescriptionError as exc:
+        print(f"[gazebo_sim_launcher] 失败：\n{exc}", file=sys.stderr)
+        return 2
+
+    if missing:
+        # 这一条能省掉一整个下午：world 漏声明基础系统时，仿真看起来完全正常
+        # （/clock 在走、create 打印成功），但机械臂从未进入 Gazebo。
+        print(
+            "[gazebo_sim_launcher] ⚠ world 缺少基础系统: "
+            + ", ".join(missing) + "\n"
+            "    world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认系统列表。\n"
+            "    缺 UserCommands → /world/<名的>/create 服务不存在 → 模型根本不会\n"
+            "    被创建，而 create 仍会打印 'OK creation of entity'。最终表现为\n"
+            "    控制器永远拉不起来。修法：在这个 world 里显式声明\n"
+            "    Physics / UserCommands / SceneBroadcaster（见文件里的注释）。",
+            file=sys.stderr,
+        )
+
+    if args.print_urdf:
+        # URDF 走 stdout（可重定向），摘要走 stderr（不污染管道）
+        sys.stdout.write(urdf)
+        print(f"\n[gazebo_sim_launcher] {report.summary()}", file=sys.stderr)
+        return 0
+
+    if args.check:
+        print("[gazebo_sim_launcher] 自检通过")
+        print(f"  仓库根      : {root}")
+        print(f"  描述        : {xacro_path}")
+        print(f"  控制器配置  : {controllers}")
+        print(f"  世界        : {world}  (world 名: {wname})")
+        print(f"  URDF 归一化 : {report.summary()}")
+        if missing:
+            print(
+                f"  ⚠ world 缺少基础系统: {', '.join(missing)}\n"
+                "    world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认系统。\n"
+                "    缺 UserCommands → /world/<名的>/create 服务不存在 → 模型不会被创建，\n"
+                "    而 create 仍打印 'OK creation of entity'，控制器永远拉不起来。"
+            )
+        else:
+            print("  world 基础系统 : Physics / UserCommands / SceneBroadcaster 齐备")
+        if not report.changed:
+            print(
+                "  ⚠ 归一化没有改动任何内容。若后续 Gazebo 报 link 不存在，\n"
+                "    先确认 flexiv_description 的版本是否已自行修好那两个上游缺陷。"
+            )
+        print("\n  下一步: gazebo_sim_launcher --dry-run   # 看会执行什么")
+        return 0
+
+    # ---- 2) 转发给 launch 文件 ---------------------------------------------
+    # 参数名在这里显式映射到 launch 参数，不做通用透传 —— 通用透传会把拼错
+    # 的参数名静默吞掉（ros2 launch 对未知的 key:=value 不报错），
+    # 而"我明明设了 --headless false 却没生效"这种问题极难查。
+    extra = [
+        "headless:=" + args.headless,
+        "verbose:=" + ("true" if args.verbose else "false"),
+    ]
+    if args.world:
+        extra.append("world:=" + args.world)
+    if args.no_gripper:
+        extra.append("load_gripper:=false")
+    if args.no_sensors:
+        extra.append("with_sensors:=false")
+
+    cmd = _build_launch_cmd(extra)
+    print(f"[gazebo_sim_launcher] {report.summary()}")
+    print(f"[gazebo_sim_launcher] exec: {' '.join(cmd)}")
+
+    if args.dry_run:
+        return 0
+
+    try:
+        os.execvp(cmd[0], cmd)
+    except FileNotFoundError:
+        print(
+            "[gazebo_sim_launcher] 找不到 ros2 命令。先激活环境：\n"
+            "    source scripts/sim_env.sh",
+            file=sys.stderr,
+        )
+        return 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py

@@ -0,0 +1,236 @@
+"""组装仿真用的机器人描述：xacro 展开 → 归一化 → 交给 ROS / Gazebo。
+
+=============================================================================
+为什么单独一个模块
+=============================================================================
+"把 devices/sim/gazebo/urdf 下的描述变成一份可用的 URDF"这件事有两处需要：
+    1. launch 文件（真正启动仿真时）
+    2. gazebo_sim_launcher 的 --print-urdf（只想看看展开+归一化后长什么样）
+两边各写一份必然漂移，所以实现只放这里，两处都 import 它。
+
+=============================================================================
+仓库根怎么找
+=============================================================================
+devices/ **不是 ROS 包**，所以 $(find ...) 找不到它，必须用文件系统路径。
+按可靠性从高到低试三种：
+    1. $ROS2_WS（scripts/sim_env.sh 会导出）—— 最可靠，显式且便宜
+    2. 从当前工作目录向上找标志文件 —— 手工跑脚本时的常见情形
+    3. 从本模块所在位置向上找 —— 兜底（安装到 install_sim/ 后路径较深）
+三者都失败时给出明确的报错和修复提示，而不是抛一个看不懂的 FileNotFoundError。
+"""
+from __future__ import annotations
+
+import os
+import re
+import subprocess
+from pathlib import Path
+from typing import Dict, List, Optional, Tuple
+
+from .urdf_normalize import NormalizeReport, normalize_urdf
+
+# 用它判断"这个目录是不是仓库根"。选它是因为这个文件是仿真描述的唯一入口，
+# 存在即说明目录结构正确；不像 .git 那样在导出/打包后可能不存在。
+_MARKER = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro"
+
+_XACRO_REL = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro"
+_CONTROLLERS_REL = "devices/sim/gazebo/config/controllers.yaml"
+_WORLD_REL = "devices/sim/gazebo/worlds/assembly.world"
+
+
+class SimDescriptionError(RuntimeError):
+    """描述准备过程中的可预期失败，消息面向使用者而非开发者。"""
+
+
+def repo_root(start: Optional[Path] = None) -> Path:
+    """定位仓库根。找不到时抛 SimDescriptionError 并说明怎么修。"""
+    env = os.environ.get("ROS2_WS", "").strip()
+    if env:
+        p = Path(env).expanduser()
+        if (p / _MARKER).is_file():
+            return p
+
+    seeds = []
+    if start is not None:
+        seeds.append(Path(start).expanduser())
+    seeds.append(Path.cwd())
+    seeds.append(Path(__file__).resolve().parent)
+
+    for seed in seeds:
+        cur = seed.resolve()
+        for _ in range(12):  # 足够覆盖仓库内的任意深度
+            if (cur / _MARKER).is_file():
+                return cur
+            if cur.parent == cur:
+                break
+            cur = cur.parent
+
+    raise SimDescriptionError(
+        "找不到仓库根（即包含 %s 的目录）。\n"
+        "  修法（任选其一）：\n"
+        "    1) source scripts/sim_env.sh   # 它会导出 ROS2_WS\n"
+        "    2) 在仓库根目录下执行\n"
+        "  已试过的起点：%s" % (_MARKER, ", ".join(str(s) for s in seeds))
+    )
+
+
+def default_xacro(root: Optional[Path] = None) -> Path:
+    return (root or repo_root()) / _XACRO_REL
+
+
+def default_controllers(root: Optional[Path] = None) -> Path:
+    return (root or repo_root()) / _CONTROLLERS_REL
+
+
+def default_world(root: Optional[Path] = None) -> Path:
+    return (root or repo_root()) / _WORLD_REL
+
+
+def world_name(world_file: Path) -> str:
+    """从 world 文件里读出 <world name="...">。
+
+    为什么解析而不是让调用方传：world 名必须和文件内容一致，让它们分别配置
+    就等于制造一个"改了文件名忘了改参数"的机会。create 参数写错 world 名时
+    Gazebo 不会报错，只是实体静默地没被创建。
+    """
+    try:
+        text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+    except OSError as exc:
+        raise SimDescriptionError(f"读不到 world 文件 {world_file}: {exc}") from exc
+
+    # 同时接受单引号与双引号：手写 world 时两种都会出现，
+    # 早期只认双引号，于是 `<world name='x'>` 会被判成"找不到 world 名"。
+    m = re.search(r"""<world\b[^>]*\bname\s*=\s*["']([^"']+)["']""", text)
+    if not m:
+        raise SimDescriptionError(
+            f"{world_file} 里找不到 <world name=\"...\">，无法确定 world 名。"
+        )
+    return m.group(1)
+
+
+# 只要 world 里声明了任何一个 <plugin>，Gazebo 就不再加载 server.config 的默认
+# 系统列表，这三个基础系统必须由 world 自己声明，否则整个仿真静默失效。
+# 详细后果见 assembly.world 里那段注释，以及这里的 check_base_plugins()。
+BASE_SYSTEMS = {
+    "Physics": "ignition::gazebo::systems::Physics",
+    "UserCommands": "ignition::gazebo::systems::UserCommands",
+    "SceneBroadcaster": "ignition::gazebo::systems::SceneBroadcaster",
+}
+
+
+def check_base_plugins(world_file: Path) -> List[str]:
+    """检查 world 是否漏声明基础系统。返回缺失的短名列表（空 = 没问题）。
+
+    背景（实测，本机 RoboStack/conda 的 Fortress 6.16）：
+        world 里**只要声明了任何一个 <plugin>**，Gazebo 就跳过 server.config 里
+        的默认系统。于是少了 UserCommands → /world/<w>/create 服务不存在 →
+        ros_gz_sim create 超时后回退发 topic、没人收 → **模型从未被创建**，
+        但 create 节点照样打印 "OK creation of entity"。最终表现是控制器永远
+        拉不起来，而每一步日志看起来都是成功的。
+
+    这个检查是纯文本的、不需要 Gazebo 在跑，所以放进 --check 里，
+    让"改了 world 忘了加基础系统"在启动前就被指出来。
+    """
+    text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+    if "<plugin" not in text:
+        return []  # 没声明任何 plugin → Gazebo 用默认配置，反而正常
+
+    missing = []
+    for short, fq in BASE_SYSTEMS.items():
+        # 名字里有点号，用 re.escape 免得被当通配
+        if not re.search(re.escape(fq), text):
+            missing.append(short)
+    return missing
+
+
+def expand_xacro(
+    xacro_path: Path,
+    mappings: Optional[Dict[str, object]] = None,
+) -> str:
+    """跑 xacro 展开。
+
+    优先用 Python 模块（避免子进程与 PATH 依赖），失败再退回命令行。
+    两者行为一致，但模块方式能拿到更干净的报错。
+    """
+    xacro_path = Path(xacro_path)
+    if not xacro_path.is_file():
+        raise SimDescriptionError(f"xacro 文件不存在: {xacro_path}")
+
+    try:
+        import xacro  # type: ignore
+    except ImportError:
+        return _expand_via_cli(xacro_path, mappings)
+
+    try:
+        doc = xacro.process_file(str(xacro_path), mappings=mappings or {})
+    except Exception as exc:  # xacro 的异常类型很杂，统一转成可读报错
+        raise SimDescriptionError(
+            f"xacro 展开失败: {xacro_path}\n  原因: {exc}\n"
+            f"  单独复现: xacro {xacro_path}"
+        ) from exc
+    return doc.toxml()
+
+
+def _expand_via_cli(xacro_path: Path, mappings: Optional[Dict[str, object]]) -> str:
+    cmd = ["xacro", str(xacro_path)]
+    for k, v in (mappings or {}).items():
+        cmd.append(f"{k}:={v}")
+    try:
+        r = subprocess.run(cmd, capture_output=True, text=True, timeout=120)
+    except FileNotFoundError as exc:
+        raise SimDescriptionError(
+            "既 import 不到 xacro 模块，也找不到 xacro 命令。\n"
+            "  修法: source scripts/sim_env.sh"
+        ) from exc
+    if r.returncode != 0:
+        raise SimDescriptionError(
+            f"xacro 展开失败（退出码 {r.returncode}）: {xacro_path}\n"
+            f"  {(r.stderr or r.stdout).strip()[:800]}"
+        )
+    return r.stdout
+
+
+def prepare_urdf(
+    xacro_path: Optional[Path] = None,
+    controllers_file: Optional[Path] = None,
+    load_gripper: bool = True,
+    with_sensors: bool = True,
+    rizon_type: str = "Rizon4",
+    arm_prefix: str = "sim",
+    robot_sn: str = "",
+) -> Tuple[str, NormalizeReport]:
+    """展开 + 归一化，返回 (URDF 文本, 归一化报告)。
+
+    调用方**必须**把这份 URDF 同时用于两处，否则会出问题：
+        - robot_state_publisher 发布到 /robot_description
+        - ros_gz_sim create 用它 spawn 进 Gazebo
+    gz_ros2_control 的插件是从 /robot_description 读 <ros2_control> 的
+    （SDF 里只存了插件名和参数文件路径，接口声明不在 SDF 里，实测确认）。
+    所以若 RSP 发的是**未归一化**的那份，控制器注册的关节名就会和 Gazebo 里的
+    对不上，症状是 "Joint 'sim_joint1' not found in the robot model"。
+    """
+    root = repo_root()
+    xacro_path = Path(xacro_path) if xacro_path else default_xacro(root)
+    controllers_file = (
+        Path(controllers_file) if controllers_file else default_controllers(root)
+    )
+
+    if not Path(controllers_file).is_file():
+        raise SimDescriptionError(
+            f"控制器配置不存在: {controllers_file}\n"
+            f"  它是 arm.sim.gazebo.xacro 里系统插件的 <parameters> 指向的文件，"
+            f"缺了会导致 Gazebo 报 'Failed to load system plugin'。"
+        )
+
+    urdf = expand_xacro(
+        xacro_path,
+        {
+            "controllers_file": str(Path(controllers_file).resolve()),
+            "load_gripper": "true" if load_gripper else "false",
+            "with_sensors": "true" if with_sensors else "false",
+            "rizon_type": rizon_type,
+            "arm_prefix": arm_prefix,
+            "robot_sn": robot_sn,
+        },
+    )
+    normalized, report = normalize_urdf(urdf)
+    return normalized, report

src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py

@@ -60,14 +60,27 @@ ZERO_MASS_FIX_KG = "1e-4"
 # 惯量全 0 时补的对角值。极致小值只为让张量非退化，不引入可观测的转动惯量。
 ZERO_INERTIA_FIX = "1e-9"
 
-# 裸露的 origin + mass + inertia 三件套（自闭合标签，连续三行）。
+# 裸露的 origin + mass + inertia 三件套。
-# 夹爪的 <inertial> 块内部也是这三行，所以必须按 link 块处理并跳过
+#
-# 已含 <inertial> 的块，否则会二次包裹 —— sdformat 会报
+# ⚠️ 标签之间的空白必须是**可选**的（\s*），不能要求有换行 —— 这是个实测踩到的坑：
+#    xacro 的输出格式在不同环境里不一样。真机环境的 xacro 会缩进成多行：
+#        <origin .../>
+#        <mass .../>
+#        <inertia .../>
+#    而仿真环境（另一份 xacro 版本）走 xacro.process_file().toxml() 时**全部挤在一行**：
+#        <link ...><origin .../><mass .../><inertia .../><visual ...>
+#    早期这里的正则要求标签之间必须有 \n，于是 Python 路径下归一化静默地什么都没做
+#    （报告"包裹 0 处"），而 CLI 路径下却正常 —— 因为命令行会缩进。
+#    后果极严重：Gazebo 会丢掉整条机械臂，正是这个归一化本来要防的事。
+#    教训：这段代码不能对空白格式有任何假设。
+#
+# 夹爪的 <inertial> 内部也是这三个标签，所以必须按 link 块处理并跳过已含
+# <inertial> 的块，否则会二次包裹 —— sdformat 会报
 # "Inertial element must have a mass element"（已踩过）。
 _TRIPLE = re.compile(
-    r"(?P<ind>[ \t]*)<origin(?P<o>[^>]*?)/>[ \t]*\n"
+    r"(?P<ind>[ \t]*)<origin(?P<o>[^>]*?)/>"
-    r"[ \t]*<mass(?P<m>[^>]*?)/>[ \t]*\n"
+    r"\s*<mass(?P<m>[^>]*?)/>"
-    r"[ \t]*<inertia(?P<i>[^>]*?)/>"
+    r"\s*<inertia(?P<i>[^>]*?)/>"
 )
 
 _LINK_BLOCK = re.compile(r"<link\b[^>]*>.*?</link>", re.S)
@@ -175,13 +188,18 @@ def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
         if "<inertial>" not in block:
             def repl(mm: re.Match) -> str:
                 ind = mm.group("ind")
-                return (
+                o, m_, i = mm.group("o"), mm.group("m"), mm.group("i")
-                    f"{ind}<inertial>\n"
+                # 原样保留输入的排版风格：多行输入就给多行输出，单行就给单行输出。
-                    f"{ind}  <origin{mm.group('o')}/>\n"
+                # 凭空插入或删除换行会让 diff 里全是噪声，也不必要。
-                    f"{ind}  <mass{mm.group('m')}/>\n"
+                if "\n" in mm.group(0):
-                    f"{ind}  <inertia{mm.group('i')}/>\n"
+                    return (
-                    f"{ind}</inertial>"
+                        f"{ind}<inertial>\n"
-                )
+                        f"{ind}  <origin{o}/>\n"
+                        f"{ind}  <mass{m_}/>\n"
+                        f"{ind}  <inertia{i}/>\n"
+                        f"{ind}</inertial>"
+                    )
+                return f"{ind}<inertial><origin{o}/><mass{m_}/><inertia{i}/></inertial>"
 
             block, n = _TRIPLE.subn(repl, block)
             report.wrapped_inertials += n

src/arm_sim_gazebo/launch/gazebo_sim.launch.py

@@ -0,0 +1,278 @@
+"""启动 Gazebo 仿真：世界 + 机器人 + 控制器。
+
+用法（推荐走 scripts/run_sim_gz.sh，它负责先激活仿真环境）：
+    ros2 launch arm_sim_gazebo gazebo_sim.launch.py
+    ros2 launch arm_sim_gazebo gazebo_sim.launch.py world:=/abs/other.world headless:=false
+
+=============================================================================
+启动顺序为什么是这样（每一环都踩过）
+=============================================================================
+    1. robot_state_publisher  发布归一化后的 URDF 到 /robot_description
+    2. Gazebo（gz_sim）        加载世界
+    3. ros_gz_sim create       从 /robot_description spawn 机器人
+    4. controller spawner      拉起 joint_state_broadcaster + rizon_arm_controller
+
+两处顺序是硬要求，颠倒了只表现为"静默不工作"：
+
+  a) RSP 必须在 create 之前就绪
+     create 用 -topic robot_description 取 URDF。RSP 以 transient_local 发布
+     （latched），迟到的订阅者也能拿到，所以这里主要是容忍，不是硬依赖。
+
+  b) controller spawner 必须在 Gazebo 起来之后
+     controller_manager **跑在 Gazebo 进程里**（由 gz_ros2_control 的系统插件
+     拉起），不是独立的 ros2_control_node。Gazebo 没起来时 spawner 找不到
+     /controller_manager 服务，会一直重试直到 timeout。
+
+  c) 归一化后的 URDF 必须同时喂给 RSP 和 create
+     gz_ros2_control 是从 /robot_description 读 <ros2_control> 段落的
+     （SDF 里只存插件名和参数路径，接口声明不在 SDF —— 实测确认）。
+     两边不是同一份时，控制器注册的关节名与 Gazebo 里的对不上，
+     症状是 "Joint 'sim_joint1' not found in the robot model"。
+
+=============================================================================
+use_sim_time
+=============================================================================
+仿真里所有节点都要 use_sim_time=True，时间来自 /clock（gz → ROS 桥接）。
+不设的话 TF 时间戳会用墙上时钟，与 Gazebo 的仿真时间不同源，
+表现为 RViz 里模型抖动、tf2 报 "Lookup would require extrapolation"。
+/clock 的桥接在下面 clock_bridge 里，故意排在最前面。
+"""
+from __future__ import annotations
+
+import os
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    LogInfo,
+    OpaqueFunction,
+    TimerAction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+# 启动延迟（秒）。都是为了避开"服务还没起来就去调"的竞态。
+# 数值偏保守：多等几秒远好过让用户对着一个静默失败的界面调试。
+SPAWN_DELAY = 4.0
+CONTROLLER_DELAY = 8.0
+
+CONTROLLER_NAMES = ("joint_state_broadcaster", "rizon_arm_controller")
+
+
+def _prepare(context, *args, **kwargs):
+    """展开 + 归一化描述，并组装所有动作。
+
+    放在 OpaqueFunction 里做，因为需要先算出 URDF 文本才能构造
+    robot_state_publisher 的参数 —— 而 launch 的 substitution 机制
+    没法在 Python 里"看到"中间结果。
+    """
+    from arm_sim_gazebo.robot_description import (
+        SimDescriptionError,
+        check_base_plugins,
+        default_controllers,
+        default_world,
+        default_xacro,
+        prepare_urdf,
+        repo_root,
+        world_name,
+    )
+
+    world = LaunchConfiguration("world").perform(context).strip()
+    load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower()
+    with_sensors = LaunchConfiguration("with_sensors").perform(context).strip().lower()
+    headless = LaunchConfiguration("headless").perform(context).strip().lower()
+    verbose = LaunchConfiguration("verbose").perform(context).strip().lower()
+
+    try:
+        root = repo_root()
+        world_path = world or str(default_world(root))
+
+        urdf, report = prepare_urdf(
+            xacro_path=default_xacro(root),
+            controllers_file=default_controllers(root),
+            load_gripper=load_gripper != "false",
+            with_sensors=with_sensors != "false",
+        )
+        wname = world_name(world_path)
+        missing_base = check_base_plugins(Path(world_path))
+    except SimDescriptionError as exc:
+        # 描述准备失败是最常见的上手障碍（环境没 source、路径不对）。
+        # 直接把可读原因打出来，而不是让它变成一个看不懂的 launch traceback。
+        raise RuntimeError(f"\n[gazebo_sim] 仿真描述准备失败：\n{exc}\n") from exc
+
+    actions = [
+        LogInfo(msg=f"[gazebo_sim] 仓库根: {root}"),
+        LogInfo(msg=f"[gazebo_sim] 世界: {world_path} (world 名: {wname})"),
+        LogInfo(msg=f"[gazebo_sim] URDF 归一化: {report.summary()}"),
+    ]
+    if missing_base:
+        actions.append(
+            LogInfo(
+                msg=(
+                    "[gazebo_sim] ⚠ world 缺少基础系统: "
+                    + ", ".join(missing_base)
+                    + "。world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认"
+                    "系统列表。缺 UserCommands 会让 /world/<名>/create 服务不存在，"
+                    "模型根本不会被创建，而 create 仍打印 'OK creation of entity' ——"
+                    "最终表现是控制器永远拉不起来。修法见 assembly.world 里的注释。"
+                )
+            )
+        )
+    if not report.changed:
+        actions.append(
+            LogInfo(
+                msg="[gazebo_sim] 注意：归一化没有改动任何内容。若 Gazebo 仍报 link 不存在，"
+                    "先确认 flexiv_description 的版本是否已自行修好这两个上游缺陷。"
+            )
+        )
+
+    # --- 1) 机器人描述 -------------------------------------------------------
+    # 显式传 URDF 字符串（而不是 Command(["xacro ..."])），因为要发的是
+    # **归一化后**的那一份。这是 RSP 与 create 必须一致的东西，见文件头 (c)。
+    actions.append(
+        Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            name="robot_state_publisher",
+            output="screen",
+            parameters=[
+                {"robot_description": urdf},
+                {"use_sim_time": True},
+            ],
+        )
+    )
+
+    # --- 2) Gazebo -----------------------------------------------------------
+    gz_args = ["-r"]  # 立刻开始推进仿真（不等 GUI 里的播放按钮）
+    if verbose == "true":
+        gz_args += ["-v", "4"]  # 打开 Gazebo 自身日志，排查插件加载失败时必需
+    if headless == "true":
+        # -s = 只跑服务端，不开 GUI。无显示器环境（本机就是）必须用这个。
+        gz_args.append("-s")
+    gz_args.append(world_path)
+
+    actions.append(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(
+                    FindPackageShare("ros_gz_sim").perform(context),
+                    "launch",
+                    "gz_sim.launch.py",
+                )
+            ),
+            launch_arguments={"gz_args": " ".join(gz_args)}.items(),
+        )
+    )
+
+    # --- 3) /clock 桥接（sim time 的来源，排在最前）-------------------------
+    actions.append(
+        Node(
+            package="ros_gz_bridge",
+            executable="parameter_bridge",
+            name="clock_bridge",
+            arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
+            output="screen",
+        )
+    )
+
+    # --- 4) spawn 机器人 -----------------------------------------------------
+    # -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
+    # 表现成"launch 一切正常但 RViz 里什么都没有"。
+    actions.append(
+        TimerAction(
+            period=SPAWN_DELAY,
+            actions=[
+                Node(
+                    package="ros_gz_sim",
+                    executable="create",
+                    name="spawn_robot",
+                    output="screen",
+                    arguments=[
+                        "-world", wname,
+                        "-name", "rizon_sim",
+                        "-topic", "robot_description",
+                    ],
+                )
+            ],
+        )
+    )
+
+    # --- 5) 控制器 -----------------------------------------------------------
+    # 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
+    # rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
+    controllers = [
+        TimerAction(
+            period=CONTROLLER_DELAY + i * 2.0,
+            actions=[
+                Node(
+                    package="controller_manager",
+                    executable="spawner",
+                    name=f"spawn_{name}",
+                    output="screen",
+                    arguments=[
+                        name,
+                        "--controller-manager", "/controller_manager",
+                        # controller_manager 在 Gazebo 进程里，它是 sim time；
+                        # spawner 也要用 sim time，否则两者对时不同源。
+                        "--controller-manager-timeout", "120",
+                    ],
+                    parameters=[{"use_sim_time": True}],
+                )
+            ],
+        )
+        for i, name in enumerate(CONTROLLER_NAMES)
+    ]
+    actions.extend(controllers)
+
+    actions.append(
+        LogInfo(
+            msg=(
+                "[gazebo_sim] 启动中。验收命令：\n"
+                "    ros2 topic echo /clock --once                    # 仿真时间在走\n"
+                "    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"
+                "  机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
+                "参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。"
+            )
+        )
+    )
+
+    return actions
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "world",
+                default_value="",
+                description="世界文件绝对路径。留空=用仓库里默认的 assembly.world",
+            ),
+            DeclareLaunchArgument(
+                "load_gripper",
+                default_value="true",
+                description="是否挂 GN01 夹爪。装配任务需要 true",
+            ),
+            DeclareLaunchArgument(
+                "with_sensors",
+                default_value="true",
+                description="是否挂 D405 与触觉。关掉跑得更快，用于纯运动学验证",
+            ),
+            DeclareLaunchArgument(
+                "headless",
+                default_value="true",
+                description="true=只开 Gazebo 服务端（无显示器环境用）；false=带 GUI",
+            ),
+            DeclareLaunchArgument(
+                "verbose",
+                default_value="false",
+                description="true=打开 Gazebo 详细日志（排查插件加载失败时开）",
+            ),
+            OpaqueFunction(function=_prepare),
+        ]
+    )

src/arm_sim_gazebo/setup.py

@@ -1,7 +1,21 @@
+import os
+from glob import glob
+
 from setuptools import find_packages, setup
 
 package_name = 'arm_sim_gazebo'
 
+
+def _existing(paths):
+    """过滤掉不存在的路径。
+
+    glob() 会把**悬空符号链接**也列出来，而 colcon 在拷贝阶段会因此直接失败
+    （"can't copy ... doesn't exist or not a regular file"）。
+    arm_application 的 setup.py 就因这个问题挂过一次构建，这里从一开始就防住。
+    """
+    return [p for p in paths if os.path.isfile(p)]
+
+
 setup(
     name=package_name,
     version='0.1.0',
@@ -9,6 +23,8 @@ setup(
     data_files=[
         ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        # launch 文件必须显式安装，否则 ros2 launch arm_sim_gazebo ... 找不到
+        (os.path.join('share', package_name, 'launch'), _existing(glob('launch/*.py'))),
     ],
     install_requires=['setuptools', 'PyYAML'],
     zip_safe=True,
@@ -18,15 +34,13 @@ 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',
-            # TODO(P5c-3): manifest 声明的启动器
-            #   devices/sim/gazebo/manifest.yaml -> driver.executable: gazebo_sim_launcher
-            # 尚未实现，所以这里**先不注册** —— 注册了但模块不存在，
-            # 构建能过、一运行就 ImportError，不如老实缺着。
-            # P5c-3 会补上 gazebo_sim_launcher.py 并在此注册。
-            # 'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
         ],
     },
 )

src/arm_sim_gazebo/test/test_robot_description.py

@@ -0,0 +1,264 @@
+"""robot_description 的单测。
+
+分两类：
+  1. 不依赖外部资源的（仓库根定位、world 名解析、报错信息）—— 任何时候都能跑
+  2. 依赖 xacro 与 flexiv_description 的（真展开）—— 环境不具备时 skip
+
+第 2 类之所以也值得写成测试：描述能否展开是仿真能不能跑的前置条件，
+而它的失败模式（找不到包、xacro 报错）在 launch 里会被包装成难读的 traceback。
+"""
+import os
+import sys
+import tempfile
+from pathlib import Path
+
+import pytest
+
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from arm_sim_gazebo.robot_description import (  # noqa: E402
+    SimDescriptionError,
+    check_base_plugins,
+    default_world,
+    expand_xacro,
+    prepare_urdf,
+    repo_root,
+    world_name,
+)
+
+
+# ---------------------------------------------------------------------------
+# 仓库根定位
+# ---------------------------------------------------------------------------
+def test_repo_root_found_from_cwd():
+    """从仓库内的任意深度都应能找到根。测试文件本身就在仓库里。"""
+    root = repo_root()
+    assert (root / "devices/sim/gazebo/urdf/arm.sim.urdf.xacro").is_file()
+
+
+def test_repo_root_prefers_ros2_ws(monkeypatch, tmp_path):
+    """ROS2_WS 有效时优先用它 —— 这是最可靠也最便宜的一条路径。"""
+    fake = tmp_path / "ws"
+    (fake / "devices/sim/gazebo/urdf").mkdir(parents=True)
+    (fake / "devices/sim/gazebo/urdf/arm.sim.urdf.xacro").write_text("<robot/>")
+    monkeypatch.setenv("ROS2_WS", str(fake))
+    assert repo_root() == fake
+
+
+def test_repo_root_ignores_bogus_ros2_ws(monkeypatch, tmp_path):
+    """ROS2_WS 指向不含标志文件的目录时要忽略它，继续往下找。
+
+    不能因为环境变量存在就盲信 —— 用户可能在别的项目里留了一个过期的 ROS2_WS。
+    """
+    monkeypatch.setenv("ROS2_WS", str(tmp_path / "empty"))
+    root = repo_root()
+    assert (root / "devices/sim/gazebo/urdf/arm.sim.urdf.xacro").is_file()
+
+
+def test_repo_root_error_is_actionable(monkeypatch, tmp_path):
+    """找不到时必须给出可执行的修复建议，而不是一句 FileNotFoundError。
+
+    这里把标志文件名换成一个不存在的，来真实地走到报错分支 ——
+    直接传一个空目录是走不到的，因为 repo_root 还有"从 cwd / 模块位置向上找"
+    两级兜底（那是有意的鲁棒性，不能为了测试方便去掉）。
+    """
+    import arm_sim_gazebo.robot_description as rd
+
+    monkeypatch.delenv("ROS2_WS", raising=False)
+    monkeypatch.setattr(rd, "_MARKER", "definitely/not/here/marker.txt")
+    with pytest.raises(SimDescriptionError) as ei:
+        repo_root(start=tmp_path)
+    msg = str(ei.value)
+    assert "sim_env.sh" in msg, "报错里应包含修复命令"
+    assert "ROS2_WS" in msg
+
+
+def test_repo_root_does_not_escape_to_filesystem_root(monkeypatch):
+    """从 / 开始也要能安全终止，不能一路爬或无限循环。"""
+    import arm_sim_gazebo.robot_description as rd
+
+    monkeypatch.delenv("ROS2_WS", raising=False)
+    monkeypatch.setattr(rd, "_MARKER", "definitely/not/here/marker.txt")
+    with pytest.raises(SimDescriptionError):
+        repo_root(start=Path("/"))
+
+
+# ---------------------------------------------------------------------------
+# world 名解析
+# ---------------------------------------------------------------------------
+def test_world_name_parsed():
+    with tempfile.NamedTemporaryFile("w", suffix=".world", delete=False) as fh:
+        fh.write('<?xml version="1.0"?>\n<sdf version="1.9">\n'
+                 '  <world name="assembly">\n  </world>\n</sdf>\n')
+        path = fh.name
+    try:
+        assert world_name(Path(path)) == "assembly"
+    finally:
+        os.unlink(path)
+
+
+def test_world_name_handles_single_quotes_and_attrs():
+    """属性顺序/引号风格变体都要认出来（手写 world 时都会出现）。"""
+    with tempfile.NamedTemporaryFile("w", suffix=".world", delete=False) as fh:
+        fh.write("<sdf><world name='my_world' version='1.9'></world></sdf>")
+        path = fh.name
+    try:
+        assert world_name(Path(path)) == "my_world"
+    finally:
+        os.unlink(path)
+
+
+def test_world_name_missing_is_actionable():
+    with tempfile.NamedTemporaryFile("w", suffix=".world", delete=False) as fh:
+        fh.write("<sdf></sdf>")
+        path = fh.name
+    try:
+        with pytest.raises(SimDescriptionError) as ei:
+            world_name(Path(path))
+        assert "world" in str(ei.value)
+    finally:
+        os.unlink(path)
+
+
+def test_world_name_missing_file_is_actionable():
+    with pytest.raises(SimDescriptionError) as ei:
+        world_name(Path("/nonexistent/whatever.world"))
+    assert "读不到" in str(ei.value)
+
+
+# ---------------------------------------------------------------------------
+# 基础系统守卫（对应实测踩到的"静默失效"坑）
+# ---------------------------------------------------------------------------
+def _world(body: str) -> Path:
+    import tempfile
+
+    fh = tempfile.NamedTemporaryFile("w", suffix=".world", delete=False)
+    fh.write('<?xml version="1.0"?>\n<sdf version="1.9">\n'
+             f'  <world name="w">\n{body}\n  </world>\n</sdf>\n')
+    fh.close()
+    return Path(fh.name)
+
+
+def test_no_plugins_world_is_fine():
+    """没声明任何 plugin → Gazebo 用默认配置，反而正常。"""
+    assert check_base_plugins(_world("    <physics/>")) == []
+
+
+def test_missing_base_systems_detected():
+    """只声明 Sensors（真实踩过的情形）必须报出三个缺失。"""
+    p = _world('    <plugin filename="libignition-gazebo6-sensors-system.so"'
+               ' name="ignition::gazebo::systems::Sensors"/>')
+    assert set(check_base_plugins(p)) == {"Physics", "UserCommands", "SceneBroadcaster"}
+
+
+def test_partial_base_systems_detected():
+    p = _world(
+        '    <plugin name="ignition::gazebo::systems::Physics"/>'
+        '    <plugin name="ignition::gazebo::systems::UserCommands"/>'
+        '    <plugin filename="libignition-gazebo6-sensors-system.so"'
+        ' name="ignition::gazebo::systems::Sensors"/>'
+    )
+    assert check_base_plugins(p) == ["SceneBroadcaster"]
+
+
+def test_all_base_systems_present():
+    p = _world(
+        '    <plugin name="ignition::gazebo::systems::Physics"/>'
+        '    <plugin name="ignition::gazebo::systems::UserCommands"/>'
+        '    <plugin name="ignition::gazebo::systems::SceneBroadcaster"/>'
+        '    <plugin name="ignition::gazebo::systems::Sensors"/>'
+    )
+    assert check_base_plugins(p) == []
+
+
+def test_repo_world_passes_the_guard():
+    """仓库里这份 world 必须通过 —— 否则仿真会静默失效。
+
+    这条测试的意义就是防止以后往 assembly.world 里加插件时把基础系统删掉。
+    """
+    assert check_base_plugins(default_world()) == []
+
+
+# ---------------------------------------------------------------------------
+# xacro 展开
+# ---------------------------------------------------------------------------
+def test_expand_missing_file_is_actionable():
+    with pytest.raises(SimDescriptionError) as ei:
+        expand_xacro(Path("/nonexistent/foo.xacro"))
+    assert "不存在" in str(ei.value)
+
+
+# ---------------------------------------------------------------------------
+# 端到端（需要 xacro + flexiv_description，缺则跳过）
+# ---------------------------------------------------------------------------
+def _flexiv_available() -> bool:
+    try:
+        import xacro  # noqa: F401
+    except ImportError:
+        return False
+    try:
+        from ament_index_python.packages import get_package_share_directory
+
+        get_package_share_directory("flexiv_description")
+        return True
+    except Exception:
+        return False
+
+
+needs_flexiv = pytest.mark.skipif(
+    not _flexiv_available(),
+    reason="需要 xacro 与 flexiv_description（先 source scripts/sim_env.sh 并构建）",
+)
+
+
+@needs_flexiv
+def test_prepare_urdf_end_to_end():
+    """真展开 + 归一化，并检查前两轮实测踩到的两个上游缺陷都被修掉。"""
+    urdf, report = prepare_urdf()
+
+    # 问题一：7 轴本体的孤儿 inertial 三件套被包起来
+    assert report.wrapped_inertials >= 8, "7 轴本体 + 其他 link 的包裹数应≥8"
+    # 问题二：夹爪的零质量 dummy link 被补成极小正值
+    assert len(report.fixed_zero_mass) == 6, "左/右指尖+tcp 共 6 个 dummy link"
+    assert len(report.fixed_zero_inertia) == 6
+
+    # 归一化后必须没有任何裸露的 origin/mass/inertia 三件套。
+    # 判定方式：把所有 <inertial>…</inertial> 块摘掉之后，URDF 里不应再出现 <mass>
+    # —— 因为 mass 只可能出现在 inertial 内。早期这里用了"三件套相邻"的正则，
+    # 但那个正则会匹配到 <inertial> 内部同样相邻的三件套，误报。
+    import re
+
+    stripped = re.sub(r"<inertial>.*?</inertial>", "", urdf, flags=re.S)
+    assert "<mass" not in stripped, "有 <mass> 落在 <inertial> 之外"
+    assert not re.search(r"<inertia\b", stripped), "有 <inertia> 落在 <inertial> 之外"
+
+    # 关键帧存在（应用层硬编码查这两个名字）
+    for name in ("sim_tcp", "sim_tool0", "sim_flange", "sim_base_link"):
+        assert f'name="{name}"' in urdf
+    # 关节前缀隔离
+    assert 'name="sim_joint1"' in urdf
+    # 控制器接口
+    assert "gz_ros2_control/GazeboSimSystem" in urdf
+
+
+@needs_flexiv
+def test_prepare_urdf_is_idempotent_on_second_pass():
+    """同一份描述连续准备两次，结果必须一致（否则每次启动都在漂移）。"""
+    a, _ = prepare_urdf()
+    b, _ = prepare_urdf()
+    assert a == b
+
+
+@needs_flexiv
+def test_prepare_urdf_no_gripper_drops_zero_mass_links():
+    """关掉夹爪后就没有零质量 link 了 —— 这也反证了那 6 个零质量确实来自夹爪。"""
+    _, report = prepare_urdf(load_gripper=False)
+    assert report.fixed_zero_mass == []
+
+
+@needs_flexiv
+def test_prepare_urdf_missing_controllers_is_actionable(tmp_path):
+    """控制器配置缺失要给出可读报错 —— 它缺失时 Gazebo 只会说插件加载失败。"""
+    with pytest.raises(SimDescriptionError) as ei:
+        prepare_urdf(controllers_file=tmp_path / "nope.yaml")
+    assert "控制器配置不存在" in str(ei.value)

src/arm_sim_gazebo/test/test_urdf_normalize.py

@@ -107,6 +107,51 @@ def test_indentation_is_preserved():
     assert "          <mass" in out  # 内部再进一级
 
 
+def test_wraps_triple_without_any_newlines():
+    """回归：标签全挤在一行时也必须能包裹。
+
+    这是实测踩到的严重坑：xacro 的输出格式在不同环境里不一样 ——
+    真机环境的 CLI 会缩进成多行，而仿真环境走 xacro.process_file().toxml()
+    时全部挤在一行。早期正则要求标签之间有 \\n，于是**Python 路径下归一化
+    静默地什么都没做**（报告"包裹 0 处"），而 CLI 路径下却正常。
+    后果是 Gazebo 丢掉整条机械臂 —— 正是这个归一化本来要防的事。
+    所以这条测试锁的是"不能对空白格式有任何假设"。
+    """
+    urdf = ROBOT_OPEN + (
+        '<link name="l1"><origin xyz="0 0 0"/><mass value="1.0"/>'
+        '<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>'
+        '<visual name="v"/></link>\n'
+    ) + ROBOT_CLOSE
+
+    out, rep = normalize_urdf(urdf)
+
+    assert rep.wrapped_inertials == 1, "单行输入也必须被包裹"
+    assert (
+        '<inertial><origin xyz="0 0 0"/><mass value="1.0"/>'
+        '<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/></inertial>'
+    ) in out
+    # 不能凭空插入换行
+    assert out.count("\n") == urdf.count("\n")
+
+
+def test_multiline_style_is_kept_multiline():
+    """多行输入保持多行 —— 排版风格原样保留，不因归一化而改变。"""
+    urdf = ROBOT_OPEN + (
+        '  <link name="l1">\n'
+        '      <origin xyz="0 0 0"/>\n'
+        '      <mass value="1.0"/>\n'
+        '      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
+        '  </link>\n'
+    ) + ROBOT_CLOSE
+
+    out, _ = normalize_urdf(urdf)
+
+    # <inertial> 在 ind(6 空格)，其子标签再进一级(8 空格)
+    assert "\n      </inertial>\n" in out
+    assert "\n        <origin xyz=\"0 0 0\"/>\n" in out
+    assert "\n        <mass value=\"1.0\"/>\n" in out
+
+
 # ---------------------------------------------------------------------------
 # 问题二：零质量 / 零惯量
 # ---------------------------------------------------------------------------