DemoRobot URDF/Xacro 建模与 Gazebo Fortress+ ros_gz + gz_ros2_control 联调

关联：索引

• 推荐系统：Ubuntu 22.04 + ROS2 Humble（课程统一口径）。
• 推荐软件：Gazebo Fortress（ign gazebo）+ ros_gz（Humble 对应 Fortress）+ gz_ros2_control + ros2_control/ros2_controllers。
• 本使用的工程路径（示例）：~/ros2_ws/src/demo_robot_description（你也可以放在任意工作空间，关键是构建后能 ros2 launch）。

0. 环境自检：先确认“命令在”

ros2 --version
ign gazebo --versions
ros2 pkg list | grep -E "ros_gz|gz_ros2_control|ros2_control|ros2_controllers|xacro" || true

逐行解释：

• ros2 --version：确认 ROS2 环境已 source（否则后续命令会提示找不到）。
• ign gazebo --versions：确认 Gazebo Fortress 可用，并输出 Gazebo Sim 及依赖版本（用于排错对齐）。
• ros2 pkg list | grep ...：确认核心包存在；如果没找到，优先安装对应的 ROS2 二进制包（见下一节）。

sudo apt update
sudo apt install -y \
  ros-humble-xacro \
  ros-humble-robot-state-publisher \
  ros-humble-joint-state-publisher \
  ros-humble-rviz2 \
  ros-humble-ros-gz \
  ros-humble-gz-ros2-control \
  ros-humble-ros2-control \
  ros-humble-ros2-controllers

逐行解释：

• ros-humble-xacro：提供 xacro（或 ros2 run xacro xacro）用于把 .urdf.xacro 展开成 URDF。
• robot_state_publisher：读取 robot_description（URDF 字符串）与 /joint_states，发布 TF（让 RViz 能显示 RobotModel）。
• joint_state_publisher：在没有真实硬件/控制器时，提供可调的 JointState（用于纯展示）。
• rviz2：可视化 RobotModel、TF、LaserScan、Image 等。
• ros-humble-ros-gz：ROS2 与 Gazebo Fortress 的集成元包（含 ros_gz_sim 与 ros_gz_bridge 等常用组件）。
• ros-humble-gz-ros2-control：Gazebo Fortress 中的 gz_ros2_control（让 Gazebo Sim 里的关节成为 ros2_control 的硬件接口）。
• ros2-control/ros2-controllers：控制器管理器与常见控制器（DiffDrive、JointStateBroadcaster 等）。

目标：把“能跑起来的 Xacro”变成“RViz 里看得见的 RobotModel”。
关键结论：RViz 看见 RobotModel 的前提不是 Gazebo，而是 robot_description + TF。

1) 本章代码结构：demo_robot_description 是一个“描述包”

目录位置：05_demo_robot_sim/ros2_ws/src/demo_robot_description
你需要先建立整体地图（不要一上来就改代码）：

• urdf/：URDF 与 Xacro 文件（机器人结构、惯量、传感器、插件标签等）。

• launch/：启动脚本（RViz 展示与 Gazebo 仿真）。

• config/：RViz 配置、ros2_control 控制器 YAML。

• world/：Gazebo world 与模型（仿真环境）。

• URDF：机器人结构“最终描述”（link/joint/inertial/visual/collision），更像“最终产品说明书”。

• Xacro：URDF 的“可复用模板语言”（include、macro、参数），更像“装配流水线 + 参数表”。

本章的组装入口是：
urdf/demo_robot/demo_robot.urdf.xacro：通过 xacro:include 引入底盘、轮子、传感器、插件等，再用宏调用把它们实例化。

3) RViz 展示链路：robot_state_publisher 做了什么

RViz 的 RobotModel 需要：

• robot_state_publisher：输入 robot_description（参数）+ /joint_states（话题），输出 TF（/tf 与 /tf_static）。
• joint_state_publisher：在没有真实控制器时，提供 /joint_states（让关节“有状态”）。

你只要把 robot_state_publisher 跑起来，TF 树就成立；RobotModel 只是 TF 的“可视化结果之一”。

0) 构建工作空间

假设你的工作空间是 ~/ros2_ws，并且 src 下已有 demo_robot_description：

cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

逐行解释：

• colcon build --symlink-install：构建并用软链接安装，方便你改 URDF/launch 后无需重复拷贝。
• source install/setup.bash：让当前终端能找到 demo_robot_description 的 share 资源与 launch 文件。

1) 先跑“最小机器人”：URDF 演示机器人（用于理解 Xacro）

