【ROS2】Gazebo 仿真
1.1 什么是Gazebo
Gazebo机器人专用的 3D 物理仿真平台，能加载 URDF/SDF 机器人模型，并结合 ROS/ROS2 实现 虚拟机器人测试。
Gazebo在虚拟环境中搭建机器人、传感器和场景，用物理引擎（如 ODE、Bullet、DART）来计算运动学、动力学、碰撞检测。
1.1 Gazebo 的功能
- 物理仿真
- 支持重力、摩擦力、碰撞、关节约束
- 可以真实地模拟机器人在环境中的运动
- 传感器仿真
- 提供虚拟的 激光雷达 (LiDAR)、相机 (Camera)、深度相机 (RGB-D)、IMU、GPS 等
- 模拟真实传感器的数据输出，方便算法测试
- 机器人建模
- 通过 URDF/Xacro/SDF 文件导入机器人模型（如 TurtleBot、机械臂、无人机等）
- 环境建模
- 可以创建复杂的场景（建筑物、地形、障碍物等），进行导航和 SLAM 测试
- 与 ROS 集成
- 可以把 Gazebo 作为 虚拟机器人实验环境
- ROS 节点可以直接订阅/发布来自 Gazebo 的话题（例如 /cmd_vel 控制小车，/scan 获取激光数据）
1.2 为什么要用 Gazebo？
- 节省成本：不用真实机器人也能开发/调试算法
- 安全：在虚拟环境中调试复杂动作，避免损坏真实硬件
- 快速迭代：更容易测试不同的机器人参数、传感器配置
1.3 硬件要求
由于3D 仿真，一般需要在带有英伟达显卡的X86 机器上运行 才能满足性能。官方要求 ：Ampere或更高版本的NVIDIA GPU架构，配备8GB或更高显示RAM。
我们先从安装演示包及其依赖项开始。
sudo apt install ros-humble-urdf-sim-tutorial
或者该使用github 仓库https://github.com/ros/urdf_sim_tutorial/tree/ros2代码。
2 从一个简单的实例开始
2.1 将一个“死”的模型加载进在Gazebo 中
我们可以使用 gazebo.launch.py 将已创建的模型生成到 Gazebo 中。
ros2 launch urdf_sim_tutorial gazebo.launch.py
这个启动文件会：
- 启动 Gazebo 仿真服务
- 读取 URDF/Xacro 文件，生成 robot_description 参数，提供给仿真/可视化节点使用。
- 订阅 /robot_description 参数，在 Gazebo 生成机器人实体。
默认情况下，Gazebo GUI 也会显示，其界面如下：
然而，它什么也没做，而且缺少很多 ROS 使用这个机器人所需的关键信息。这只是一个“死” 模型放进了Gazebo中。 它并不能和ROS 进行交互。
目前，Gazebo 只会显示机器人模型，但：
- 不会自动移动关节
- 不会发布传感器数据
- ROS 无法控制机器人
要让机器人可以与您和 ROS 进行交互，我们需要指定两样东西：插件（Plugins） 和 控制器（Controllers）。
2.1 Gazebo Plugins（插件）
在 Gazebo 中，插件是用于 扩展机器人模型功能的程序模块。插件可以：
- 控制关节运动
- 发布传感器数据（如激光雷达、IMU、相机）
- 响应 ROS 消息或服务
- 添加自定义逻辑（例如碰撞检测、控制算法）
插件是仿真里的“虚拟硬件驱动”，负责把物理动作/传感器数据暴露给 ROS。
插件就是“Gazebo/机器人与 ROS 之间的桥梁”，实现交互功能。
例如，常见插件， gazebo_ros_control：让机器人关节可以被 ROS 控制。 gazebo_ros_camera：发布相机图像到 ROS topic。 gazebo_ros_joint_state_publisher：发布关节状态。
2.1.1 gazebo_ros_control
gazebo_ros_control 是 ROS 2 中的一个 桥接插件，作用是将 ROS2世界 （ROS 2 中运行的 ros2_control 控制器）与 Gazebo 世界的仿真机器人连接起来，实现仿真中的机器人关节控制和状态反馈。
它主要解决两个问题：
- 仿真控制：在 Gazebo 中控制机器人关节（位置、速度、力/扭矩）。
- 状态反馈：把仿真机器人关节状态（位置、速度、力）发布到 ROS 2，使控制器能读取。
gazebo_ros_control就是一个库， 这个库就像一个“翻译官”，把 ROS 发出的命令（比如移动关节、读取传感器）翻译成 Gazebo 能理解的动作。
2.1.2 如何申明使用gazebo_ros_control 插件？
要使用gazebo_ros_control 插件，需要在URDF/Xacro 中进行申明。我们在 URDF 文件中 紧接着关闭的 标签之前，添加以下内容：
<!-- 用于告诉 Gazebo 仿真器：这个 URDF 模型要加载额外的 Gazebo 插件。-->
<gazebo>
<!-- filename指定要加载的 插件库文件，这里是 Gazebo 提供的 ROS 2控制插件。-->
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<!--
1.指定gazebo_ros2_control插件加载的YAML 配置文件
2. 让 GazeboSystem 知道如何按照 ROS 2 控制逻辑去控制机器人模型-->
<parameters>$(find urdf_sim_tutorial)/config/09a-minimal.yaml</parameters>
</plugin>
</gazebo>
任何在 内部声明的插件都会在 Gazebo 启动模型时自动加载。
插件加载的YAML 配置文件本身描述的是 ros2_control （后面会讲）的 ControllerManager 配置，包括：
- 控制器类型（JointTrajectoryController、JointStateBroadcaster 等）
- 控制哪些关节
- 命令接口 / 状态接口
- 更新频率
它不是 Gazebo 的插件配置，Gazebo/插件只负责读取和加载，但并不解析具体控制逻辑。
gazebo_ros2_control 的角色
gazebo_ros2_control 插件做的事情可以理解为：
- 加载启动 ControllerManager
- 插件在加载时会启动（不是创建） ROS 2 的 ControllerManager 节点（这是 ros2_control 的核心节点）
- ControllerManager 本身负责解析 YAML，创建控制器实例
- 绑定仿真关节
- 插件把 ControllerManager 创建的控制器和 Gazebo 仿真关节绑定。
- 插件在 Gazebo 仿真循环中把控制器命令作用到关节，并把状态回写给 ControllerManager。
- 控制器仍然由 ros2_control 管理，插件不直接创建控制器
其简化流程大致如下：
[Gazebo 启动]
↓
[插件加载]
↓
（启动 ControllerManager 节点） ← 插件做的事
↓
（ControllerManager 根据 YAML 创建控制器实例） ← ros2_control 做的事
↓
（控制器命令 → 插件 → 仿真关节）
↑
（仿真状态 → 插件 → 控制器 → ROS 2 topic）
2.2 Controllers（控制器）
前面讲了Controllers 是由ControllerManager 根据gazebo_ros2_control 传入的yaml 创建的。而ControllerManager 又是ros2_control核心节点。 所以我们先来了解ros2_control。
2.2.1 ros2_control
ros2_control 是 ROS 2 官方的硬件抽象层（Hardware Abstraction Layer, HAL）和控制框架，它的作用是：
- 统一接口：让控制器（controllers）可以用同样的方式操作机器人关节，不管底层是 真实硬件 还是 仿真环境。
- 模块化控制器：你可以加载不同类型的控制器，比如：
- JointStateBroadcaster（读取关节状态）
- PositionJointController（位置控制）
- VelocityJointController（速度控制）
- EffortJointController（力矩控制）
ros2_control中的核心概念&架构
┌─────────────┐
│ ROS 2 Nodes │
└─────┬───────┘
│ ROS 2 topics / actions
v
┌───────────────────┐
│ ControllerManager │ ← 核心节点
└───┬───────────────┘
│ manages
v
┌─────────────┐ ┌─────────────┐
│ Controller 1│ │ Controller 2│
└─────┬───────┘ └─────┬───────┘
│ │
│ commands │ commands
v v
┌─────────────────────────────────┐
│ Hardware Interface (Abstract HW)│
│ - position, velocity, effort │
│ - reads joint states │
└─────────────────────────────────┘
│
v
┌───────────────┐
│ Robot Hardware│
└───────────────┘
概念详解如下：
- ControllerManager
- 创建/启动/停止控制器
- 调度控制循环（根据 update_rate）
- 提供 ROS 2 接口：Topic/Service/Action
- Controllers
- 类型：
- ointTrajectoryController：轨迹控制
- JointPositionController：单关节位置控制
- JointVelocityController：速度控制
- JointEffortController：力/扭矩控制
- JointStateBroadcaster：发布关节状态到 /joint_states
- 每个控制器需要知道：
- 哪些关节被控制
- 命令接口类型
- 状态接口类型
- 类型：
- Hardware Interface
- 抽象硬件 API，必须实现：
- read()：读取关节状态
- write()：写入控制命令
- 常用实现：
- 仿真： GazeboSystem是由 gazebo_ros2_control 插件提供的一个类，实现了 ros2_control 的 HardwareInterface 接口：
- read()：从 Gazebo 里读取当前关节角度、速度
- write()：把控制器输出的命令写入 Gazebo 仿真模型
- 真实硬件：基于 EtherCAT, serial, CAN 总线等
- 仿真： GazeboSystem是由 gazebo_ros2_control 插件提供的一个类，实现了 ros2_control 的 HardwareInterface 接口：
- 抽象硬件 API，必须实现：
- Robot Hardware
- 实际机器人或仿真关节
- 前面讲的 插件 就是ros2_control 的一个插件
- gazebo_ros2_control 是 ros2_control 的一个 “硬件接口插件（Hardware Interface Plugin）”。
2.2.2 如何申明使用ros2_control？
和gazebo_ros2_control 申明方式一样，一般需要在 URDF 文件中 紧接着关闭的 标签之前。通常在紧贴 之前。其格式如下：
<!-- 声明这个机器人模型要使用 ROS 2 Control。
GazeboSystem：给硬件接口/控制系统起名
类型为 “system”，通常用于 Gazebo 仿真系统级别的硬件接口-->
<ros2_control name="GazeboSystem" type="system">
<!-- 硬件接口-->
<hardware>
<!-- 使用 Gazebo 插件GazeboSystem作为硬件接口。GazeboSystem是 gazebo_ros2_control 插件提供的 Hardware Interface 实现。
gazebo_ros2_control/GazeboSystem让 Gazebo 模拟的机器人「假装」成真实硬件。-->
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<!-- 声明了一个机器人关节（例如头部旋转关节）将由此控制系统管理。-->
<joint name="head_swivel" />
</ros2_control>
gazebo_ros2_control 插件创建并启动 ControllerManager。 在 ControllerManager 初始化过程中，ros2_control 会解析 <ros2_control> 标签并动态加载 hardware_interface 插件（也就是 gazebo_ros2_control/GazeboSystem）。
2.2.3 ros2_control 配置YAML
章节2.2.1 已经描述过，gazebo_ros2_control 插件加载了这个配置yaml， 传递给ControllerManager（ros2_control）用的配置：告诉它 update_rate、要创建哪些控制器（类型、绑定哪些关节、需要哪些接口），以及硬件接口（hardware）需要的参数。我们来先看一个demo:
controller_manager: #固定键（ControllerManager 的参数命名空间）
ros__parameters: #符合 ROS 2 参数声明风格
update_rate: 100 #控制循环频率（Hz），ControllerManager 调度控制回路的频率。
# 控制器（controllers） ...
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
joints: [joint1, joint2]
my_position_controller:
type: joint_trajectory_controller/JointTrajectoryController
joints: [joint1, joint2]
command_interfaces: [position]
state_interfaces: [position]
update_rate
控制循环频率（read → update controllers → write）
- 类型：整数（Hz）
- 建议：仿真常用 100，速度控制或高频控制可能需要更高。
2.2.3 控制器( Controller)
每个控制器以一个 key（名字）出现，其值包含若干字段：
- type：控制器类型（package/ControllerClass，例如 joint_state_broadcaster/JointStateBroadcaster 或 joint_trajectory_controller/JointTrajectoryController）。
- **joints：控制的关节列表（数组）**这些关节的名称 必须与 URDF 模型中定义的 joint 名称完全一致。
- command_interfaces：控制命令接口（如 position, velocity, effort）。
- state_interfaces：状态接口（如 position, velocity, effort）。
- 其它 controller 特定参数（见具体控制器文档）。
控制器如何使用这些 joints
控制器类型不同，处理方式也不同，但逻辑一致：
| 控制器类型 | 作用 | joints 的意义 |
|---|---|---|
JointStateBroadcaster |
读取状态并发布 /joint_states |
从 hardware 读取这些关节的位置、速度、力矩 |
JointTrajectoryController |
接收轨迹命令并控制关节位置/速度 | 控制这些关节的运动 |
EffortController |
发送力矩命令 | 对这些关节施加 effort |
ForwardCommandController |
直接写入命令 | 把 command 写入这些 joints 对应的接口 |
我们给出一个样例，两个关节的控制器：
arm_controller:
# 这是一个 位置控制器。
type: joint_trajectory_controller/JointTrajectoryController
joints:
# 它控制 shoulder_joint 和 elbow_joint 两个关节。
- shoulder_joint
- elbow_joint
# 它会向硬件（或仿真中的 GazeboSystem）发送位置命令。
command_interfaces: [position]
#控制器需要从硬件接口中读取的 位置 和 速度 状态。
state_interfaces: [position, velocity]
画成图如下：
┌──────────────────────────────┐
│ Gazebo (仿真世界) │
│ 模拟机械臂、物理动力学 │
└──────────────┬───────────────┘
│
│ 提供/接收关节状态与命令
│
┌──────────────▼───────────────┐
│ gazebo_ros2_control 插件层 │
│ plugin: gazebo_ros2_control/ │
│ GazeboSystem │
│ ← 实现 Hardware Interface │
└──────────────┬───────────────┘
│
state_interfaces: │ command_interfaces:
position, velocity │ position
│
┌─────────────────────────────┴──────────────────────────┐
│ ros2_control Controller Manager │
│--------------------------------------------------------│
│ # Controller │
│ arm_controller (JointTrajectoryController) │
│ ├─ joints: shoulder_joint, elbow_joint │
│ ├─ 从 GazeboSystem 读取状态 (position, velocity) │
│ └─ 发送控制命令 (position) 到 GazeboSystem │
│ │
└────────────────────────────────────────────────────────┘
│
│ ROS 2 话题接口
│ (发布状态 / 订阅轨迹)
▼
┌───────────────────────────────────┐
│ ROS 2 Topics 层 │
│ /joint_states (发布状态) │
│ /arm_controller/joint_trajectory │
│ (接收 MoveIt / Planner 轨迹) │
└───────────────────────────────────┘
其数据流如下运行：
- Controller Manager (简称CM) 定时调用每个 Controller 的 update()。
- Controller 从 HardwareInterface 获取关节状态（state_interfaces）。
- Controller 根据控制算法计算命令，写入 HardwareInterface（command_interfaces）。
- 硬件接口把命令作用到关节或 Gazebo 模型上。
- 下一次循环继续重复。
2.2.4 关节( Joint)
在 URDF（Unified Robot Description Format）中，joint 表示机器人中两个连杆（link）之间的连接关系。Gazebo 和 ros2_control 都通过它了解：
- 这个关节的类型（转动、滑动等）；
- 它的运动范围（最大/最小角度或位移）；
- 控制方式（由谁来驱动、是否受力限制等）。
我们已经研究了如何在URDF 中定义一个关节，这里不再赘述。但是ros2_control并不知道这些信息。ros2_control不能直接从 URDF 自动读取所有关节，虽然可以解析 URDF 获取基本信息，但它只使用 <ros2_control> 标签中指定的硬件接口和关节进行控制。 所以，我们必须要在 <ros2_control> 中声明我们使用的关节。
声明我们要使用的关节( Joint)
在 URDF 中这样声明：
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="head_swivel" />
</ros2_control>
<ros2_control> 中的 <joint> 不是重新定义（之前在URDF的其它地方已经被定义），而是告诉 ros2_control 我要控制这个关节。
<joint> 表示 Hardware Interface 会管理这个关节的状态接口（position/velocity/effort）以及命令接口（position/velocity/effort）。可以在 内显式指定接口：
<joint name="head_swivel">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
- command_interface：控制器发送命令的接口
- state_interface：控制器读取状态的接口
- 如果省略，ros2_control 会使用默认接口。
常见的命令&状态接口
position、velocity、effort 等 就是约定好的标准名字，控制器和 Hardware Interface 都知道这些名字代表什么。
command_interface（控制命令接口)
用于向硬件或仿真关节发送命令。常见的有：
| 接口名称 | 说明 | 常用场景 |
|---|---|---|
position |
控制关节目标位置（角度或位移） | JointPositionController, JointTrajectoryController |
velocity |
控制关节速度 | VelocityJointController |
effort / torque |
控制关节力矩或力 | EffortJointController, 力控 |
position_raw |
原始位置命令，绕过内部滤波 | 高级应用，通常不常用 |
velocity_raw |
原始速度命令 | 高级应用 |
effort_raw |
原始力矩/力命令 | 高级应用 |
state_interface（状态接口）
用于让控制器读取关节当前状态。常见的有：
| 接口名称 | 说明 | 常用场景 |
|---|---|---|
position |
当前关节位置 | 所有位置控制器都需要 |
velocity |
当前关节速度 | 轨迹控制、速度控制器 |
effort / torque |
当前关节力矩或力 | 力控、PID 控制反馈 |
acceleration |
当前关节加速度 | 高级控制器或动力学估计 |
temperature |
关节/电机温度 | 安全监控 |
current |
电机电流 | 安全监控或高级控制 |
2.3 将Gazebo中的机器人“活”起来 。
有了以上 插件 和 控制器 的概念后， 让我们继续2.1 章节的死机器人的例子。要让Gazebo中的机器人 能动起来， 就需要将这个机器人和ROS2 的世界连接起来。 就需要我们上面提到两个概念。
2.3.1 将ROS2 世界和 Gazebo 世界连接起来
根据前面的概念， 为了让 Gazebo 机器人能按照预期的行为动作， 就需要ros2_control（ROS2 世界） + 它的插件gazebo_ros2_control （gazebo 世界）来完成对应动。我们需要在URDF 中来声明它。我们在 URDF 文件中 紧接着关闭的 标签之前，添加以下内容：
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="head_swivel" />
</ros2_control>
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find urdf_sim_tutorial)/config/09a-minimal.yaml</parameters>
</plugin>
</gazebo>
其中09a-minimal.yaml 的内容如下
controller_manager:
ros__parameters:
update_rate: 100
使用以下命令运行：
ros2 launch urdf_sim_tutorial 09a-minimal.launch.py
发现和死的差别并不大，因为这个yaml 并没有定义controller，也就没有controller 被启动。只是启用gazebo_ros2_control 插件。
通过给URDF 中添加以上内容，我们就将ROS2 世界和 Gazebo 世界连接起来了。
2.3.2 添加一个Controller
将ROS2 世界和 Gazebo 世界连接起来了，但没有controller，就无法控制关节，获取关节的状态，Gazebo 机器人就无法动起来。我们添加一个Controller其内容如下：
controller_manager:
ros__parameters:
update_rate: 100
use_sim_time: true
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
通过以上配置文档，我们创建了一个joint_state_broadcaster 的manager。该manager从 gazebo_ros2_control/GazeboSystem 中读取这些关节的位置、速度、力矩， 并将其状态并发布 /joint_states 的ROS2 topic。 这样gazebo 世界中的关节状态就可以在ROS2 世界中订阅了。
使用以下命令启动：
ros2 launch urdf_sim_tutorial 09-joints.launch.py
使用ros2 topic 监听功能，查看对应的topic：
$ ros2 topic echo /joint_states
header:
stamp:
sec: 13
nanosec: 331000000
frame_id: ''
name: []
position: []
velocity: []
effort: []
哇, joint_state_broadcaster 这个controller 确实将关节的状态的发布到了ROS2 世界。但细心的你以定发现，这个里面竟然没有数据，为什么呢？ 很简单，因为你的yaml 中并没有定义关节。所以控制器不能获取关节的状态，只能发布空消息。
2.3.2 添加一个关节（joint）
根据以前的概念， controller 的一个作用就是获取关节的状态，控制关节。
对于每一个非固定关节，我们需要在 ros2_control 标签中添加关于该关节的信息，以告诉它支持哪些接口。让我们从头部关节开始。将你的 URDF 中的关节标签修改为如下内容：
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="head_swivel">
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<!--
* 注意，这里的关节名称应与标准 URDF <joint> 标签中的关节名称一致。
* 目前，我们先关注 state_interface，在这里我们指定希望发布该关节的位置和速度信息。-->
你可以使用我们之前的启动配置来运行这个 URDF。
ros2 launch urdf_sim_tutorial 09-joints.launch.py urdf_package_path:=urdf/10-firsttransmission.urdf.xacro
现在，头部已经能在 RViz 中正确显示了，因为头部关节已被包含在 joint_states 消息中。
$ ros2 topic echo /joint_states
header:
stamp:
sec: 4
nanosec: 707000000
frame_id: ''
name:
- head_swivel
position:
- -2.9051283156888985e-08
velocity:
- 7.575990694887896e-06
effort:
- .nan
2.3.2 定义关节控制（Joint Control）
在ros2_control 标签中声明关节后，如果没有在controller的YAML 里没有指定 joints: […]，默认情况下，joint_state_broadcaster 依然会发布所有已注册关节的状态。已注册的关节指通过URDF中 <ros2_control> + 定义的 joints。
对于关节的state interface， controller 的 YAML 中不需要声明，Controller 会 自动读取硬件接口提供的所有 state interfaces。
但对于command interface， controller 的 YAML 中必须指定（同一个关节可能同时支持位置、速度、力矩控制，Controller 需要明确你想用哪一种），interface_name 指定的是 controller 向硬件发送命令的接口，也就是 command interface。
接下来，我们就在yaml 中定义一个名为 head_controller 的“关节组位置控制器，来指定我要控制的关节。并且说明我们要控制的是位置（position）。
controller_manager:
ros__parameters:
update_rate: 100
use_sim_time: true
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
head_controller:
#这是一个 位置控制器，可同时控制一组关节的位置
type: position_controllers/JointGroupPositionController
joints: #要控制的关节
- head_swivel
interface_name: position
之所以能这样做，是因为在 URDF 文件里的 标签中，我们已经写了：
<command_interface name="position" />
这表示这个关节支持“位置控制接口”，也就是可以接收目标位置的命令（例如旋转到某个角度）。
使用以下命令运行：
$ ros2 launch urdf_sim_tutorial 10-head.launch.py
position_controllers/JointGroupPositionController
JointGroupPositionController 是 ROS 2 控制器（ros2_control）库提供的一个 位置控制器（position controller），它控制一组关节的 位置（position）。其有以下功能：
- 接收ROS 2 目标关节位置控制消息
- 从硬件接口读取关节的当前状态（position、velocity，可选 effort）
- 根据控制算法（通常是 PID）计算命令（根据状态）
- 将计算出的命令写入硬件接口
ROS 2 的话题的常见topic
| 功能 | 话题名 | 消息类型 |
|---|---|---|
| 单关节控制 | /head_controller/commands |
std_msgs/msg/Float64MultiArray |
| 多关节轨迹 | /arm_controller/joint_trajectory |
trajectory_msgs/JointTrajectory |
了解了以上信息，我们知道JointGroupPositionController 订阅一个topic :/head_controller/commands 来接收position 。我们可以通过在 ROS 中发布一个值来控制机器人的头部位置。
ros2 topic pub /head_controller/commands std_msgs/msg/Float64MultiArray "data: [-0.707]"
当这个命令被发布时，关节的位置会立即变为指定的值。
至此，我们终于让 Gazebo中的机器人“活”起来了！！！
3. 控制多个关节与动作模仿
解下来，我们将使用类似的方法，修改夹爪（Gripper）关节的 URDF & ROS 参数，但在这种情况下，我们会将多个关节关联到同一个控制器。
URDF 的文件中添加以下内容：
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="head_swivel">
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 夹爪（gripper）伸缩 机构的关节-->
<joint name="gripper_extension">
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 夹爪左指头关节-->
<joint name="left_gripper_joint">
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 夹爪右指头关节-->
<joint name="right_gripper_joint">
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find urdf_sim_tutorial)/config/gripper.yaml</parameters>
</plugin>
</gazebo>
gripper.yaml 描述如下：
controller_manager:
ros__parameters:
update_rate: 100
use_sim_time: true
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
head_controller:
type: position_controllers/JointGroupPositionController
gripper_controller:
type: position_controllers/JointGroupPositionController
head_controller:
ros__parameters:
joints:
- head_swivel
interface_name: position
gripper_controller:
ros__parameters:
joints:
# 多个关节关联到同一个控制器
- gripper_extension
- left_gripper_joint
- right_gripper_joint
interface_name: position
启动机器人：
ros2 launch urdf_sim_tutorial 12-gripper.launch.py
3.1 如何控制gripper 呢？
首先我们来看下这个消息类型结构：std_msgs/msg/Float64MultiArray
Float64MultiArray是 ROS 2 的一个标准消息类型，用于发送多维浮点数组
$ ros2 interface show std_msgs/msg/Float64MultiArray
MultiArrayLayout layout # specification of data layout
MultiArrayDimension[] dim #
string label #
uint32 size #
uint32 stride #
uint32 data_offset #
float64[] data # array of data
- layout：描述数组的多维结构（行列数、步长等），一般在一维数组时可以忽略。
- data：真正的浮点数数组内容。
给出一个样例， data: [0.0, 0.5, 0.5] ，该data是一个长度为 3 的浮点数组，对应三个要控制的 关节命令值。根据上面的 URDF：
<joint name="gripper_extension" ... />
<joint name="left_gripper_joint" ... />
<joint name="right_gripper_joint" ... />
通常这三个浮点值分别对应于这三个关节的目标位置（position command），
顺序取决于 YAML 控制器配置文件（gripper.yaml）中 joints: 的定义。
根据上面的gripper.yaml 中的：
gripper_controller:
type: position_controllers/JointGroupPositionController
joints:
- gripper_extension
- left_gripper_joint
- right_gripper_joint
那么就表示：
| 数组索引 | 关节名称 | 命令值 | 物理含义 |
|---|---|---|---|
0 |
gripper_extension |
0.0 |
不伸出（夹爪底座缩回） |
1 |
left_gripper_joint |
0.5 |
左夹爪张开 0.5 弧度 |
2 |
right_gripper_joint |
0.5 |
右夹爪张开 0.5 弧度 |
所以gripper_controller 中 有几个joints， Float64MultiArray的data 长度就由几个。
让我们执行以下命令，让 爪夹 张开并伸出：
ros2 topic pub /gripper_controller/commands std_msgs/msg/Float64MultiArray "data: [0.4, 0.5, 0.5]"
闭合并收回：
ros2 topic pub /gripper_controller/commands std_msgs/msg/Float64MultiArray "data: [-0.4, 0.0, 0.0]"
我们始终希望左夹爪关节（left_gripper_joint）和右夹爪关节（right_gripper_joint）保持相同的数值。
通过以下几个步骤，我们可以在 URDF 和 控制器 中实现这种“联动（mimic）”关系。
- 在 URDF 中的 right_gripper_joint 定义里插入：
<param name="mimic">left_gripper_joint</param>
- 在 ros2_control 的 right_gripper_joint 接口定义中加入：
<param name="mimic">left_gripper_joint</param>
- 在新的控制参数文件中（例如控制器的 YAML 配置）
- 只需在夹爪控制器下列出 两个关节（例如 gripper_extension 和 left_gripper_joint），
不再列出 right_gripper_joint，因为它会自动跟随左夹爪关节的动作。
- 只需在夹爪控制器下列出 两个关节（例如 gripper_extension 和 left_gripper_joint），
其URDF XML 如下：
<xacro:gripper prefix="right" reflect="-1">
<mimic joint="left_gripper_joint"/>
</xacro:gripper>
....
<ros2_control name="GazeboSystem" type="system">
......
<joint name="right_gripper_joint">
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="mimic">left_gripper_joint</param>
<param name="multiplier">1</param>
</joint>
</ros2_control>
yaml 配置如下：
.....
gripper_controller:
ros__parameters:
joints:
- gripper_extension
- left_gripper_joint
interface_name: position
启动命令如下：
ros2 launch urdf_sim_tutorial 12-gripper.launch.py urdf_package_path:=urdf/12a-mimic-gripper.urdf.xacro
现在我们只需要使用两个数值就可以控制夹爪了，例如：
ros2 topic pub /gripper_controller/commands std_msgs/msg/Float64MultiArray "data: [0.0, 0.5]"
4. 让机器人上的轮子转起来
要让机器人移动起来，我们首先必须在 URDF 文件的 <ros2_control> 标签中为四个轮子分别指定更多的接口，不过这次只需要 速度（velocity）控制接口。
<ros2_control name="GazeboSystem" type="system">
....
<joint name="left_front_wheel_joint"> <!-- 左前轮-->
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="left_back_wheel_joint"> <!-- 左后轮-->
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_front_wheel_joint"> <!-- 右前轮-->
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_back_wheel_joint"> <!-- 右后轮-->
<command_interface name="velocity" />
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
当然，我们可以为每个轮子分别定义控制器，但那样就太繁琐了，也没什么意思。相反，我们希望能够同时控制所有轮子。
为此，我们需要在 ROS 中配置更多参数，以便使用 DiffDriveController（差速驱动控制器） —— 它会订阅标准的 Twist 类型的 /cmd_vel 消息，并根据该消息驱动机器人运动。
4.1 Differential Drive Controller
在讲解yaml 之前， 我们先来看差速驱动控制器， 它的主要作用是：
- 接收速度指令（如ROS2 topic /cmd_vel 的线速度与角速度）；
- 计算出两侧驱动轮的转速；
- 将速度命令发布到各轮的关节接口；
- 同时通过编码器反馈计算里程（odometry）；
- 发布 TF（odom → base_link）和 /odom 话题。
这类控制器非常常见于：
- 移动机器人底盘（AGV、小车）；
- 双轮差速驱动平台；
- 两轮+万向轮结构（如 TurtleBot）。
控制器运行逻辑如下图：
/cmd_vel ─────┐
│
▼
+------------------+
| DiffDriveController |
+------------------+
│ │
▼ ▼
left_wheel right_wheel
joint joint
4.1.1关节声明
left_wheel_names: [left_front_wheel_joint, left_back_wheel_joint]
right_wheel_names: [right_front_wheel_joint, right_back_wheel_joint]
对于其它控制器，joint 都是独立控制的， 所以使用joint 来描述。对于 DiffDriveController，左右两侧车轮是成组控制的，控制器内部会自动：
- 把左右轮的速度平均；
- 计算里程；
- 推导出机器人整体的 linear.x 与 angular.z；
- 再通过几何模型反推出每个轮子的速度。
所以它必须区分「左轮组」和「右轮组」。 控制器会自动认为：
- 左前轮和左后轮速度相同（平均处理）；
- 右前轮和右后轮速度相同；
- 最终控制器只输出两个命令：
- 左轮组转速
- 右轮组转速
4.1.2 几何参数
wheel_separation: 0.44 # 左右轮中心的距离 (m)
wheel_radius: 0.035 # 轮半径 (m)
wheel_separation：这是机器人两个驱动轮中心点之间的直线距离。你可以简单地理解成两个轮子之间的“轴距”在宽度方向上的体现。
wheel_radius： 这是机器人驱动轮的半径。
我们定义：
v： 机器人本体的线速度（前进/后退速度，单位：米/秒）
ω： 机器人本体的角速度（转弯速度，单位：弧度/秒）
v_left： 左轮的线速度（单位：米/秒）
v_right： 右轮的线速度（单位：米/秒）
r = wheel_radius = 0.035 m
l = wheel_separation = 0.44 m
计算机器人线速度 v:
v = (v_right + v_left) / 2
机器人的整体前进速度是两个轮子速度的平均值。
计算机器人角速度 ω:
ω = (v_right - v_left) / l
机器人的转弯速度是由两个轮子的速度差决定的，而这个速度差的效果会被轮间距 l 放大或缩小。轮间距 l 越大，产生同样的角速度就需要越大的速度差。
由此可见，wheel_separation 和wheel_radius， 是计算机器人各种速度的重要参数。
4.1.3 话题发布与帧设置
publish_rate: 50.0 # 控制器发布数据的频率为每秒50次
base_frame_id: base_link # TF 的基准坐标系
use_stamped_vel: false # 是否用带时间戳的速度命令
base_frame_id 指定了控制器和里程计计算的参考框架。所有的运动和里程计数据都将相对于这个坐标系来发布。
map (世界固定坐标系)
│
└── odom (里程计坐标系)
│
└── base_link (机器人本体坐标系) ← 就是这个！
│
├── laser_link (激光雷达坐标系)
├── camera_link (摄像头坐标系)
├── imu_link (IMU坐标系)
└── wheel_left_link (左轮坐标系)
4.1.4 速度与加速度限制
这些限制主要用于：
- 保护机器人安全（不会突然加速或旋转过快）
- 仿真中保持物理合理性
- 与导航栈或控制算法一致
linear:
x:
has_velocity_limits: true
max_velocity: 0.2
min_velocity: -0.2
has_acceleration_limits: true
max_acceleration: 0.6
angular:
z:
has_velocity_limits: true
max_velocity: 2.0
min_velocity: -2.0
has_acceleration_limits: true
max_acceleration: 6.0
min_acceleration: -1.0
线速度（linear）
- linear：线性速度，通常只有 x 方向（前进/后退）有效。
- has_velocity_limits：是否启用速度限制。
- true 表示会限制机器人前进和后退速度。
- max_velocity：允许的最大前进速度（m/s）。
- 这里是 0.2 m/s，机器人前进不会超过 0.2 m/s。
- min_velocity：允许的最大后退速度（负值，m/s）。
- 这里是 -0.2 m/s，机器人后退不会超过 0.2 m/s。
- has_acceleration_limits：是否启用加速度限制。
- true 表示加速度会被限制。
- max_acceleration：最大线加速度（m/s²）。
- 这里是 0.6 m/s²，机器人加速或减速不会超过 0.6 m/s²。
- 注意：线加速度没有 min_acceleration，通常是对称的，正负方向都限制为 0.6 m/s²。
角速度（angular）
- angular：角速度，通常只有 z 方向有效（绕垂直轴旋转，即转向）。
- z：绕 z 轴的旋转速度限制。
- has_velocity_limits：是否启用角速度限制。
- max_velocity：最大顺时针旋转速度（rad/s）。
- 这里是 2.0 rad/s。
- min_velocity：最大逆时针旋转速度（负值，rad/s）。
- 这里是 -2.0 rad/s。
- has_acceleration_limits：是否启用角加速度限制。
- max_acceleration：最大角加速度（rad/s²）。
- 这里是 6.0 rad/s²，表示机器人旋转加速不会超过这个值。
- min_acceleration：最小角加速度（负方向，rad/s²）。
- 这里是 -1.0 rad/s²，表示旋转减速时的限制不同于加速。
- 注意：角加速度可以不对称，这里设置了正向和反向不同的限制。
4.1.5 里程计与噪声建模
ROS 中的里程计（nav_msgs/Odometry）消息包含两个关键部分：
- pose：机器人在地图或基座坐标系下的位置和姿态
- 类型：geometry_msgs/Pose
- twist：机器人在自身坐标系下的线速度和角速度
- 类型：geometry_msgs/Twist
每个部分都可以有 协方差矩阵，用来表示测量误差或不确定性。
- 类型：geometry_msgs/Twist
- 协方差矩阵是 6×6 的矩阵，对应 [x, y, z, roll, pitch, yaw]
- ROS 里通常只配置对角线（非零的对角线），即 pose_covariance_diagonal 和 twist_covariance_diagon
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
长度 6 对应顺序：[x, y, z, roll, pitch, yaw]
pose_covariance_diagonal
| 位置 | 值 | 含义 |
|---|---|---|
| x | 0.001 | x 方向位置的不确定性（m²） |
| y | 0.001 | y 方向位置的不确定性（m²） |
| z | 0.001 | z 方向位置的不确定性（m²） |
| roll | 0.001 | 绕 x 轴旋转的不确定性（rad²） |
| pitch | 0.001 | 绕 y 轴旋转的不确定性（rad²） |
| yaw | 0.03 | 绕 z 轴旋转的不确定性（rad²） |
差分驱动机器人主要在平面上移动，所以 x, y, yaw 最重要，roll/pitch/z 通常很小。这里 yaw 的不确定性比 roll/pitch 大，合理。
twist_covariance_diagonal
| 位置 | 值 | 含义 |
|---|---|---|
| x | 0.001 | x 方向速度的不确定性（m²/s²） |
| y | 0.001 | y 方向速度的不确定性（m²/s²） |
| z | 0.001 | z 方向速度的不确定性（m²/s²） |
| roll | 0.001 | 绕 x 轴角速度的不确定性（rad²/s²） |
| pitch | 0.001 | 绕 y 轴角速度的不确定性（rad²/s²） |
| yaw | 0.03 | 绕 z 轴角速度的不确定性（rad²/s²） |
差分驱动机器人主要绕 z 轴旋转，所以 yaw 的误差最大，线速度 x/y 不确定性很小。
4.1.6 配置参数
diffdrive.yaml 内容如下
controller_manager:
ros__parameters:
update_rate: 100
use_sim_time: true
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
head_controller:
type: position_controllers/JointGroupPositionController
gripper_controller:
type: position_controllers/JointGroupPositionController
diff_drive_base_controller:
type: diff_drive_controller/DiffDriveController
head_controller:
ros__parameters:
joints:
- head_swivel
interface_name: position
gripper_controller:
ros__parameters:
joints:
- gripper_extension
- left_gripper_joint
interface_name: position
diff_drive_base_controller:
ros__parameters:
publish_rate: 50.0
left_wheel_names: [left_front_wheel_joint, left_back_wheel_joint]
right_wheel_names: [right_front_wheel_joint, right_back_wheel_joint]
wheel_separation: 0.44
wheel_radius: 0.035
# Odometry covariances for the encoder output of the robot. These values should
# be tuned to your robot's sample odometry data, but these values are a good place
# to start
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
# Top level frame (link) of the robot description
base_frame_id: base_link
use_stamped_vel: false
# Velocity and acceleration limits for the robot
linear:
x:
has_velocity_limits: true
max_velocity: 0.2 # m/s
min_velocity: -0.2 # m/s
has_acceleration_limits: true
max_acceleration: 0.6 # m/s^2
angular:
z:
has_velocity_limits: true
max_velocity: 2.0 # rad/s
min_velocity: -2.0
has_acceleration_limits: true
max_acceleration: 6.0 # rad/s^2
min_acceleration: -1.0
运行如下命令
ros2 launch urdf_sim_tutorial 13-diffdrive.launch.py
除了加载上述配置外，这还会打开 RobotSteering 面板，使你能够操控 R2D2 机器人移动，同时观察它在 Gazebo 中的实际运动行为，以及在 RViz 中的可视化行为。
4.1.7 diff_drive_base_controller/cmd_vel
ros2 interface show geometry_msgs/msg/Twist
linear:
x: float64 # 前后速度，单位 m/s
y: float64 # 侧向速度，一般为 0
z: float64 # 垂直速度，一般为 0
angular:
x: float64 # 绕前后轴旋转，一般为 0
y: float64 # 绕左右轴旋转，一般为 0
z: float64 # 绕垂直轴旋转，单位 rad/s
前进 0.1 m/s：
ros2 topic pub /diff_drive_base_controller/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"
原地逆时针旋转 0.5 rad/s：
ros2 topic pub /diff_drive_base_controller/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}"
前进并向右转：
ros2 topic pub /diff_drive_base_controller/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1}, angular: {z: -0.2}}"
恭喜！现在你已经可以使用 URDF 进行机器人仿真了。
立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。
更多推荐
8
0- 0
已为社区贡献5条内容
所有评论(0)