slam_toolbox 总览与架构

01 总览与架构

1.1 它解决什么问题

slam_toolbox 是 2D 激光 SLAM，给室内 AMR / AGV 用。输入是 sensor_msgs/LaserScan 和 TF 里的轮速里程计，输出是占用栅格 /map 和 map → odom。

相对“扫一圈存 pgm”多出来的能力：

• 位姿图无损序列化，下次继续建图或做弹性定位
• 同步 / 异步两种处理节奏
• 定位模式：已有图 + 滚动窗口，不改底层地图
• RViz 交互：手动挪节点、人工回环
• 实验性 lifelong（按 IoU 删旧节点）、去中心化多机器人

匹配内核是工业界评价很高的 Karto 相关扫描匹配。作者 Steve Macenski，论文：Macenski & Jambrecic, JOSS 2021。

它不是 3D LIO，也不是视觉 SLAM。RGB-D / 多传感器看 RTAB-Map。

1.2 三层结构

┌─────────────────────────────────────────────┐
│  ROS 封装  include/ + src/                  │
│  LifecycleNode、TF、服务、栅格化、RViz     │
├─────────────────────────────────────────────┤
│  模式层  async / sync / localization / …    │
│  只决定：何时、以哪种 Process* 送进 Mapper  │
├─────────────────────────────────────────────┤
│  Karto  lib/karto_sdk/                      │
│  SequentialScanMatcher + MapperGraph        │
│  回环后调用 ScanSolver（Ceres 插件）        │
└─────────────────────────────────────────────┘

代码量大致：ROS 封装约 4.7k 行，Karto Mapper.cpp 约 3.3k 行，Ceres 插件约 0.5k 行。真正的前端/回环在 Karto；ROS 层负责时间、坐标系和产品接口。

1.3 目录

slam_toolbox/
├── include/slam_toolbox/     头文件，从 common 读起
├── src/                      公共层 + 各模式 + 工具
│   └── experimental/         lifelong、建图/定位切换
├── lib/karto_sdk/            Open Karto 改版
│   ├── include/karto_sdk/    Mapper.h / Karto.h / Math.h
│   └── src/                  Mapper.cpp（核心）、Karto.cpp
├── solvers/                  Ceres 插件（G2O/SPA/GTSAM 源码在但默认不编）
├── rviz_plugin/              交互面板
├── launch/  config/  srv/  msg/

可执行文件（CMakeLists.txt）：

可执行文件	类	用途
async_slam_toolbox_node	AsynchronousSlamToolbox	在线建图（首选）
sync_slam_toolbox_node	SynchronousSlamToolbox	离线/不丢帧
localization_slam_toolbox_node	LocalizationSlamToolbox	已有图定位
lifelong_slam_toolbox_node	LifelongSlamToolbox	实验性终身建图
map_and_localization_slam_toolbox_node	MapAndLocalizationSlamToolbox	运行时切建图/定位
decentralized_multirobot_slam_toolbox_node	多机	交换 LocalizedScan
merge_maps_kinematic	工具	运动学拼图

*_node.cpp 只有 main：构造对应类，spin 其 LifecycleNode 的 base interface。

1.4 类继承

rclcpp_lifecycle::LifecycleNode
        │
        ▼
   SlamToolbox                         src/slam_toolbox_common.cpp
        │  laserCallback() = 0
   ┌────┼──────────────┬────────────────────┐
   ▼    ▼              ▼                    ▼
 Async Sync      Localization         DecentralizedMR
                    │
                    ▼
            MapAndLocalization         experimental

SlamToolbox 持有：

• SMapper：包一层 karto::Mapper
• karto::Dataset：雷达模型 + 扫描对象（序列化用）
• pluginlib::ClassLoader<karto::ScanSolver> → 默认 CeresSolver
• 一组助手：GetPoseHelper、LaserAssistant、MapSaver、LoopClosureAssistant

模式子类几乎只覆盖 laserCallback 和反序列化时的 ProcessType 限制。

1.5 线程模型

on_activate 里起两条 boost::thread（同步模式再加一条处理线程）：

线程	函数	周期
TF	publishTransformLoop	transform_publish_period（默认 0.02–0.05 s）
可视化	publishVisualizations	map_update_interval（建图配置里常 2–5 s）
同步处理	SynchronousSlamToolbox::run	100 Hz 出队

ROS 回调在 executor 线程。进 Karto 前要拿 smapper_mutex_。map_to_odom_ 另有 map_to_odom_mutex_。

on_deactivate 对线程 interrupt + join。循环里有 interruption_point()。

1.6 和 Nav2 的边界

slam_toolbox 只负责：

• 发 map → odom（回环时这条边跳）
• 发 /map（占用栅格，QoS：KeepLast(1) + Transient Local + Reliable）
• 发 pose（PoseWithCovarianceStamped，map 系）

底盘必须自己发 odom → base_footprint。Nav2 消费地图和 TF，不进 Karto。