本包提供了两个版本：

• 纯 URDF：urdf/urdf_demo_robot.urdf

启动 RViz 展示（推荐使用 Xacro 文件）：

ros2 launch demo_robot_description display_robot.launch.py \
  model:=$(ros2 pkg prefix demo_robot_description)/share/demo_robot_description/urdf/urdf_demo_robot.urdf.xacro

逐段解释：

• ros2 launch ... display_robot.launch.py：启动 RViz 展示链路（joint_state_publisher + robot_state_publisher + rviz2）。
• model:=...urdf_demo_robot.urdf.xacro：把 Xacro 路径传给 launch；launch 内部用 xacro <path> 生成 robot_description。该文件的 <robot name="..."> 为 urdf_demo_robot（避免“first/第一个”这种含义不清的命名）。

2) 证据链自检：确认 TF 与 RobotModel 成功

ros2 topic list | grep -E "^/tf$|^/tf_static$|^/joint_states$"

解释：

• /joint_states：来自 joint_state_publisher。
• /tf /tf_static：来自 robot_state_publisher；一旦存在，RViz 的 RobotModel 才可能正确显示。

可选更强自检（看 TF 帧名是否符合预期）：

ros2 topic echo /tf --once

解释：

• --once：只打印一次后退出，避免刷屏。
• 你应该能在输出中看到 base_link、imu_link 等帧名（取决于模型）。

四、练习（至少 2 题，含提示）

1. 练习 1：改一个参数，看 RobotModel 变化
   要求：在 urdf/urdf_demo_robot.urdf.xacro 中，把 <xacro:base_link length="0.12" radius="0.1" /> 的 length 改为 0.18，重新启动 RViz。
   提示：Xacro 的参数变化会直接改变最终 URDF 的 <cylinder length="...">。
2. 练习 2：增加一个 IMU 实例，理解“宏复用”
   要求：在 urdf/urdf_demo_robot.urdf.xacro 中再添加一个 <xacro:imu_link imu_name="imu_left" .../>，位置放到 0.05 0.05 0.02，并验证 TF 帧名出现 imu_left_link。
   提示：宏的 imu_name 会拼接进 link/joint 名字；重复命名会报错（URDF 不允许同名 link/joint）。

• 目标：能把 demo_robot.urdf.xacro 的 include 与宏调用画成“装配流程图”，并标注每个子模块产出的 link/joint。
• 步骤：

1. 打开 urdf/demo_robot/demo_robot.urdf.xacro，列出 include 顺序（base、sensor、actuator、plugins、ros2_control）。
2. 对照每个子文件（base/wheel/caster/imu/laser/camera），整理它们新增的 link/joint 名称。

• 给出 1 张结构图（手画/电子皆可）。

• 能回答：为什么 camera_optical_link 需要一个固定关节并带旋转（光学坐标系约定）。

• 提交物：

• RViz 截图 1 张：包含 RobotModel + TF（或能证明 TF 正常的界面信息）。

• 终端证据 1 份：ros2 topic list 过滤出 /tf /tf_static /joint_states 的输出。

• RobotModel 可见、无明显错位（不会所有 link 都堆在原点）。

• 终端证据可复现：同学换一台机器按步骤能得到同类输出。

七、大模型任务（给 AI 的指令模板 + 期望输出 + 校验点）

指令模板（直接复制给 AI）：

Xacro 文件路径与关键片段如下：

• urdf/demo_robot/demo_robot.urdf.xacro（include 与宏调用顺序）
• urdf/demo_robot/sensor/camera.urdf.xacro（camera_link/camera_optical_link）

期望输出：

• 一个表格：模块 → 新增 link → 新增 joint → 关键 frame_id。
• 一段风险清单：至少列出 3 个常见坑（重名/坐标系/路径解析）。

校验点（你必须自己验证，不盲信 AI）：

• AI 给出的 link/joint 名称必须能在源文件中逐一对应到 <link name="..."> 或 <joint name="...">。

八、小结与常见错误

• 常见错误 1：ros2 launch 找不到包
  原因：忘记 source install/setup.bash。
  验证：ros2 pkg list | grep demo_robot_description。
• 常见错误 2：RViz 打开但 RobotModel 空白
  原因：robot_state_publisher 没拿到正确的 robot_description（xacro 执行失败/模型路径错误）。
  验证：终端是否有 xacro 报错；ros2 topic list 是否有 /tf。
