用 Gazebo 学习 ROS 2
本文基于约翰霍普金斯大学"机器人学导论"课程中教授的机器人学之避障主题。我们将自定义一个ROS 2机器人并在Gazebo Ionic仿真环境中运行这个机器人。
1、基础（设置与启动）
在我们能够传递传感器数据或驱动机器人之前，我们需要让ROS 2和Gazebo Ionic无缝地一起启动。
两个不同的世界
ROS 2和Gazebo是完全独立的软件生态系统。
- ROS 2使用一种名为DDS（数据分发服务）的中间件协议进行通信。
- Gazebo Ionic使用自己的内部消息系统Gazebo Transport进行通信。
因为它们使用不同的语言，我们使用一个名为ros_gz的元包。这个包包含从ROS终端启动Gazebo的启动文件，以及在两者之间转换消息所需的桥接器。
以下是我们构建第一个集成启动文件的实践练习。
步骤1：安装集成包
首先，确保你已为特定的ROS 2发行版（例如Jazzy、Humble或Rolling）安装了ros_gz包。打开终端并运行：
sudo apt update
sudo apt install ros-jazzy-ros-gz
步骤2：创建启动文件
专业的ROS开发者不会从桌面手动打开Gazebo，而是直接从ROS 2 Python启动文件启动模拟器。
首先创建ROS 2包。
ros2 pkg create --build-type ament_python gazebo_control
创建一个名为gazebo_launch.py的新文件，并将以下代码粘贴到其中：
# launch/gazebo_launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
# 1. Find the official ros_gz_sim package on your computer
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
# 2. Setup the IncludeLaunchDescription to trigger Gazebo
gz_sim = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')
),
# 3. Pass arguments to Gazebo: load an empty world and run immediately (-r)
launch_arguments={'gz_args': 'empty.sdf -r'}.items(),
)
# 4. Return the LaunchDescription
return LaunchDescription([
gz_sim
])
步骤3：理解代码
让我们分解一下你刚才写的内容：
get_package_share_directory('ros_gz_sim')：动态查找官方ros_gz集成包在你硬盘上的位置。IncludeLaunchDescription：这就像一个"盗梦空间"命令。它告诉你的自定义启动文件去触发一个完全不同的启动文件（Gazebo提供的gz_sim.launch.py）。gz_args: 'empty.sdf -r'：这是传递给Gazebo的命令行参数。empty.sdf告诉它加载一个空白世界。-r标志至关重要——它告诉Gazebo按下"播放"并立即启动物理引擎。没有它，模拟会以暂停状态启动。
步骤4：执行启动
要测试我们的基础，打开终端，导航到保存文件的文件夹，并运行：
colcon build --packages-select gazebo_control
source install/setup.bash
ros2 launch gazebo_control gazebo_launch.py
如果一切配置正确，Gazebo Ionic GUI应该会弹出一个空白的3D世界，完全从ROS 2生态系统内部启动！
2、ros_gz_bridge和管理多个机器人
通用翻译器
目前，ROS 2和Gazebo就像两个坐在同一个房间里但说着完全不同语言的人。如果ROS 2使用geometry_msgs/msg/Twist消息大喊"向前开！"，Gazebo会忽略它，因为它只理解gz.msgs.Twist消息。
ros_gz_bridge就是我们的翻译器。要使用它，我们必须准确定义我们想要翻译的主题、消息类型是什么，以及翻译应该沿哪个方向进行。
标准语法如下：
/topic_name@ROS_2_Type[Direction]Gazebo_Type
方向括号：
]：从ROS 2翻译到Gazebo。[：从Gazebo翻译到ROS 2。==：双向翻译。
让我们亲自动手驱动机器人。
步骤1：启动机器人世界
首先，我们需要一个可以驾驶的机器人。Gazebo附带了一个默认的差速驱动车辆可以使用。关闭之前的Gazebo窗口（在该终端中使用Ctrl+C），改为运行：
ros2 launch ros_gz_sim gz_sim.launch.py gz_args:="diff_drive.sdf -r"
当Gazebo打开时，你会注意到一个有趣的事情： 有两个机器人！一个蓝色的和一个绿色的。
因为有两个独立的车辆，我们不能只向/cmd_vel主题发送一个通用的速度命令——Gazebo不知道哪个机器人应该移动。相反，Gazebo根据实体树中的模型名称为每个机器人分配一个特定的命名空间（例如，/model/vehicle_green/cmd_vel）。
步骤2：启动桥接器（ROS 2 -> Gazebo）
我们想从ROS 2向Gazebo中的机器人发送速度命令。机器人在/cmd_vel主题上监听。
打开第二个终端，source你的ROS 2环境，并使用蓝色车辆的特定命名空间运行桥接命令：
ros2 run ros_gz_bridge parameter_bridge \
/model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist
/model/vehicle_blue/cmd_vel：这是蓝色机器人的特定命名空间主题。@geometry_msgs/msg/Twist：我们正在发送的标准ROS 2消息类型。]：告诉桥接器将数据从ROS 2发送到Gazebo的方向括号。gz.msgs.Twist：需要翻译成的Gazebo特定消息类型。
步骤3：驱动机器人！
现在是有趣的部分。打开第三个终端，source你的ROS 2环境，手动发布一个ROS 2 Twist消息告诉机器人向前行驶并转弯绕圈：
ros2 topic pub /model/vehicle_blue/cmd_vel \
geometry_msgs/msg/Twist "{linear: {x: 1.0}, angular: {z: 0.5}}"
看看你的Gazebo窗口——蓝色车辆应该立即开始绕圈行驶！
步骤4：桥接数据回传（Gazebo -> ROS 2）
驾驶很好，但我们也需要从模拟器获取数据。最基本的数据是模拟时钟（它确保ROS 2算法与Gazebo的物理时间保持同步）。
在第二个终端中停止桥接器（Ctrl+C），让我们运行一个同时处理/model/vehicle_blue/cmd_vel（发送到Gazebo）和/clock（返回ROS 2）的新桥接器：
ros2 run ros_gz_bridge parameter_bridge \
/model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist \
/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock
注意时钟的[括号，表示Gazebo正在向ROS 2发送数据。
为了证明它正在工作，转到你的第三个终端（用Ctrl+C停止发布命令）并回显时钟主题：
ros2 topic echo /clock
你应该看到连续的时间数据流从Gazebo物理引擎直接涌入你的ROS 2终端。
clock:
sec: 527
nanosec: 401000000
---
clock:
sec: 527
nanosec: 402000000
---
clock:
sec: 527
nanosec: 403000000
3、生成URDF（让机器人活起来）
在第2章中，我们学习了如何通过驾驶一些默认的预构建方块来与Gazebo通信。但模拟的目标不是驾驶通用形状——而是测试你特定的机器人设计。
在这里，我们遇到了一个经典的语言障碍。Gazebo原生使用SDF（仿真描述格式）文件构建其世界和机器人。然而，整个ROS 2生态系统依赖于URDF（统一机器人描述格式）文件。
ros_gz_sim包提供了一个出色的工具——create节点——而不是强迫你手动将URDF重写为SDF。它可以读取你的ROS 2 URDF并将其动态注入到运行中的Gazebo世界中。
让我们使用以下URDF文件进行此测试。
<?xml version="1.0"?>
<robot name="basic_robot">
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
<link name="chassis">
<visual>
<geometry>
<box size="0.825 0.825 0.075"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.825 0.825 0.075"/>
</geometry>
</collision>
<inertial>
<mass value="10.0"/>
<inertia ixx="0.57" ixy="0.0" ixz="0.0" iyy="0.57" iyz="0.0" izz="1.13"/>
</inertial>
</link>
<link name="left_wheel">
<visual>
<geometry>
<cylinder radius="0.4" length="0.05"/>
</geometry>
<material name="blue"/>
</visual>
<visual>
<origin xyz="0.2 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.06"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.4" length="0.05"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<inertia ixx="0.08" ixy="0.0" ixz="0.0" iyy="0.08" iyz="0.0" izz="0.16"/>
</inertial>
</link>
<joint name="left_wheel_joint" type="continuous">
<parent link="chassis"/>
<child link="left_wheel"/>
<origin xyz="0 0.4375 0" rpy="-1.570796 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<link name="right_wheel">
<visual>
<geometry>
<cylinder radius="0.4" length="0.05"/>
</geometry>
<material name="red"/>
</visual>
<visual>
<origin xyz="0.2 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.06"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.4" length="0.05"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<inertia ixx="0.08" ixy="0.0" ixz="0.0" iyy="0.08" iyz="0.0" izz="0.16"/>
</inertial>
</link>
<joint name="right_wheel_joint" type="continuous">
<parent link="chassis"/>
<child link="right_wheel"/>
<origin xyz="0 -0.4375 0" rpy="-1.570796 0 0"/>
<axis xyz="0 0 1"/>
</joint>
</robot>
让我们编写一个启动文件来完成这个任务。
步骤1：启动文件架构
我们将创建一个名为spawn_robot.launch.py的新启动文件。这个文件需要同时完成三件事：
- 启动带空白世界的Gazebo。
- 启动ROS 2
robot_state_publisher（它读取你的basic_robot.urdf并将其物理结构广播到ROS网络）。 - 运行Gazebo的
create节点来获取广播的结构并将其生成到3D物理引擎中。
步骤2：编写代码
在你的gazebo_control包中创建spawn_robot.launch.py文件并粘贴以下内容：
# launch/spawn_robot.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
# 1. Paths to your package and the URDF file
pkg_gazebo_control = get_package_share_directory('gazebo_control')
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
urdf_file = os.path.join(pkg_gazebo_control, 'urdf', 'basic_robot.urdf')
with open(urdf_file, 'r') as infp:
robot_desc = infp.read()
# 2. Start Gazebo with an empty world
gz_sim = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')
),
launch_arguments={'gz_args': 'empty.sdf -r'}.items(),
)
# 3. Start the Robot State Publisher
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{
'robot_description': robot_desc,
'use_sim_time': True # CRITICAL: Tells ROS to use Gazebo's clock!
}]
)
# 4. The Spawner Node: Bridges URDF into Gazebo
spawn_robot = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-topic', 'robot_description',
'-name', 'my_custom_diffdrive',
'-z', '0.5' # Drop it slightly above the ground
],
output='screen'
)
return LaunchDescription([
gz_sim,
robot_state_publisher,
spawn_robot
])
步骤3：理解create节点
仔细看上面代码的第4部分。来自ros_gz_sim的create可执行文件在这里承担了主要工作。让我们看看传递给它的参数：
-topic robot_description：告诉Gazebo，"嘿，不要在硬盘上找文件了。相反，监听ROS 2的/robot_description主题并构建你听到的内容。"-name my_custom_diffdrive：为我们的机器人在Gazebo实体树中设置特定的命名空间。-z 0.5：这是物理引擎的一个专业技巧。我们将机器人在空中生成0.1米，让它落到地面上。如果你正好在0.0处生成，车轮可能会穿入地面，导致物理引擎将你的机器人猛烈弹射到天空中！
步骤4：启动并验证
构建你的工作空间（colcon build），source你的终端，并运行新的启动文件：
colcon build --packages-select gazebo_control
source install/setup.bash
ros2 launch gazebo_control spawn_robot.launch.py
Gazebo应该会弹出。你应该能看到你的自定义差速驱动机器人——带有平坦底盘、蓝色左轮、红色右轮和绿色激光雷达穹顶——完美地坐在物理模拟器中！
4、传感器集成与RViz2可视化（给机器人眼睛）
在第3章中，我们成功将你的自定义URDF机器人放入了Gazebo物理引擎。它存在于世界中，但目前完全看不见。
机器人需要外感受传感器（如LiDAR或摄像头）来绘制环境地图并避障。在Gazebo中，我们通过将Gazebo插件附加到现有URDF链接来创建传感器。然后，我们使用ros_gz_bridge将丰富的传感器数据从物理引擎传输到ROS 2进行可视化和处理。
让我们把机器人上那个绿色的lidar_dome变成一个全功能的360度激光扫描器。
步骤1：在URDF中添加传感器插件
打开你的basic_robot.urdf文件。在文件最底部（紧邻最后的</robot>标签上方），粘贴这个<gazebo>块：
<joint name="lidar_joint" type="fixed">
<parent link="chassis"/>
<child link="lidar_dome"/>
<origin xyz="0.3 0 0.0375" rpy="0 0 0"/>
</joint>
<gazebo reference="lidar_dome">
<sensor name="lidar_sensor" type="gpu_lidar">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>10</update_rate>
<topic>scan</topic>
<gz_frame_id>lidar_dome</gz_frame_id>
<lidar>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3.14159</min_angle>
<max_angle>3.14159</max_angle>
</horizontal>
</scan>
<range>
<min>0.15</min>
<max>10.0</max>
<resolution>0.01</resolution>
</range>
</lidar>
</sensor>
</gazebo>
这做了什么：
reference="lidar_dome"：将虚拟传感器精确地附加到绿色球体所在的位置。type="gpu_lidar"：告诉Gazebo使用计算机显卡计算激光射线，这比使用CPU快得多。visualize="true"：将在Gazebo窗口内绘制蓝色激光射线，让你看到它们击中物体。min/max_angle：-3.14到3.14弧度提供完整的360度扫描。
步骤2：添加仿真描述文件（SDF）
仿真描述格式（SDF）文件定义了你的机器人在Gazebo中模拟的环境（"世界"）。
虽然URDF（统一机器人描述格式）用于描述机器人本身（其链接、关节和传感器），但SDF用于描述模拟中其他所有内容。
以下是我的my_world.sdf供参考。
<?xml version="1.0" ?>
<sdf version="1.8">
<world name="my_world">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<gui fullscreen="0">
<plugin filename="MinimalScene" name="3D View">
<gz-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</gz-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
</plugin>
<plugin filename="EntityContextMenuPlugin" name="Entity context menu">
<gz-gui>
<property key="state" type="string">floating</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="GzSceneManager" name="Scene Manager">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="InteractiveViewControl" name="Interactive view control">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="CameraTracking" name="Camera Tracking">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="VisualizeLidar" name="Visualize Lidar">
</plugin>
</gui>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
</world>
</sdf>
以下是我们创建的my_world.sdf文件的详细解析：
1. 物理和核心系统
- 物理引擎：设置物理步长和实时因子，告诉Gazebo运行模拟的速度以及计算碰撞和重力的频率。
- 核心插件：加载必要的Gazebo插件，如gz-sim-physics-system（处理重力和碰撞）和gz-sim-scene-broadcaster-system（将3D数据发送到GUI供查看）。
2. 传感器系统（LiDAR修复）
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
Gazebo中默认的empty.sdf世界默认不加载传感器插件。没有这个插件，Gazebo会忽略你URDF中的<sensor>标签，这就是为什么你的激光雷达最初没有发射射线的原因。这个块告诉Gazebo主动模拟摄像头和激光雷达等传感器。
3. GUI插件
定义Gazebo用户界面。加载3D视图、实体树和相机控制。关键的是，它包含了<plugin filename="VisualizeLidar" name="Visualize Lidar">块，强制Gazebo GUI自动加载激光雷达可视化工具。
4. 环境（太阳和地面）
- light（太阳）：创建一个方向光源，使世界不会漆黑一片并让物体投射阴影。
- ground_plane模型：创建你的机器人行驶的平坦灰色网格地板。它包括visual（让你可以看到它）和collision（让机器人不会掉入虚空）。
总之，my_world.sdf是舞台、灯光和物理引擎，而你的basic_robot.urdf是在那个舞台上表演的演员！
步骤3：启动世界并添加障碍物
保存你的URDF并运行我们在第3章中构建的启动文件：
ros2 launch en613_control spawn_robot.launch.py
当Gazebo打开时，你现在应该能看到一个淡淡的蓝色激光环从你的机器人穹顶射出！
为了让数据更有趣，使用Gazebo顶部菜单栏（3D形状图标）在激光路径中直接投放几个方块或球体。
步骤4：桥接传感器数据（Gazebo -> ROS 2）
激光正在Gazebo中发射，但ROS 2毫不知情。我们需要桥接/scan主题。
打开第二个终端，source你的环境，运行桥接命令。注意我们使用的是[括号，因为数据从Gazebo流向ROS 2。
ros2 run ros_gz_bridge parameter_bridge /scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan
步骤5：在RViz2中可视化
在Gazebo中看到蓝色线条很好，但自主算法不看Gazebo——它们看ROS 2数据。让我们使用RViz2证明数据已经成功通过桥接。
打开第三个终端并启动可视化器：
rviz2
配置RViz2查看激光：
- 在左侧的"Displays"面板中，将Fixed Frame更改为
lidar_dome。 - 点击左下角的**"Add"**按钮。
- 选择**"By topic"**并双击
/scan主题添加LaserScan显示。 - 在LaserScan显示设置中将"Size"参数增加到
0.05，使点更容易看到。
你现在应该在RViz2中看到明亮的红点，形成了你放入Gazebo世界中的方块的精确轮廓！当你在Gazebo中移动方块时，RViz2中的红点将实时更新。
5、变换树（TF2）与让机器人动起来
在第4章中，我们通过添加LiDAR传感器给自定义机器人装上了"眼睛"。然而，它目前只是一个花哨的高科技雕像。它没有电机，更重要的是，ROS 2不知道机器人的物理部件如何运动。
Gazebo完美地处理物理，但RViz2（以及你的自主算法）依赖于TF2（变换）树。TF2是ROS 2的神经系统。它精确追踪chassis相对于世界（odom）的位置，以及车轮相对于chassis的精确位置。
让我们添加虚拟电机并连接这个神经系统！
步骤1：添加"肌肉"（差速驱动插件）
要让你的自定义URDF行驶，我们需要将差速驱动控制器附加到车轮上。
打开你的basic_robot.urdf文件。在底部，紧接LiDAR <gazebo>块下方，添加这个新插件来定义电机：
<gazebo>
<plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.875</wheel_separation>
<wheel_radius>0.4</wheel_radius>
<odom_publish_frequency>30</odom_publish_frequency>
<topic>cmd_vel</topic>
<odom_topic>odom</odom_topic>
<frame_id>odom</frame_id>
<child_frame_id>chassis</child_frame_id>
</plugin>
</gazebo>
这做了什么： 告诉Gazebo监听/cmd_vel主题并对left_wheel_joint和right_wheel_joint施加物理扭矩。它还自动计算机器人的里程计并发布连接世界（odom）和机器人（chassis）的主要TF（变换）。
步骤2：发布车轮角度（关节状态）
差速驱动插件移动机器人，但RViz2不会动画显示车轮旋转，除非它知道它们的确切角度。我们在差速驱动插件下方再添加一个快速插件：
<gazebo>
<plugin filename="gz-sim-joint-state-publisher-system" name="gz::sim::systems::JointStatePublisher">
<topic>joint_states</topic>
<joint_name>left_wheel_joint</joint_name>
<joint_name>right_wheel_joint</joint_name>
</plugin>
</gazebo>
这做了什么： 每当车轮在物理引擎中转动一度的几分之一时，这个插件都会将该关节角度广播到/joint_states主题。
步骤3：终极桥接
我们需要桥接三个关键内容：速度命令（进入）、关节状态（输出）和TF树（输出）。我们可以将ros_gz_bridge直接添加到我们的spawn_robot.launch.py文件中！
bridge = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=[
'/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
'/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model',
'/model/my_custom_diffdrive/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
'/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan'
],
remappings=[
('/model/my_custom_diffdrive/tf', '/tf')
],
output='screen'
)
这更加方便，因为现在单个启动命令将启动Gazebo、生成机器人、启动机器人状态发布器，并为所有主题（cmd_vel、joint_states、tf和scan）启动桥接器。
注意：我们还需要正确的重映射，以便Gazebo的/model/my_custom_diffdrive/tf被转换为RViz期望的标准ROS 2 /tf主题。
关闭所有正在运行的终端，重新构建并启动你的世界：
ros2 launch gazebo_control spawn_robot.launch.py
步骤4：驾驶和可视化！
打开第二个终端并启动RViz2：
rviz2
配置RViz2：
更改Fixed Frame
- 将Fixed Frame更改为
odom（现在我们有了TF桥接器运行，这将完美运行！）。
添加RobotModel显示
默认情况下，RViz尝试从参数读取机器人描述，但我们的启动文件将其发布到主题。以下是在RViz中修复的方法：
- 展开左侧面板中的RobotModel显示。
- 找到名为Description Source的属性。
- 将其从Topic更改为Topic（如果还不是的话）。
- 找到名为Description Topic的属性。
- 点击旁边的空框并输入/robot_description（或从下拉菜单中选择）。
添加LaserScan
- 添加一个LaserScan显示（主题
/scan）。
你将在RViz中看到机器人。
最后，打开第三个终端并发布驾驶命令：
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 1.0}, angular: {z: 0.5}}"
注意：要停止机器人，运行以下命令。
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0}, angular: {z: 0.0}}"
看看RViz2。你不仅应该看到LiDAR数据在更新，而且你还会看到URDF上的车轮在odom坐标系中精确地旋转，同时机器人绕圈行驶，完美地镜像了Gazebo物理窗口中发生的事情！
你将在Gazebo中看到以下内容，即机器人将会移动。
步骤5：使用TeleopTwistKeyboard控制机器人
teleop_twist_keyboard是在ROS 2中使用键盘控制机器人的标准且最简单的方法。
由于我们已经在使用/cmd_vel主题，它将开箱即用，因为teleop_twist_keyboard默认发布到/cmd_vel。
使用方法：
1. 运行Teleop节点打开一个新终端（确保已source你的ROS 2安装）并运行：
ros2 run teleop_twist_keyboard teleop_twist_keyboard
如果需要安装此包：
sudo apt install ros-jazzy-teleop-twist-keyboard
2. 如何控制机器人节点运行后，确保你的终端窗口处于聚焦/活动状态。你可以使用以下按键移动机器人：
- i：前进
- ,：后退
- j：左转
- l：右转
- k：停止
调整速度：
- q / z：增加/减少最大速度10%
- w / x：仅增加/减少线速度10%
- e / c：仅增加/减少角速度10%
6、优化（组合与YAML桥接）
如果你跟着第5章操作，你可能注意到了一个明显的问题：终端疲劳。
要让我们的机器人完全运行，有时我们需要打开几个独立的终端——一个启动模拟、一个桥接传感器、另一个桥接速度和TF数据，还有一个用于RViz2。在专业的机器人环境中，依赖人类输入大量多行终端命令而不出拼写错误是灾难的根源。
生产系统通过使用YAML配置文件并将所有内容打包到单个自动化启动文件来解决这个问题。我们在第5章中已经应用了一些清理，让我们进一步改进。
步骤1：YAML配置文件
与其在终端中使用神秘的括号（]、[），ros_gz_bridge可以读取一个干净、易读的YAML文件来精确定义数据应该如何在两个生态系统之间流动。
在你的gazebo_control包中，创建一个名为config的新文件夹。在该文件夹中，创建一个名为bridge_config.yaml的文件并粘贴以下内容：
# bridge_config.yaml
# 1. Drive Commands (ROS 2 to Gazebo)
- ros_topic_name: "/cmd_vel"
gz_topic_name: "/cmd_vel"
ros_type_name: "geometry_msgs/msg/Twist"
gz_type_name: "gz.msgs.Twist"
direction: ROS_TO_GZ
# 2. LiDAR Sensor Data (Gazebo to ROS 2)
- ros_topic_name: "/scan"
gz_topic_name: "/scan"
ros_type_name: "sensor_msgs/msg/LaserScan"
gz_type_name: "gz.msgs.LaserScan"
direction: GZ_TO_ROS
# 3. Joint States for Wheels (Gazebo to ROS 2)
- ros_topic_name: "/joint_states"
gz_topic_name: "/joint_states"
ros_type_name: "sensor_msgs/msg/JointState"
gz_type_name: "gz.msgs.Model"
direction: GZ_TO_ROS
# 4. Transform Tree / TF2 (Gazebo to ROS 2)
- ros_topic_name: "/tf"
gz_topic_name: "/model/my_custom_diffdrive/tf"
ros_type_name: "tf2_msgs/msg/TFMessage"
gz_type_name: "gz.msgs.Pose_V"
direction: GZ_TO_ROS
# 5. Simulation Clock (Gazebo to ROS 2)
- ros_topic_name: "/clock"
gz_topic_name: "/clock"
ros_type_name: "rosgraph_msgs/msg/Clock"
gz_type_name: "gz.msgs.Clock"
direction: GZ_TO_ROS
看这有多容易阅读！如果你以后添加新传感器（比如摄像头），只需在这个文件中追加一个新块。
步骤2："一键"启动文件
现在，让我们更新spawn_robot.launch.py来自动加载这个YAML文件并为我们启动桥接节点。
我们需要在现有Python启动文件中添加两样东西：YAML文件的路径和桥接节点本身。以下是直接在spawn_robot节点下方添加的代码：
# Find the config file path
bridge_params = os.path.join(
pkg_en613_control,
'config',
'bridge_config.yaml'
)
# 5. Start the Bridge Node using the YAML file
gz_bridge = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
parameters=[{
'config_file': bridge_params,
'use_sim_time': True
}],
output='screen'
)
（不要忘记将gz_bridge添加到文件底部的LaunchDescription数组中！）
步骤3：测试优化后的系统
因为我们添加了新的config文件夹，你需要更新CMakeLists.txt或setup.py以确保config文件夹在构建时被安装，然后运行colcon build。
现在，见证真相的时刻。打开一个终端并运行：
ros2 launch en613_control spawn_robot.launch.py
会发生什么？ 只需一个命令，你的计算机就会启动Gazebo物理引擎、读取你的自定义URDF、将机器人生成到3D世界中、使用YAML配置建立五个不同数据流的双向桥接，并开始主动将TF树和传感器数据广播到ROS 2网络的其余部分。
打开RViz2，在机器人前面放一个方块，并发布/cmd_vel消息。一切都会完美运行，你不再有充满终端窗口的杂乱桌面！
7、命令机器人前往在RVIZ中选择的目标点
让我们添加PID控制器节点
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
from tf2_ros import Buffer, TransformListener
import math
class DiffDrivePID(Node):
def __init__(self):
super().__init__('diffdrive_pid')
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.goal_sub = self.create_subscription(
PoseStamped,
'/goal_pose',
self.goal_callback,
10
)
self.cmd_vel_pub = self.create_publisher(
Twist,
'/cmd_vel',
10
)
self.timer = self.create_timer(1.0 / 30.0, self.timer_callback)
self.goal_x = None
self.goal_y = None
self.goal_theta = None
self.prev_x = None
self.prev_y = None
self.prev_theta = None
self.prev_time = None
# PD Gains
self.kp_v = 1.0
self.kd_v = 0.2
self.kp_omega = 2.0
self.kd_omega = 0.2
self.get_logger().info('DiffDrive PID Controller Node started.')
def goal_callback(self, msg):
self.goal_x = msg.pose.position.x
self.goal_y = msg.pose.position.y
q = msg.pose.orientation
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
self.goal_theta = math.atan2(siny_cosp, cosy_cosp)
self.get_logger().info(f'New goal received: x={self.goal_x:.2f}, y={self.goal_y:.2f}, theta={self.goal_theta:.2f}')
def timer_callback(self):
if self.goal_x is None:
return
try:
t = self.tf_buffer.lookup_transform('odom', 'chassis', rclpy.time.Time())
except Exception as e:
return
current_time = self.get_clock().now()
current_x = t.transform.translation.x
current_y = t.transform.translation.y
q = t.transform.rotation
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
current_theta = math.atan2(siny_cosp, cosy_cosp)
if self.prev_time is None:
self.prev_x = current_x
self.prev_y = current_y
self.prev_theta = current_theta
self.prev_time = current_time
return
dt = (current_time - self.prev_time).nanoseconds / 1e9
if dt <= 0:
return
dx_robot = current_x - self.prev_x
dy_robot = current_y - self.prev_y
dtheta_robot = current_theta - self.prev_theta
dtheta_robot = math.atan2(math.sin(dtheta_robot), math.cos(dtheta_robot))
current_v = (dx_robot * math.cos(current_theta) + dy_robot * math.sin(current_theta)) / dt
current_omega = dtheta_robot / dt
dx_goal = self.goal_x - current_x
dy_goal = self.goal_y - current_y
distance_error = math.sqrt(dx_goal**2 + dy_goal**2)
angle_to_goal = math.atan2(dy_goal, dx_goal)
heading_error = angle_to_goal - current_theta
heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))
if distance_error < 0.05:
distance_error = 0.0
heading_error = self.goal_theta - current_theta
heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))
v_cmd = self.kp_v * distance_error - self.kd_v * current_v
omega_cmd = self.kp_omega * heading_error - self.kd_omega * current_omega
v_cmd = max(min(v_cmd, 1.0), -1.0)
omega_cmd = max(min(omega_cmd, 2.0), -2.0)
if distance_error < 0.05 and abs(heading_error) < 0.05:
v_cmd = 0.0
omega_cmd = 0.0
self.goal_x = None
self.get_logger().info('Goal reached!')
twist = Twist()
twist.linear.x = float(v_cmd)
twist.angular.z = float(omega_cmd)
self.cmd_vel_pub.publish(twist)
self.prev_x = current_x
self.prev_y = current_y
self.prev_theta = current_theta
self.prev_time = current_time
def main(args=None):
rclpy.init(args=args)
node = DiffDrivePID()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
使用PID控制器的步骤：
1. 启动模拟在我们的第一个终端中，像之前一样启动Gazebo模拟和RViz：
ros2 launch gazebo_control spawn_robot.launch.py
2. 运行PID控制器打开一个新终端，source你的工作空间，运行PID节点：
source /opt/ros/jazzy/setup.bash
source /home/ubuntu/gazebo/install/setup.bash
ros2 run gazebo_control diffdrive_pid
3. 在RViz中命令机器人
- 转到你的RViz窗口。
- 在顶部工具栏中，点击"2D Goal Pose"按钮（看起来像一个绿色箭头）。
- 在RViz中的网格上点击任意位置并拖动以设置机器人的目标位置和方向（航向）。
- 观察机器人在Gazebo中移动到目标位置！PID控制器会自动计算必要的速度并在机器人到达目的地时停止。
如何避免在单独的终端中运行PID节点？
bridge_config.yaml文件专门用于在Gazebo和ROS 2之间桥接数据，所以我们不能把PID控制器放在里面。
然而，我们有更好的解决方案！我们可以直接将PID控制器添加到spawn_robot.launch.py文件中并重新构建包。
现在，当我们运行：
ros2 launch gazebo_control spawn_robot.launch.py
它将自动同时在一个终端中启动Gazebo、RViz、bridge和PID控制器！
为什么Gazebo中的机器人与RViz中的最终位置不完全一致？
是的，这完全正常！你所看到的是机器人学中一个叫做里程计漂移的基本概念。
以下是Gazebo中的机器人（"真实"世界）最终位置与RViz（"机器人大脑"）认为的位置略有不同的确切原因：
1. PID控制器如何工作当我们在RViz中点击"2D Goal Pose"时，我们告诉PID控制器："移动到相对于起始点（odom）的坐标(X, Y)。"PID控制器然后查看机器人在odom坐标系中的当前位置并计算车轮需要多快旋转才能到达目标。
2. 问题：车轮打滑Gazebo是一个物理模拟器。当机器人转弯或加速时，车轮可能会在地面上轻微打滑。然而，odom坐标系（RViz用来绘制机器人的）纯粹通过计算车轮旋转了多少次来计算。
- Gazebo："车轮转了10圈，但打滑了一点，所以机器人移动了9.5米。"
- 里程计（odom）："车轮转了10圈，所以机器人一定精确移动了10.0米。"
3. 结果由于这种打滑，机器人对自己位置的认知（RViz）慢慢偏离了其实际物理位置（Gazebo）。PID控制器成功地将机器人驱动到它认为目标所在的位置（在RViz中），但由于其内部地图略有偏移，Gazebo中的物理机器人最终位置略有偏差。
真正的机器人如何解决这个问题？为了解决这个问题，真正的机器人使用LiDAR或摄像头等传感器观察环境并纠正其里程计漂移。这被称为定位（如AMCL算法）。
当机器人使用定位时，它会创建一个名为map的新坐标系。map坐标系代表世界的真实绝对位置，机器人不断纠正odom坐标系以与之对齐。
由于我们的机器人目前仅依赖车轮编码器（odom）且还没有运行定位算法，两个窗口之间的轻微漂移是完全正常的，并证明了我们的物理模拟运行得很逼真！
为什么Gazebo中的机器人方向与RViz中的相反？
是的，这也是正常的，原因在于两个不同工具中坐标系的视觉表示方式！
ROS 2标准（右手定则）Gazebo和RViz都使用完全相同的数学坐标系（标准ROS 2右手定则）：
- X轴（红色）：前方
- Y轴（绿色）：左方
- Z轴（蓝色）：上方
视觉差异混淆来自于我们首次打开窗口时的默认相机角度：
- RViz（俯视图）：默认情况下，RViz从天空直视下方。
- X轴（红色）指向屏幕的右侧。
- Y轴（绿色）指向屏幕的顶部。
- 所以，如果我们告诉机器人"向下"移动，我们是在告诉它沿负Y方向移动。
- Gazebo（3D透视图）：Gazebo通常以一个角度打开。根据生成时相机的旋转方式：
- X轴（红色）可能指向屏幕的"上方"或"远离你"。
- Y轴（绿色）可能指向"左方"或"右方"。
如何证明它们实际上是匹配的：要看到它们实际上在做完全相同的事情，我们需要对齐我们的相机：
- 查看RViz中的网格。找到红线（X轴）和绿线（Y轴）。
- 转到Gazebo并旋转你的相机，使你从正上方俯视机器人。
- 旋转Gazebo相机，直到地面上的红线和绿线与RViz中的方向完全一致（红色指向右，绿色指向上）。
现在，如果你在RViz中点击"向下"，你会看到机器人在Gazebo中也"向下"移动！机器人并不是反向移动；你只是从两个不同的角度在观察它。
8、结束语
你已经成功将自定义ROS 2机器人迁移到Gazebo Ionic仿真环境中。你学会了如何建模物理属性、转换原生数据流、配置虚拟传感器、映射运动学变换树以及编写生产级启动系统。完整代码请参考这里。
仿真世界现在就是你的游乐场。祝你构建愉快！
原文链接: Robotics# Gazebo with Ros2
BimAnt翻译整理，转载请标明出处