ROS基础配置可参考《ROS2+Gazebo+VLA占位服务：纯仿真环境下的具身智能闭环实现》https://blog.csdn.net/weixin_55221858/article/details/15665962

SLAM Toolbox 建图原理

核心算法

SLAM Toolbox 是一款基于 2D 激光雷达 的 SLAM 方案，其核心架构区别于 Cartographer 的 submap 紧耦合 IMU 设计，而是主要依赖 位姿图优化（Pose Graph Optimization） 与 外部提供的里程计。

特性	说明
扫描匹配	将当前激光帧与历史帧或局部地图进行匹配，估计相对位姿
位姿图构建	将每一帧的位姿作为节点，帧间约束作为边，构建图结构
回环检测	当机器人回到已访问区域时，检测闭环并添加约束，消除累积漂移
图优化	使用 g2o 等优化库全局优化位姿图，修正历史轨迹
占据栅格地图	将优化后的激光数据投影为 2D Occupancy Grid Map

数据流

SLAM Toolbox 的输入与输出非常清晰：

输入:  /scan (LaserScan)  ← 激光雷达
       /odom (Odometry)   ← 轮式/视觉里程计
       /tf                ← 坐标变换树 (odom→base_link→laser)

输出:  /map (OccupancyGrid)  ← 占据栅格地图
       /tf (map→odom)        ← 定位修正变换

三种工作模式

模式	Launch 文件	适用场景
在线异步	online_async_launch.py	实时建图，不阻塞主线程，最常用
在线同步	online_sync_launch.py	实时建图，同步处理，精度略高但延迟大
离线建图	offline_launch.py	对录制的 bag 数据进行后处理建图

Nav2 导航原理

Nav2（Navigation 2）是 ROS2 官方导航栈，采用模块化 + 行为树（Behavior Tree）架构。

核心组件

组件	功能	典型插件
Planner Server	全局路径规划：从起点到目标点的无碰撞路径	NavFn, Smac Planner (Hybrid-A*)
Controller Server	局部轨迹控制：跟踪路径并实时避障	DWB, Regulated Pure Pursuit, MPPI
Recovery Server	恢复行为：导航失败时的自救策略	Clear Costmap, Spin, Backup, Wait
Behavior Tree	行为编排：以树状结构组织导航逻辑	NavigateToPose, PipelineSequence
Costmap 2D	代价地图：融合多层地图信息用于碰撞检测	Static Layer, Obstacle Layer, Inflation Layer

代价地图（Costmap）分层

Nav2 维护两张代价地图：

• Global Costmap：基于静态地图（/map）+ 全局障碍物层，用于全局规划

• Local Costmap：基于实时传感器（/scan）+ 局部障碍物层，用于局部避障

Costmap 层级:
┌─────────────────────────────────┐
│  Inflation Layer (膨胀层)        │  ← 为障碍物添加安全缓冲区
├─────────────────────────────────┤
│  Obstacle Layer (障碍层)         │  ← 实时传感器障碍物
├─────────────────────────────────┤
│  Static Layer (静态层)           │  ← SLAM 生成的地图 /map
└─────────────────────────────────┘

行为树（Behavior Tree）决策流

Nav2 使用 BehaviorTree.CPP 库，默认导航行为树逻辑如下：

NavigateToPose
├── ComputePathToPose (全局规划)
├── FollowPath (局部控制)
│   └── 循环: 速度指令 → /cmd_vel
├── 如果失败:
│   ├── ClearEntireCostmap (清除代价地图)
│   ├── Spin (原地旋转)
│   └── 重试规划
└── 到达目标 → 成功

SLAM Toolbox + Nav2 集成关系

坐标系依赖

整个系统的核心纽带是 TF2 坐标变换树，Nav2 对 TF 有严格要求：

map ──(SLAM发布)──→ odom ──(里程计发布)──→ base_link ──(URDF)──→ laser
 ↑                                                            ↑
 └────────────── Nav2 全局规划使用 ───────────────────────────┘
                                                              └─ SLAM 扫描匹配使用

• map → odom：由 SLAM Toolbox（或 AMCL）发布，修正里程计漂移