• 常见错误 3：RViz 配置文件路径不对
  本目录中 RViz 配置文件为 config/rviz/display_model.rviz，并且已在 display_robot.launch.py 中对齐为同一路径。\

目标：把“RViz 里看得见的机器人”推进到“Gazebo 里跑得动、ROS2 里拿得到数据”。
关键结论：Gazebo Fortress（Gazebo Sim）里动起来常见两条路：

1. Gazebo 原生运动插件 + ros_gz_bridge：Gazebo 内部接收 Gazebo Transport 的速度指令（需要桥接 Gazebo Transport 话题，工程复杂度更高）。
2. gz_ros2_control + ROS2 控制器（本章主线）：Gazebo Sim 作为“硬件接口”，ROS2 的 DiffDriveController 原生订阅其私有速度话题（例如 ~/cmd_vel_unstamped），并发布 ~/odom 与 /tf。为让学生统一使用标准话题，本工程在 launch 中增加了一个 topic relay：把 /cmd_vel 转发到控制器话题（见第三部分自检命令）。

1) 仿真链路总览：从 robot_description 到控制器

本章启动文件：launch/gazebo_sim.launch.py（简化描述为 6 步）：

1. Xacro 展开 → 生成 robot_description（参数）
2. 设置 Gazebo 资源路径（IGN_GAZEBO_RESOURCE_PATH / GZ_SIM_RESOURCE_PATH）并启动 Gazebo Fortress（内部命令是 ign gazebo，由 ros_gz_sim 的 gz_sim.launch.py 拉起）加载 world
3. ros_gz_sim create 从 ROS 参数 robot_description 生成并插入实体 demo_robot（launch 内部对应 -param robot_description）
4. URDF 中的 <plugin filename="gz_ros2_control-system" ...> 启动 controller_manager（运行在 Gazebo 进程内）
5. 用 controller_manager spawner 激活控制器（JointStateBroadcaster、DiffDriveController）
6. 用 ROS2 发布 /cmd_vel，验证机器人在 Gazebo 中运动

补充说明（为什么要设置资源路径）：

• world 文件与它引用的模型资源（例如 world/room/model.sdf）需要 Gazebo 能搜索到对应目录。
• 本工程在 launch 中把包内 world/ 目录追加到资源搜索路径，避免出现“world 能打开但模型/材质找不到”的问题。

你需要记住的“证据点”：

• Gazebo 里是否出现实体 demo_robot
• ROS2 里是否出现 /controller_manager
• ros2 control list_controllers 是否能看到控制器 active
• /cmd_vel 发布后，Gazebo 里机器人是否运动

2) ros2_control 配置文件：YAML 与 URDF 插件如何对应

本章配置文件：config/demo_robot_ros2_controller.yaml
关键结构分两层：

• controller_manager.ros__parameters：注册“有哪些控制器”以及 update_rate

• 具体控制器名.ros__parameters：该控制器的业务参数（轮子 joint 名称、轮距、半径、坐标系等）

• 控制器名必须一致：launch 中 load_controller 的名字，必须与 YAML 中的键一致。

• 关节名必须一致：YAML 里的 left_wheel_joint/right_wheel_joint 必须在 URDF/Xacro 中真实存在。

3) 传感器插件：Gazebo 内部传感器 → ROS2 消息

本目录的传感器配置采用“Gazebo 传感器原生输出 + ros_gz_bridge 桥接”的思路：

