orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
端到端验收（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>
This commit is contained in:
arm_dev
2026-09-23 14:48:16 +08:00
parent e9da641160
commit 01940f54e7
11 changed files with 1253 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
devices/sim/gazebo/manifest.yaml
+32 -18
View File
@@ -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 项单测）
# ✘ 启动器 driver.executable 声明的 gazebo_sim_launcher 尚未实现
# ✔ 描述归一化 arm_sim_gazebo 包的 urdf_normalize（37 项单测）
# ✔ 启动器 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 走的仍是
# arm_bringup 的 mock 驱动（假数据）。接线步骤见本目录 README.md。
# 所以：D405/D435 与触觉还要等 P5c-4 / P5c-5，装配状态机（P6）依赖它们。
#
# 一个重要结论（接线过程中实测得出，值得记住）：
# 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉，有两处不兼容
# （7 轴本体缺 <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN）。
# 两者都只有 Gazebo 会暴露，RViz/MoveIt 完全正常，所以此前从未被发现。
# 解决方式是在启动时归一化，见 arm_sim_gazebo/urdf_normalize.py 的长注释。
# 三个实测踩到、且都属于"静默失效"的坑（后续改仿真时务必先看这三条）：
# 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
# <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。两者都只有
# Gazebo 会暴露，RViz/MoveIt 完全正常。已在启动时归一化解决。
# 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 触觉插件尚未实现，
因此仿真目前还不能替代 run_sim.sh 的 mock 路径。
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
场景模型（工件/通止规）与 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 里暂未注册这个入口点 —— 注册了模块不存在，构建能过、
一运行就 ImportError。P5c-3 补上实现后再注册。
在它可用之前，run_sim.sh 仍走 arm_bringup 的 mock 驱动路径。
已实现并在 setup.py 注册。它只做"准备描述（展开 + 归一化）+ 转发给
launch/gazebo_sim.launch.py"，启动逻辑不在可执行文件里 —— ros2 launch 的
日志/退出码/中断处理都更成熟，自己重造一份只会更难调试。
入口建议走 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
+35
View File
@@ -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
Executable
+81
View File
@@ -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
+41
View File
@@ -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
+190
View File
@@ -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
+236
View File
@@ -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
+31 -13
View File
@@ -60,14 +60,27 @@ ZERO_MASS_FIX_KG = "1e-4"
# 惯量全 0 时补的对角值。极致小值只为让张量非退化，不引入可观测的转动惯量。
ZERO_INERTIA_FIX = "1e-9"
# 裸露的 origin + mass + inertia 三件套（自闭合标签，连续三行）。
# 夹爪的 <inertial> 块内部也是这三行，所以必须按 link 块处理并跳过
# 已含 <inertial> 的块，否则会二次包裹 —— sdformat 会报
# 裸露的 origin + mass + inertia 三件套。
#
# ⚠️ 标签之间的空白必须是**可选**的（\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"[ \t]*<mass(?P<m>[^>]*?)/>[ \t]*\n"
r"[ \t]*<inertia(?P<i>[^>]*?)/>"
r"(?P<ind>[ \t]*)<origin(?P<o>[^>]*?)/>"
r"\s*<mass(?P<m>[^>]*?)/>"
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 (
f"{ind}<inertial>\n"
f"{ind} <origin{mm.group('o')}/>\n"
f"{ind} <mass{mm.group('m')}/>\n"
f"{ind} <inertia{mm.group('i')}/>\n"
f"{ind}</inertial>"
)
o, m_, i = mm.group("o"), mm.group("m"), mm.group("i")
# 原样保留输入的排版风格：多行输入就给多行输出，单行就给单行输出。
# 凭空插入或删除换行会让 diff 里全是噪声，也不必要。
if "\n" in mm.group(0):
return (
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
+278
View File
@@ -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
+20 -6
View File
@@ -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
+264
View File
@@ -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
+45
View File
@@ -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
# ---------------------------------------------------------------------------
# 问题二：零质量 / 零惯量
# ---------------------------------------------------------------------------