• odom → base_link：由里程计节点发布，反映短程高精度运动

• base_link → laser：由 robot_state_publisher 根据 URDF 发布

两种典型集成模式

模式 A：先建图，后导航（最常用）

Step 1: 启动机器人 + SLAM Toolbox → 遥控建图 → 保存地图
Step 2: 关闭 SLAM，启动 AMCL + Nav2 → 在已有地图上导航

保存地图命令：

ros2 run nav2_map_server map_saver_cli -f ~/map/my_map
# 生成: my_map.pgm (图片) + my_map.yaml (元数据)

模式 B：边建图边导航（Online SLAM + Nav2）

同时运行: SLAM Toolbox + Nav2 (不启动 AMCL 和 Map Server)
SLAM 实时发布 /map 和 map→odom 变换
Nav2 直接订阅 /map 进行规划

启动命令：

ros2 launch nav2_bringup navigation_launch.py
ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True

SLAM Toolbox、Nav2的安装

执行sudo apt update更新

执行sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup安装Nav2导航规划包，出现询问输入y再按回车，出现更新服务配置的紫色弹窗按回车即可

执行sudo apt install ros-humble-slam-toolbox安装二维SLAM包

执行cd ~/Project/turtlebot3_ws回到目录，以你目录为准，执行source install/setup.bash设置仿真包的路径，执行export TURTLEBOT3_MODEL=waffle_pi告知 ROS 2 要使用的机器人型号是 waffle_pi

执行ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py启动仿真环境

执行rviz2 来观察和监控SLAM建图的实时过程

执行ros2 launch slam_toolbox online_sync_launch.py启动SLAM Toolbox的建图节点，开始处理传感器数据并构建实时地图

点击Add按钮，然后再点击By topic，下拉选择/map的Map，点击OK

可以看到在建立地图的一部分

当然，也可以根据需要显示其他话题，比如相机Camera和激光雷达LaserScan

可通过执行ros2 run teleop_twist_keyboard teleop_twist_keyboard通过键盘控制机器人移动）或其他《ROS2+Gazebo+VLA占位服务：纯仿真环境下的具身智能闭环实现》（ROS2+Gazebo+VLA占位服务：纯仿真环境下的具身智能闭环实现_gazebo vla ros-CSDN博客）或其他教程方法

键盘控制的按键参考

u(左前)   i(前进)   o(右前)
j(左转)   k(停止)   l(右转)

m(左后)   ,(后退)   .(右后)

Shift + 上面任意键 = 全向平移模式（横着走）
t = 上升(z+)    b = 下降(z-)

速度调节：
q/z = 整体最大速度 ±10%
w/x = 只调线速度 ±10%
e/c = 只调角速度 ±10%

启动机器人运行一段时间，生成地图，蓝色是本次机器人经过的路线

保存地图

因为此时地图已经比较完整，新建终端，执行ros2 run nav2_map_server map_saver_cli -f ~/test_map保存地图

查看保存结果

自动导航

中断rviz，可选重新执行执行ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py启动仿真环境

执行rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz加载官方默认rviz配置

包含：

• Fixed Frame: 已设为 map

• Map 显示: 已订阅 /map 话题

• Global Costmap: 已订阅 /global_costmap/costmap

• Local Costmap: 已订阅 /local_costmap/costmap

• LaserScan: 已订阅 /scan

• Path: 已订阅 /plan

• Particle Cloud: 已订阅 /particlecloud（AMCL 粒子）

• Nav2 Goal Tool: 已配置好 2D Nav Goal 工具

• Nav2 Pose Tool: 已配置好 2D Pose Estimate 工具

如果只运行 rviz2，需要手动添加和配置所有上述显示，比较繁琐：

1. 左下角 Add → 逐个添加 Map、LaserScan、Path 等

2. 设置每个显示的话题

3. 左上方 Fixed Frame 手动改为 map

4. 工具栏手动配置 2D Pose Estimate 和 2D Nav Goal

执行ros2 launch nav2_bringup navigation_launch.py map:=~/test_map.yaml use_sim_time:=True，发现没有加载地图，是因为~解析失败，需要改成$HOME，确保能加载pgm文件的地图