• URDF/Xacro 里写 <gazebo reference="..."><sensor ...><topic>...</topic>...</sensor></gazebo>，让 Gazebo Sim 发布 Gazebo Transport 话题。
• launch 里启动 ros_gz_bridge parameter_bridge，把 Gazebo Transport 的话题桥接成 ROS2 的 sensor_msgs/*。

0) 启动仿真

cd ~/ros2_ws
source install/setup.bash
ros2 launch demo_robot_description gazebo_sim.launch.py

逐行解释：

• 仍然要 source install/setup.bash：否则找不到包资源（world/urdf/launch）。
• gazebo_sim.launch.py：启动 Gazebo + spawn demo_robot + 加载控制器（若一切正常）。

1) 自检 1：实体是否生成（Gazebo 可视化 + ROS2 侧证据）

Gazebo GUI 侧：应能在场景里看到 demo_robot。
ROS2 侧建议先看节点列表：

ros2 node list | grep -E "ros_gz|controller_manager|robot_state_publisher" || true

解释：

• ros_gz_* 相关节点出现：说明 ROS2-Gazebo 集成链路启动成功。
• controller_manager 出现：通常说明 gz_ros2_control 插件已加载。

2) 自检 2：控制器是否激活

ros2 control list_controllers

解释：

• 你应该看到 demo_robot_joint_state_broadcaster 与 demo_robot_diff_drive_controller 至少一个处于 active。
• 如果是 inactive/unconfigured，优先看 launch 的 load_controller 是否执行成功（终端日志会显示）。

ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

逐段解释：

• -r 10：以 10Hz 持续发布；DiffDrive 通常需要持续速度指令，单次发布可能立刻超时停下（取决于 cmd_vel_timeout）。
• linear.x：前进速度（m/s）；angular.z：角速度（rad/s）。

停止方法（两种任选其一）：

• 在发布终端 Ctrl+C 停止话题发布。
• 再发一个全 0 的 Twist。

4) 自检 4：至少跑通 1 个传感器话题（推荐 /demo_robot/scan）

ros2 topic list | grep -E "^/demo_robot/scan$|^/demo_robot/imu$|camera|image" || true
ros2 topic echo /demo_robot/scan --once

解释：

• /demo_robot/scan 存在且能 echo --once 输出：说明 Gazebo 传感器已工作，且 bridge 正常把数据转换为 ROS2 消息。
• 若 /demo_robot/scan 不存在：回到“常见错误排查”按顺序定位（不要直接改一堆参数）。

四、练习（至少 2 题，含提示）

1. 练习 1：控制器参数对现象的影响（轮距/半径）
   要求：在 config/demo_robot_ros2_controller.yaml 中把 wheel_separation 改大（例如 0.17 → 0.22），让机器人原地转圈（angular.z=0.5）观察转向半径变化。
   提示：轮距变大通常会导致同样角速度下转向表现变化；你要记录“参数→现象”的证据。
2. 练习 2：传感器频率与噪声对数据的影响（Laser）
   要求：在 urdf/demo_robot/plugins/gazebo_sensor_plugin.xacro 中把激光 <update_rate> 从 10 调到 15，并在 ros2 topic hz /demo_robot/scan（可选）观察频率变化。
   提示：先确保 /demo_robot/scan 能正常输出，再做频率实验；不要在“还没跑通”时调参。

工坊主题：完成一次“可控可感知”的闭环联调，并修复 2 个工程常见坑

工坊 A（推荐，全班做）：核对 RViz 配置文件路径是否正确

• 背景：RViz 默认配置文件应位于 config/rviz/display_model.rviz，launch 里通过 -d 参数加载它。
• 目标：学生能定位并确认“配置文件路径正确 + RViz 能按预设加载 Displays”。

工坊 B（进阶，小组挑战）：做一次“可审计的 Xacro 展开 + 控制器/桥接最小联调”

• 背景：本目录已按 Humble 口径统一为 $(find-pkg-share demo_robot_description)，并已修复传感器 Xacro 的 XML 结构问题；因此本工坊从“修 Bug”升级为“做证据链”。
• 目标：让 xacro 稳定展开；Gazebo 中实体生成；控制器 active；至少 /demo_robot/scan 可读。

1. xacro ... > /tmp/demo_robot.urdf 成功落盘
2. ros2 control list_controllers 显示 active
3. ros2 topic echo /clock --once 有输出
4. ros2 topic echo /demo_robot/scan --once 有输出

用于自检的展开命令（把 Xacro 输出成 URDF 纯文本）：

xacro $(ros2 pkg prefix demo_robot_description)/share/demo_robot_description/urdf/demo_robot/demo_robot.urdf.xacro > /tmp/demo_robot.urdf
head -n 20 /tmp/demo_robot.urdf

逐行解释：

• xacro ... > /tmp/demo_robot.urdf：把展开结果落盘，便于你检查最终有没有 <ros2_control>、有没有 <gazebo> 插件段、link/joint 名称是否出现。

• head -n 20：先快速检查文件确实生成且是 URDF 头部（避免输出被报错信息污染）。

• 视频/动图 1 份：Gazebo 中 demo_robot 能响应速度指令前进或转向。

• 终端证据 2 份：

• ros2 control list_controllers 输出（显示 active）。

• ros2 topic echo /demo_robot/scan --once（或 /demo_robot/imu --once）输出截图。

• 控制与传感至少各跑通 1 条链路（例如 /cmd_vel + /demo_robot/scan）。

• 证据链能解释：话题来自哪个插件/控制器、frame_id 是什么、与 TF 是否一致。

七、大模型任务（给 AI 的指令模板 + 期望输出 + 校验点）

指令模板（直接复制给 AI）：

你是 ROS2 Humble + Gazebo Fortress（ign gazebo）联调专家。请审计下面的 URDF/Xacro 设计，给出“最小修改方案”，让它满足：

1. xacro 能在 ROS2 Humble 下展开（路径替换正确）
2. gz_ros2_control 插件能读取控制器 YAML
3. 至少 /demo_robot/scan 能发布到 ROS2
   请输出：修改后的关键片段（只贴需要改的段落），并解释每一处修改为什么必要、会影响什么证据点（topic/node/controller）。
   输入文件：

• urdf/demo_robot/demo_robot.urdf.xacro
• urdf/demo_robot/demo_robot.ros2_control.xacro
• urdf/demo_robot/plugins/gazebo_sensor_plugin.xacro

期望输出：

• “改动清单”：每项包含文件名、原片段、改后片段、验证命令。
• “验证路径”：从 xacro 展开 → ros2 launch → ros2 control list_controllers → ros2 topic echo /demo_robot/scan 的顺序化检查表。

校验点（你必须自己验证，不盲信 AI）：

• 任何路径替换必须在你的系统下真实存在（用 ls 或 ros2 pkg prefix 验证）。
• AI 若建议新增依赖包，必须给出可安装的 Ubuntu/ROS2 包名，并解释其作用。

八、小结与常见错误（按排查优先级排序）

1. xacro 解析失败（最常见、也最该先解决）
   症状：launch 日志里出现 “xacro: error …” 或 robot_description 为空。
   排查顺序：

• 先单独跑 xacro ... > /tmp/demo_robot.urdf，让错误最小化可见。
• 再检查 include 路径替换：优先把 $(find ...) 统一为 $(find-pkg-share ...)（ROS2 口径）。

1. Gazebo 启动成功但没有 demo_robot 实体
   症状：world 有，机器人没有。
   排查：

• 看 ros_gz_sim create 是否报错（通常是 robot_description 没生成或 XML 非法）。
• 确认 launch 中使用的参数：-param robot_description 与实体名 demo_robot。

1. 控制器加载失败
   症状：ros2 control list_controllers 看不到控制器或状态非 active。
   排查：

• gz_ros2_control 插件是否加载（看是否出现 /controller_manager）。
• YAML 里的控制器名与 load_controller 命令是否一致（完全匹配）。

1. /demo_robot/scan 或 /demo_robot/imu 不发布
   症状：话题不存在或 echo 无输出。
   排查：

• ros_gz_bridge 是否在跑（桥接不工作时，ROS2 侧看不到 /demo_robot/scan / /demo_robot/imu）。
• Gazebo 侧传感器的 <topic> 是否与 launch 中 ros_gz_bridge parameter_bridge 的 arguments 完全一致（本工程直接用 /demo_robot/scan、/demo_robot/imu 等话题名，不依赖 remapping）。
• frame_name 是否与 URDF 中的 link 名称一致（例如 laser_link）。

课后作业

参考与延伸（官方/通用资料）

• ROS2 Humble：ros2_control 概念与控制器生态（官方文档）
• https://control.ros.org/master/doc/ros2_control/doc/index.html
• Gazebo Fortress 与 ROS2 互操作（ros_gz_bridge / ros_gz_sim）
• https://gazebosim.org/docs/ionic/ros2_integration/
• gz_ros2_control（Gazebo Sim 与 ros2_control 集成）
• https://control.ros.org/humble/doc/gz_ros2_control/doc/index.html
• Xacro（宏与路径替换语法）
• http://wiki.ros.org/xacro

自检清单（交付前必做）

Markdown 自检（本已执行）

• 标题层级连续：# → ## → ### → ####，无跳级。
• 列表缩进一致：所有列表均使用 -，未混用编号列表导致渲染错位。
• 代码块闭合：所有 fenced code block 均成对闭合，且带语言标识（bash）。
• 链接为完整 URL：参考链接均为完整可访问格式。

命令与代码自检（基于本目录内容的静态核对）

• 涉及的文件路径在目录中存在：
• launch/display_robot.launch.py
• launch/gazebo_sim.launch.py
• config/demo_robot_ros2_controller.yaml
• urdf/urdf_demo_robot.urdf.xacro
• urdf/demo_robot/demo_robot.urdf.xacro
• world/custom_room.world
• /clock 未桥接或节点未设置 use_sim_time，会导致 TF/时间相关功能异常（最典型是“仿真暂停后节点还在走 wall time”或“时间戳在未来/过去”）。