ROS开发专栏---基于 SLAM Toolbox 实现仿真环境 SLAM 建图--适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ：《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ：《freertos专栏》《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》《Ai智能体专栏）《ROS开发专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

前言

[一、激光雷达SLAM 基础原理](#一、激光雷达SLAM 基础原理)

[二、SLAM Toolbox 简介](#二、SLAM Toolbox 简介)

[安装 SLAM Toolbox](#安装 SLAM Toolbox)

三、仿真环境实现SLAM建图

[3.1、创建 SLAM 功能包](#3.1、创建 SLAM 功能包)

[3.2、编写 SLAM 启动 Launch 文件](#3.2、编写 SLAM 启动 Launch 文件)

[3.3、配置 CMakeLists.txt](#3.3、配置 CMakeLists.txt)

[4.2.1、启动 Gazebo 仿真场景](#4.2.1、启动 Gazebo 仿真场景)

[4.2.2、启动 SLAM 建图 + RViz2 可视化](#4.2.2、启动 SLAM 建图 + RViz2 可视化)

4.2.3、键盘控制机器人巡游建图

五、保存建好的栅格地图

总结

前言

上一期博客我们已经实现了激光雷达数据读取、IMU 姿态解析、机器人运动控制等基础实验。

从本期博客开始进入机器人核心关键技术 ------SLAM 即时定位与地图构建。

ROS1 时代一般都是用 Gmapping、Hector 建图；

ROS2 Humble 官方首选推荐 SLAM Toolbox，配置简单、稳定性高、参数可调、仿真和真机通用，替代传统建图算法。

本期博客我们完成基于 SLAM Toolbox 实现仿真环境 SLAM 建图实验。

一、激光雷达SLAM 基础原理

SLAM 全称：Simultaneous Localization And Mapping（ 即时定位与地图构建**）**

作用：机器人在陌生未知环境里，一边自己定位自己在哪，一边实时构建环境地图，两件事同时完成。

激光雷达的扫描数据可以理解为一个障碍物分布的切面图，如图所示，其反映的是在一个特定高度上，障碍物面向雷达的边缘形状和分布位置。如下图所示：

激光雷达建图逻辑

激光雷达单次扫描，只能获取当前位置一圈障碍物轮廓；

机器人移动到 A、B、C 不同位置；

SLAM 算法把多帧雷达数据匹配、拼接、融合；

拼成完整室内平面地图，同时解算出机器人在地图中的坐标。

如下图所示：

SLAM 建图最终生成标准 ROS 栅格地图，由两个文件组成

map.pgm

灰度栅格图片，每个像素代表一个现实栅格区域

颜色规则：

黑色：障碍物、墙壁、不可通行

白色：空旷区域、可通行

灰色：未知未扫描区域

如下图所示：

map.yaml

地图配置说明书，记录地图关键参数，典型内容如下所示：

cpp 复制代码

image: map.pgm
resolution: 0.05
origin: [-10.0, -10.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0

字段解释：

resolution：地图分辨率，每个像素对应现实多少米。

origin：地图左下角在世界坐标系的原点。

occupied_thresh：超过该概率判定为障碍物。

free_thresh：低于该概率判定为可通行区域。

后续 NAV2 自主导航，就是加载这两个文件实现地图定位与路径规划。

二、SLAM Toolbox 简介

ROS2 官方主推 2D SLAM 开源方案。

替代 ROS1 的 gmapping、hector_mapping。

社区维护活跃，适配 Humble、Iron、Rolling 等版本。

对比维度	gmapping (ROS1 时代)	SLAM Toolbox (ROS2 时代)
算法核心	基于粒子滤波的传统算法	基于图优化与滤波结合的新一代算法
ROS 支持	仅原生支持 ROS1，ROS2 需通过桥接使用	原生支持 ROS2 Humble 及以上版本
对里程计依赖	强依赖，里程计漂移会直接导致地图重影	依赖较弱，激光匹配能力更强，抗漂移
地图稳定性	小场景可用，大场景易出现地图错位	稳定性更高，大场景也能保持地图对齐
功能扩展	仅支持在线建图	支持在线 / 离线建图、地图优化、重定位等
维护状态	基本停止更新	持续维护更新，适配新版本 ROS2

支持在线建图、参数可调、RViz 实时可视化。

安装 SLAM Toolbox

cpp 复制代码

sudo apt install ros-humble-slam-toolbox

三、仿真环境实现SLAM建图

本次实验我们在仿真环境中使用SLAM Toolbox进行SLAM建图。为了观察建图的过程，还会启动RViz2，实时显示建图的进展情况。建图时的数据通讯如下图所示：

3.1、创建 SLAM 功能包

进入工作空间 src 目录创建功能包，代码如下：

cpp 复制代码

cd ~/ros2_zice/src
ros2 pkg create slam_pkg

整体结构如下所示：

slam_pkg

├── launch

└── slam.launch.py

├── CMakeLists.txt

└── package.xml

3.2、编写 SLAM 启动 Launch 文件

在 slam_pkg 下新建launch 文件夹，新建 slam.launch.py，整体代码如下所示：

python 复制代码

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # SLAM基础配置参数
    slam_params = {
        "use_sim_time": True,        # 使用仿真时间
        "base_frame": "base_footprint",
        "odom_frame": "odom",
        "map_frame": "map",
        "map_update_interval": 1.0   # 每移动1米更新一次地图
    }

    # 启动SLAM建图节点
    slam_cmd = Node(
        package="slam_toolbox",
        executable="sync_slam_toolbox_node",
        parameters=[slam_params]
    )

    # 加载仿真自带RViz配置文件
    rviz_file = os.path.join(
        get_package_share_directory('wpr_simulation2'),
        'rviz',
        'slam.rviz'
    )

    # 启动RViz2可视化
    rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_file]
    )

    ld = LaunchDescription()
    ld.add_action(slam_cmd)
    ld.add_action(rviz_cmd)

    return ld

use_sim_time：仿真环境必须开启，同步 Gazebo 仿真时钟。

base_frame：机器人底盘坐标系。

odom_frame：里程计坐标系。

map_frame：全局地图坐标系。

map_update_interval：机器人移动指定距离才刷新地图，避免频繁更新卡顿。

3.3、配置 CMakeLists.txt

ROS2 功能包编译必须依赖 CMakeLists.txt，作用是声明编译版本、加载依赖包、配置文件安装规则。

这里我们用到了 SLAM 建图、激光雷达、地图消息、坐标变换等功能，需要逐一声明对应依赖；同时必须配置 launch 文件夹安装规则，否则 ros2 launch 无法找到启动文件。

完整总代码如下：

cpp 复制代码

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_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(slam_toolbox REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)

# 安装launch文件夹，让系统能识别launch启动文件
install(
  DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

# 测试相关配置
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

3.4、配置package.xml

package.xml 是 ROS2 功能包依赖声明文件，作用是告诉 ROS2 系统当前功能包依赖哪些官方消息包和工具包。凡是代码、Launch 中用到的功能，都必须在这里用 <depend> 声明依赖，缺少任意一个依赖都会导致编译报错、节点运行异常。

完整代码如下：

cpp 复制代码

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>slam_pkg</name>
  <version>0.0.0</version>
  <description>SLAM Toolbox 仿真环境激光建图功能包</description>
  <maintainer email="todo@todo.todo">todo</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <!-- 项目所需所有依赖 -->
  <depend>rclcpp</depend>
  <depend>slam_toolbox</depend>
  <depend>nav_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

四、编译与运行

4.1、编译

配置好所有文件后，回到工作空间编译，刷新环境变量，指令如下：

cpp 复制代码

cd ~/ros2_zice
colcon build
source install/setup.bash

4.2、仿真环境完整建图流程

4.2.1、启动 Gazebo 仿真场景

打开终端，输入以下代码：

cpp 复制代码

source ~/ros2_zice/install/setup.bash
ros2 launch wpr_simulation2 robocup_home.launch.py

作用：启动仿真环境、机器人模型、激光雷达、里程计驱动。

'如下图所示：

4.2.2、启动 SLAM 建图 + RViz2 可视化

打开终端 2，输入以下代码：

cpp 复制代码

source ~/ros2_zice/install/setup.bash
ros2 launch slam_pkg slam.launch.py

作用：启动 SLAM Toolbox 建图节点，自动打开 RViz2 实时显示建图过程。

如下图所示：

4.2.3、键盘控制机器人巡游建图

打开终端3，输入以下代码：

cpp 复制代码

source ~/ros2_zice/install/setup.bash
ros2 run wpr_simulation2 keyboard_vel_cmd

如下图所示：

按键控制说明：

W：向前直行

S：向后后退

A：向左平移

D：向右平移

Q：原地左转

E：原地右转

X：退出控制节点

现在我们就可以操控机器人移动了，这里我们先操控往前走了一段路

可以看到RViz2里已经在同步构图了，如下图所示：

稍微溜了一圈，可以如下所示：

在 RViz 中，

浅灰色是机器人确认的可通行区域。

黑色是障碍物墙壁。

深灰色是未扫描到的未知区域。

红色点是激光雷达实时扫描的障碍物数据。

中间的模型就是机器人的当前位置。

五、保存建好的栅格地图

建图轮廓完整、无空洞无重影后，在任意终端执行地图保存命令：

cpp 复制代码

ros2 run nav2_map_server map_saver_cli -f map

此时可以看到多了两个文件，如下图所示：

map.pgm：栅格地图图片。

map.yaml：地图配置文件。

总结

本期博客我们完成了基于 SLAM Toolbox 实现仿真环境 SLAM 建图实验。