使用仿真机器人时，use_sim_time必须设置为True，否则 Nav2 使用系统时间，Gazebo 使用仿真时间，导致 TF 时间戳不匹配；而真实机器人不需要设置

过几分钟加载好地图，注意到Global Status：Error

这时点击2D Pose Estimate设置机器人的大致初始位置，会出现绿色箭头，你需要点击机器人初始位置（一般是下方或右下角），然后箭头一般朝上（与机器人的实际朝向相同），红色虚线接近边缘说明找对位置，否则待会设置目标位置没有效果

下一步点击Nav2 Goal设置目标位置，可以在机器人能移动的范围任意设置，比如偏中右边位置，设置完后机器人会在rviz中移动到目标位置

机器人沿着导航路线移动中

机器人到达目标位置

继续设置新目标位置，同样会生成导航路线，机器人会缓慢移动过去

报错处理

 如果出现机器人卡住或超时，可尝试以下解决方法

执行sudo apt install ros-<你的Ubuntu版本名称>-rmw-cyclonedds-cpp，比如执行sudo apt install ros-humble-rmw-cyclonedds-cpp安装ros-humble-rmw-cyclonedds-cpp

然后执行export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

因为Fast-DDS 默认使用 多播发现，在某些网络环境（如 WSL、Docker、VPN、特定网卡配置）下会失败，所以可以尝试切换为对复杂网络环境更加友好、更稳健的Cyclone DDS 

再执行ros2 pkg list | grep rmw_cyclonedds

执行ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True map:=$HOME/test_map.yaml use_composition:=False

其中use_composition设置为False会禁用组件化启动，让每个 Nav2 节点作为独立进程运行，绕过 bad_weak_ptr 问题。

如果使用上面方法，出现Goal failed和[RTPS_READER_HISTORY Error] Change payload size of '32' bytes is larger than the history payload size of '19' bytes and cannot be resized. -> Function can_change_be_added_nts，机器人几乎不移动，或移动几秒就停下，建议重启，重启后可以先不切换Cyclone DDS 和设置use_composition看看效果

完整数据流与流程总结

端到端数据流

┌─────────────┐   ┌─────────────┐   ┌─────────────┐   ┌─────────────┐
│   传感器层   │ → │   TF坐标系   │ → │  SLAM/定位   │ → │   Nav2导航   │
│             │   │             │   │             │   │             │
│ • LiDAR     │   │ map→odom    │   │ 扫描匹配     │   │ 全局规划     │
│ • IMU       │   │ odom→base   │   │ 位姿图优化   │   │ 局部控制     │
│ • 编码器     │   │ base→laser  │   │ 栅格地图     │   │ 恢复行为     │
└─────────────┘   └─────────────┘   └─────────────┘   └──────┬──────┘
                                                              │
                                                              ↓
┌─────────────┐   ┌─────────────┐   ┌─────────────┐   ┌─────────────┐
│   执行层     │ ← │  ros2_control│ ← │   /cmd_vel   │ ← │  Controller │
│             │   │             │   │             │   │   Server    │
│ • 电机驱动   │   │ 差分控制器   │   │  Twist消息   │   │             │
│ • 轮速输出   │   │             │   │             │   │             │
└─────────────┘   └─────────────┘   └─────────────┘   └─────────────┘

阶段化操作流程

阶段	关键动作	验证点
① 建图	启动 SLAM Toolbox，遥控机器人遍历环境，确保回环闭合	RViz 中 /map 话题正常显示，灰色区域逐步消除
② 保存	map_saver_cli 保存 .pgm + .yaml	检查地图边缘清晰，无动态障碍物残留
③ 定位	加载地图，启动 AMCL，RViz 中点击 2D Pose Estimate	粒子云收敛，激光扫描与地图墙壁对齐
④ 导航	启动 Nav2，RViz 中点击 Nav2 Goal	全局路径出现，机器人沿路径行驶并避障
⑤ 联用	SLAM 异步模式 + Nav2 同时运行（可选）	地图动态更新，导航不中断

创作不易，禁止抄袭，转载请附上原文链接及标题