第7章 ROS2中的SLAM环境建图
7.1 SLAM原理
“SLAM”，英文全称是“Simultaneous Localization And Mapping”，直译过来就是“即时定位与地图构建”。要理解SLAM，先得理解激光雷达的数据特点，激光雷达的扫描数据可以理解为一个障碍物分布的切面图，如图所示，其反映的是在一个特定高度上，障碍物面向雷达的边缘形状和分布位置。
将三个位置的激光雷达扫描轮廓拼合在一起，就能得到一个相对更完整的平面地图，同时也得出A、B、C三个位置在这个地图中的位置，如图所示：
7.2 SLAM Toolbox简介
在ROS2的官方教程中，推荐使用SLAM Toolbox来替代ROS1的Hector Mapping和Gmapping等建图软件包。SLAM Toolbox是由工程师Steve Macenski在Simbe Robotics公司就职期间构建的一套2D SLAM开源项目，目前由三星机器人研究院在维护。
要在ROS2中使用SLAM Toolbox，需要先安装相关软件包。安装指令为：
sudo apt install ros-humble-slam-toolbox
7.3 仿真环境实现SLAM建图
下面介绍如何在仿真环境中使用SLAM Toolbox进行SLAM建图。为了观察建图的过程，还会启动RViz2，实时显示建图的进展情况。所以建图时的数据通讯如图所示：
依旧先建包
cd ~/ros2_ws/src
ros2 pkg create slam_pkg
由于要同上启动建图的Toolbox节点和激光雷达lidar节点，我们用launch文件同时启动：
类似topic包，创建一个launch文件夹，创建alsm.launch.xml
复制：
<node>是要启动的节点，要填写该node属于哪个pkg，生成什么可执行文件exec 名称是什么，以及一切参数
<launch>
<!– 1. 启动 SLAM Toolbox 节点，并直接传入参数 –>
<node pkg="slam_toolbox" exec="sync_slam_toolbox_node" name="slam_toolbox" output="screen">
<param name="use_sim_time" value="true"/>
<param name="base_frame" value="base_footprint"/>
<param name="odom_frame" value="odom"/>
<param name="map_frame" value="map"/>
</node>
<!– 2. 启动 RViz2 节点，并加载指定的配置文件 –>
<node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share wpr_simulation2)/rviz/slam.rviz" output="screen"/>
</launch>
这部分是传给 slam_toolbox 算法的“配置单”：
-
use_sim_time: True：非常关键！ 这告诉 SLAM 节点：“现在时间是由 Gazebo 仿真器控制的，不要用电脑的真实时钟。” 你在 Gazebo 里跑必须设为 True，否则 TF 变换和雷达数据的时间戳对不上，SLAM 会失败或乱飘。
-
base_frame: "base_footprint"：机器人底盘在地面上的投影坐标系（通常雷达和 IMU 都挂在这个坐标系上面或附近）。
-
odom_frame: "odom"：里程计坐标系，由轮子编码器或仿真器提供的相对位置。
-
map_frame: "map"：地图坐标系，也就是 SLAM 最终计算出来的全局位置。
-
exec 启动了 slam_toolbox 包里的 sync_slam_toolbox_node（同步建图节点）。
-
同步的意思是：每收到一帧激光雷达数据，就立刻处理并更新地图，适合计算资源不是很紧张、需要实时看图的场景
获取 wpr_simulation2 功能包里预置的 rviz/slam.rviz 配置文件路径。
-
启动 rviz2，并通过 -d 参数加载这个配置文件。
-
这个 .rviz 文件通常已经配置好了：显示地图、显示激光雷达点云 (/scan)、显示机器人模型 (RobotModel)，以及正确的 Fixed Frame（通常是 map）。省去了你每次打开 RViz 手动添加话题的麻烦。
修改CmakeList
需要添加launch文件的地址：
install( DIRECTORY launch DESTINATION share/${PROJECT_NAME})
cmake_minimum_required(VERSION 3.8)
project(slam_pkg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
# ==================== 新增部分 ====================
install(
DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
# ==================================================
ament_package()
在ros2_ws colconbuild一下
cd ros2_ws
colcon build
关闭之前的wpr，新开一个home的仿真场景：
source install/setup.bash
ros2 launch wpr_simulation2 robocup_home.launch.py
在之前colconbuild的终端source一下
source install/setup.bash
执行
ros2 launch slam_pkg slam.launch.xml
可以看到，激光雷达/scan的数据收到并展示在rviz中，实际场景中机器人的方向如红色箭头所指。
我们把wpr的遥控器打开：
source install/setup.bash
ros2 run wpr_simulation2 keyboard_vel_cmd
现在控制机器人在仿真环境中转一转吧
随着我们的控制，机器人把房间大部分地图都剪出来了
接下来保存地图
新开终端输入
cd ros2_ws
ros2 run nav2_map_server map_saver_cli -f map
可以在ros2_ws文件夹下看到这两个文件
7.3.4 SLAM Toolbox的参数设置
除了上面例子的四个参数， SLAM Toolbox还提供了大量的参数可供设置。举个例子，设置地图更新间隔参数map_update_interval。在软件包slam_pkg的[slam.launch.xml]里，为字典slam_params添加一个参数。如图所示，参数名为“map_update_interval”，数值为1.0，表示机器人每移动1.0米就更新一次地图。
<launch>
<!– 1. 启动 SLAM Toolbox 节点，并直接传入参数 –>
<node pkg="slam_toolbox" exec="sync_slam_toolbox_node" name="slam_toolbox" output="screen">
<param name="use_sim_time" value="true"/>
<param name="base_frame" value="base_footprint"/>
<param name="odom_frame" value="odom"/>
<param name="map_frame" value="map"/>
<param name="map_update_interval" value="1.0"/>
</node>
<!– 2. 启动 RViz2 节点，并加载指定的配置文件 –>
<node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share wpr_simulation2)/rviz/slam.rviz" output="screen"/>
</launch>
修改后colcon build一下
然后重新启动三个终端的服务：
ros2 launch wpr_simulation2 robocup_home.launch.py
ros2 launch slam_pkg slam.launch.py
ros2 run wpr_simulation2 keyboard_vel_cmd
你会发现建图更新的频率变高了。
