ROS2学习CH5 简单的Ros2与Gazebo建图与导航
注意注意，本笔记几乎所有的仿真都是基于我在下方github上上传的pakage。
当然有AI的辅助实现与编写。
Gazebo是一款机器人仿真器，旨在为机器人研究提供高保真度的仿真环境。它能够模拟复杂室内外环境中的机器人群体，提供动力学仿真、高质量的3D图形渲染，并支持多种传感器、噪声模型及物理引擎。
当然，需要注意的是，Gazebo与Ros2是截然不同的两个系统，Gazebo并不是Ros2生态下的组成部分，这也意味着两者实际上是无法直接通信的。
但是我们可以借助Ros2提供的工具，如ros_gz_bridge，来实现Gazebo与Ros2之间的通信。通过这种方式，Gazebo中的机器人状态、传感器数据等信息可以被Ros2节点订阅，从而在Rviz2中进行可视化。
Gazebo 界面
本文章使用的Gazebo为Gazebo Harmonic, 也就是不同于Gazebo Classic的版本。相较于经典版，其用户交互界面更加现代化，提供了更直观的模型编辑、场景管理和仿真控制功能。
终端中输入
gz sim shapes.sdf
即可打开Gazebo自带的一个示例场景。其中放置着很多3D图形，你可以通过鼠标和键盘与这些图形进行交互，调整视角、移动物体等。具体的UI操作可以参考GazeboUI操作
Rviz2
Rviz2是Ros2提供的可视化工具，主要用于显示机器人状态、传感器数据等信息。通过Rviz2，用户可以直观地查看机器人在仿真环境中的运行情况。
终端中输入
rviz2
即可打开Rviz2界面。在Rviz2中，你可以添加各种显示组件，如RobotModel、TF、LaserScan等，以便更好地观察机器人的状态和环境信息。当然，这些显示组件的数据来源通常是Ros2节点发布的话题，因此需要确保相关话题已经正确发布。
使用Gazebo与Rviz2查看机器人
这里使用我跟着 nav2 官方教程配置机器人以及导航环境的方法，将 Gazebo 与 Rviz2 结合使用，实现机器人的仿真与可视化。
代码仓库见 sam_bot_description
当然，在使用之前，需要确保已经正确安装了 Gazebo 和 Ros2，并且已经配置好了相关的开发环境。
启用配置好的launch文件, 显示的rviz以及Gazebo界面将同时打开，显示如下图：
显然，gazebo中包含了更多详细的3D模型和物理仿真信息，而Rviz2则主要用于显示机器人状态和传感器数据等信息。而且Rviz之所以能够显示出机器人的模型，实际上是我们在display.launch.py中通过robot_state_publisher节点以及使用sdformat_urdf（即一个将sdf模型转换为URDF格式的插件）将机器人的模型信息发布到了/robot_description话题上。而Rviz2通过订阅该话题，能够实时显示机器人的模型。
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{
'robot_description': ParameterValue(
Command(['xacro ', LaunchConfiguration('model')]),
value_type=str,
),
'use_sim_time': LaunchConfiguration('use_sim_time'),
}]
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
)
桥接
在使用Gazebo与Rviz2时，桥接（Bridge）是连接两者的关键。通过ros_gz_bridge，Gazebo中的话题可以被Ros2节点订阅，从而实现数据的实时传输。例如，Gazebo中的机器人状态、传感器数据等可以通过桥接在Rviz2中可视化。
ros2 run ros_gz_bridge parameter_bridge /话题名@ROS中的消息类型@or[or]Gazebo中的消息类型
比如
ros2 run ros_gz_bridge parameter_bridge /scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan
上面的命令的意思即为
# 将Gazebo中的/scan话题的数据桥接到Ros2中，使用sensor_msgs/msg/LaserScan消息类型，对应Gazebo中的gz.msgs.LaserScan消息类型，同时发布到Ros2的/scan话题
当然，对于一个复杂的机器人系统，可能需要桥接多个话题，以便在Rviz2中全面显示机器人的状态和环境信息。这样使用命令行的方式可能会显得繁琐，因此可以考虑使用配置文件来管理多个桥接规则，
从而简化操作。ros_gz_bridge允许我们使用yaml格式的配置文件来管理多个桥接规则。其写法如下：
- 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
# 这里定义了一个从Gazebo到Ros2的桥接规则，将Gazebo中的/clock话题的数据桥接到Ros2中，使用rosgraph_msgs/msg/Clock消息类型，对应Gazebo中的gz.msgs.Clock消息类型。
其中每个桥接规则都包含了Ros2话题名称、Gazebo话题名称、Ros2消息类型、Gazebo消息类型以及数据流向等信息。通过这种方式，可以方便地管理多个桥接规则，确保Gazebo与Ros2之间的数据能够正确传输。
建图与导航的常见话题
在机器人仿真与导航系统中，建图与导航涉及多个话题。以下是一些常见的话题及其用途：
/scan：激光雷达数据话题，用于建图和避障。/odom：里程计数据话题，提供机器人的运动信息。/map：地图数据话题，存储构建的环境地图。/tf：坐标系变换话题，提供各坐标系之间的变换关系。/robot_description：机器人模型信息话题，用于在Rviz2中显示机器人模型。/joint_states：关节状态话题，提供机器人各关节的位置、速度和加速度信息。/imu：惯性测量单元数据话题，提供机器人的姿态和加速度信息。/cmd_vel：速度控制话题，用于向机器人发送速度指令。/clock：系统时钟话题，提供仿真时间信息。
在整个建图与导航系统中，每个部分的模块通常会订阅上述的一个或者几个话题，从而获取所需的数据以完成各自的功能。同时，上述的话题其实只是一种命名规范，具体的实现可以根据实际需求进行调整。比如在我们的sam_bot_description中的sam_bot_description文件中，对于gazebo仿真的差速插件在sdf中的定义如下
<plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
<!-- wheels -->
<left_joint>drivewhl_l_joint</left_joint>
<right_joint>drivewhl_r_joint</right_joint>
<!-- kinematics -->
<wheel_separation>${base_width + 2*wheel_ygap}</wheel_separation>
<wheel_radius>${wheel_radius}</wheel_radius>
<!-- limits -->
<max_linear_velocity>0.3</max_linear_velocity>
<min_linear_velocity>-0.15</min_linear_velocity>
<max_angular_velocity>1.0</max_angular_velocity>
<min_angular_velocity>-1.0</min_angular_velocity>
<max_linear_acceleration>0.5</max_linear_acceleration>
<min_linear_acceleration>-0.5</min_linear_acceleration>
<max_angular_acceleration>1.5</max_angular_acceleration>
<min_angular_acceleration>-1.5</min_angular_acceleration>
<!-- input -->
<topic>/demo/cmd_vel</topic>
<!-- output -->
<odom_topic>/demo/odom</odom_topic>
<tf_topic>/tf</tf_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_footprint</child_frame_id>
</plugin>
其中定义了输入的话题为/demo/cmd_vel，即这个插件接受来自这个话题的数据，并将其提供的速度指令通过转换变为两个轮子的控制信号。
输出的里程计话题为/demo/odom，坐标系变换话题为/tf，指的是处理后的话题，即经过差速插件处理后的里程计和坐标系变换数据。
- ros_topic_name: "/demo/cmd_vel"
gz_topic_name: "/demo/cmd_vel"
ros_type_name: "geometry_msgs/msg/TwistStamped"
gz_type_name: "gz.msgs.Twist"
direction: ROS_TO_GZ
上面在bridge_config.yaml中定义了从Ros2到Gazebo的桥接规则，将Ros2中的/demo/cmd_vel话题的数据桥接到Gazebo中，使用geometry_msgs/msg/TwistStamped消息类型，对应Gazebo中的gz.msgs.Twist消息类型。当然，这个消息类型的定义如下
ros2 interface show geometry_msgs/msg/TwistStamped
# A twist with reference coordinate frame and timestamp
std_msgs/Header header
builtin_interfaces/Time stamp # 时间戳
int32 sec
uint32 nanosec
string frame_id # 坐标系ID
Twist twist
Vector3 linear # 线速度
float64 x
float64 y
float64 z
Vector3 angular # 角速度
float64 x
float64 y
float64 z
通过这个桥接规则，Ros2中的速度指令可以被传递到Gazebo仿真环境中，从而控制机器人的运动。比如我们可以使用Ros2的teleop_twist_keyboard节点，通过键盘输入来控制机器人的速度指令。
ros2 run teleop_twist_keyboard teleop_twist_keyboard
使用上面的TUI提示的键位操作，可以增加或者减少机器人的线速度和角速度，同时可以通过相应的键位来控制机器人的运动方向。当然，该节点会将输出默认发布到/cmd_vel话题。
ros2 run teleop_twist_keyboard teleop_twist_keyboard \
--ros-args -p stamped:=true --remap cmd_vel:=/demo/cmd_vel
# stamped: 是否发布带时间戳的速度指令
# remap: 将默认的cmd_vel话题重映射为/demo/cmd_vel
此时通过键盘输入的速度指令会被发布到/demo/cmd_vel话题，同时有由于我们使用gz_ros_bridge将Ros2中的话题桥接到Gazebo中，因此Gazebo仿真环境中的机器人也会接收到相应的速度指令并做出相应的运动。
在sam_bot_description中的主要桥接配置如下
---
- 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
# 时钟话题桥接配置，让Gazebo中的时钟信息桥接到Ros2中，保证仿真时间与Ros2时间同步
- ros_topic_name: "/demo/imu"
gz_topic_name: "/demo/imu"
ros_type_name: "sensor_msgs/msg/Imu"
gz_type_name: "gz.msgs.IMU"
direction: GZ_TO_ROS
# IMU话题桥接配置，让Gazebo中的IMU数据桥接到Ros2中获取机器人的姿态信息
- ros_topic_name: "/demo/odom"
gz_topic_name: "/demo/odom"
ros_type_name: "nav_msgs/msg/Odometry"
gz_type_name: "gz.msgs.Odometry"
direction: GZ_TO_ROS
# 里程计话题桥接配置，让Gazebo中的里程计数据桥接到Ros2中
- 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
# 关节状态话题桥接配置，让Gazebo中的关节状态数据桥接到Ros2中，从而可以让robot_state_publisher节点获取到机器人的关节状态信息，进而发布机器人的TF变换信息。当然此时发布的是活动关节(如轮子关节等)
- ros_topic_name: "/demo/cmd_vel"
gz_topic_name: "/demo/cmd_vel"
ros_type_name: "geometry_msgs/msg/TwistStamped"
gz_type_name: "gz.msgs.Twist"
direction: ROS_TO_GZ
# 速度指令话题桥接配置，让Ros2中的速度指令桥接到Gazebo中，从而控制机器人的运动
- 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
# 激光雷达话题桥接配置，让Gazebo中的激光雷达数据桥接到Ros2中
- ros_topic_name: "/scan/points"
gz_topic_name: "/scan/points"
ros_type_name: "sensor_msgs/msg/PointCloud2"
gz_type_name: "gz.msgs.PointCloudPacked"
direction: GZ_TO_ROS
# 激光雷达点云话题桥接配置，让Gazebo中的激光雷达点云数据桥接到Ros2中
可以看出，几乎大部分的传感器仿真数据都是从Gazebo环境中产出，并通过桥接配置传递到Ros2中，之后我们可以利用Ros2的各种包对这些数据进行处理，如用Rviz进行可视化，用Nav2进行导航，用Slam_Toolbox进行建图等等。
建图
在Ros2中，官方推荐使用slam_toolbox包来进行建图。该包提供了用于建图的节点和工具，可以方便地在Ros2环境中进行SLAM（同时定位与建图）操作。同时，支持同步在线建图（synchronous online mapping）和离线建图（offline mapping）模式，前者适用于实时建图，后者适用于对已收集的传感器数据的rosbag进行离线建图。
ROS2 中的 SLAM 建图流程
- 传感器获取环境信息：通常使用 2D/3D 激光雷达，也可以使用相机进行视觉 SLAM。
- 获取机器人运动信息：通过轮式里程计、IMU 等提供机器人的运动估计。
- SLAM 算法处理：结合传感器观测和里程计信息，一边估计机器人在环境中的位姿，一边逐步构建地图。
- 发布地图与定位结果：例如 slam_toolbox 会发布 /map，同时维护 map → odom 的 TF 变换。
- 保存地图：二维栅格地图通常可以保存为：
- map.pgm：地图图像
- map.yaml：地图分辨率、原点、图像路径等配置信息
slam_toolbox的工作流程如下所示(chatgpt帮我生成的图片)
也就是说，为了使用
slam_toolbox进行建图，我们需要确保机器人能够正确获取激光雷达数据和里程计信息，并且正确配置TF变换关系。在实现层面上，需要做的准备工作包括：
- 确保Gazebo仿真的雷达数据能够正确桥接到Ros2中，并将话题发布到
/scan。 - 确保Gazebo仿真的里程计数据能够正确桥接到Ros2中，并将话题发布到
/odom。 - 确保Gazebo仿真的IMU数据能够正确桥接到Ros2中，并将话题发布到
/imu。 - 确保机器人相关的坐标TF已经发布。
其中，/odom以及/imu话题会经过数据融合后，发布从odom到base_footprint的TF变换。建图过程中的TF变换关系如下图所示：
使用sam_bot_description实现建图体验，需要在终端中执行如下：
# 构建
colcon build
source ./install/setup.bash
# 使用launch文件启动仿真环境
ros2 launch sam_bot_description display.launch.py
# 该环境会启动Gazebo以及Rviz，用于可视化和交互。同时Rviz会读取config文件中的配置信息，实现直接加入一些预配置的选项
ros2 launch slam_toolbox online_async_launch.py use_sim_time:=true
# 启动slam_toolbox进行在线异步建图
# 至此，我们已经完成了使用sam_bot_description和slam_toolbox进行在线异步建图的配置和启动。
ros2 run teleop_twist_keyboard teleop_twist_keyboard \
--ros-args -p stamped:=true --remap cmd_vel:=/demo/cmd_vel
# 使用键盘控制机器人移动，去扫描环境，构建地图
此时通过键盘操纵小车移动，随着机器人的移动和激光雷达数据的采集，slam_toolbox会不断更新地图，并将地图发布到/map话题，同时维护map → odom的TF变换。我们可以在Rviz的map显示中实时查看地图的构建情况。
在Rviz中，我们可以通过添加Map显示类型来查看slam_toolbox发布的地图。
让小车不断移动，知道地图逐渐构建完成，结果大概如下图所示。
执行如下命令，可以保存构建完成的地图：
source /opt/ros/jazzy/setup.bash
ros2 run nav2_map_server map_saver_cli \
-f path/to/save/maps/map_name
--ros-args -p use_sim_time:=true
# 将地图保存在path/to/save/maps文件夹下，会生成map_name.pgm和map_name.yaml文件
地图解释
显然，在我们保存的地图文件中，map.pgm 是地图的图像文件，而 map.yaml 是地图的配置信息文件。
我们将map.pgm用krita（一个开源的图像编辑软件）打开，并将其局部放大：
可以看出，该地图实际上是由一个又一个的小格子即像素组成的，每个像素代表地图中的一个格子，其颜色表示该格子的占用状态。
- 黑色 表示障碍物
- 白色 表示自由空间
- 灰色 表示未知区域
当然，既然每个格子都是一个灰度表示的像素，则每个pixel的取值范围通常是0到255，至于不同的取值与其显示的颜色为什么如上图所示，见下。
然后我们再来查看map.yaml 文件内容，其包含以下信息：
image: map.pgm # 地图的图像文件路径
mode: trinary # 地图模式，trinary 表示三值地图（占用、自由、未知）
resolution: 0.050 # 地图分辨率，单位为米/格子, 即每个格子代表的实际距离
origin: [-1.566, -1.341, 0] # 地图原点，单位为米
negate: 0 # 是否反转地图的占用状态，0 表示不反转
occupied_thresh: 0.65 # 占用阈值，大于该值的格子被认为是障碍物
free_thresh: 0.196 # 自由阈值，小于该值的格子被认为是自由空间
可以看到，trinary标签规定了每个格子只有三种可能的取值，同时occupied_thresh和free_thresh分别定义了占用和自由的阈值，从而将像素值划分为障碍物、自由空间和未知区域。即当像素值大于occupied_thresh时被认为是障碍物，小于free_thresh时被认为是自由空间，介于两者之间的则被认为是未知区域。同时，在Nav2的定义中，
- 障碍物 = 0
- 自由空间 = 254
- 未知区域 = 205
上述的灰度设置是固定的编码，即Nav2中定义的障碍物、自由空间和未知区域的像素值。
在加载地图时，Nav2会根据map.yaml中的配置信息，将map.pgm中的像素值映射为障碍物、自由空间和未知区域，从而构建出机器人的环境模型，具体映射关系如下所示。
加载地图到Rviz并启用导航
在导航的时候，我们需要将上面保存的地图加载并发布，让Rviz能够显示地图，并且Nav2能够使用地图进行导航。
发布地图的功能节点叫做nav2_map_server。当然我们可以使用nav2官方提供的启动文件来启动该节点，这里我们使用已经写好的sam_navigation2.launch.py来启动导航。
该启动文件会启动Nav2的各个节点，包括地图服务器、导航控制器等，从而让机器人能够使用加载的地图进行导航。同时启动Rviz以及Gazebo，同时显示地图和机器人的状态。
在Rviz中，我们点击2D point estimate按钮，然后在地图上点击一个点，Rviz会将该点作为机器人的初始位姿发布到/initialpose话题，然后AMCL(自适应蒙特卡洛定位，)工具会根据该位姿进行导航。同时会发布map到odom的TF变换。
然后就会出现下面的界面，显示机器人的初始位姿以及全局代价地图。
当然，这个初始位姿的选取不用过于精确，只要大致正确即可，当导航开始时，AMCL会根据机器人的传感器数据逐渐修正位姿，使定位结果更加准确。
此后点击Nav2 Goal按钮，然后在地图上点击一个目标点，机器人就会根据当前的全局代价地图规划出一条从初始位姿到目标点的路径，并开始在Gazebo中移动。
最终其会到达目标点，完成导航任务。
浙公网安备 33010602011771号