32. 地图保存、定位与坐标验证¶
第 31 章运行 SLAM 时，建图节点会根据雷达扫描和机器人运动不断更新一张二维地图，并通常发布 /map 话题。该话题的消息类型一般是 nav_msgs/msg/OccupancyGrid，表示由许多小方格组成的占据栅格：每个方格记录这里是空闲、被占用，还是尚未观测到。
/map 可以直接交给 RViz2 显示。打开 RViz2 后，把 Fixed Frame 设为 map，再添加 Map 显示并选择 /map，就能看到正在生成的地图；同时添加 LaserScan 和 TF，可以观察当前扫描是否落在墙面附近。
地图话题不只用于可视化。建图完成后，通常还要把它保存为文件，之后由地图服务器重新发布，交给定位节点和下一章介绍的导航框架使用：
SLAM 发布 /map
├── RViz2：实时显示地图
├── 地图保存工具：保存为 .pgm + .yaml
└── 地图服务器：重新发布 /map
├── 定位节点：估计机器人在地图中的位置
└── 导航框架：规划路径并控制机器人
32.1 保存地图¶
停止机器人运动后，使用当前 ROS 2 发行版和 SLAM 方案提供的保存工具。常见命令形式是：
ros2 run nav2_map_server map_saver_cli -f ~/maps/office_map
通常会得到两个文件：
office_map.pgm # 栅格地图图像
office_map.yaml # 地图分辨率、原点和阈值等元数据
.pgm 保存每个栅格的图像数据，.yaml 告诉地图服务器如何解释这张图。例如：
image: office_map.pgm
resolution: 0.05
origin: [-10.0, -8.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.25
negate: 0
resolution: 0.05 表示每个栅格边长约 0.05 m；origin 表示图像原点在 map 坐标系中的位置和方向。移动或重命名 .pgm 后，必须同步修改 yaml 中的 image 路径。保存后应打开地图图像，确认墙体没有明显断裂、拉伸或整体旋转。
32.2 从建图切换到定位¶
建图和定位是两个不同阶段：
建图：当前雷达扫描 + 运动估计 → 机器人位姿 + 新地图
定位：已有地图 + 当前雷达扫描 + 运动估计 → 机器人在地图中的位姿
建图阶段允许 SLAM 修改 /map；定位阶段地图通常保持不变，定位节点只回答“机器人现在位于这张地图的什么位置”。因此，完成保存后，通常停止 SLAM，启动地图服务器和定位节点，不要同时让另一套 SLAM 继续发布地图或 map → odom。
定位启动链可以理解为：
- 地图服务器读取
office_map.yaml，发布/map； - 雷达驱动发布当前
LaserScan； - 底盘节点发布里程计和
odom → base_link； - 静态 TF 节点提供
base_link → laser； - 定位节点比较当前扫描与地图，发布
map → odom； - TF2 将三段关系组合，得到机器人在地图中的
base_link位姿。
定位系统估计 map → odom，底盘里程计提供 odom → base_link，静态发布器或机器人模型提供 base_link → sensor（实际雷达坐标常命名为 laser）。完整链路可以写成 map → odom → base_link → sensor。每段关系只能有一个明确发布者。
32.3 设置初始位姿¶
仅加载地图并不能自动知道机器人当前位于地图哪一处。启动定位后，在 RViz2 中使用“设置初始位姿”工具：先在地图上点击机器人所在位置，再沿车头方向拖动箭头。定位节点会以此作为初始猜测，用后续雷达扫描和里程计逐步修正。
如果初始位置相差太远、方向完全相反，或者环境中有许多相同的走廊，定位可能无法收敛。此时先重新设置初始位姿，或把机器人移动到有明显墙角、门框等特征的位置，不要马上修改地图分辨率。
32.4 在 RViz2 和命令行中验证¶
在 RViz2 中建议同时显示 Map、LaserScan、TF 和 RobotModel，分别确认地图已经加载、当前扫描与墙面基本重合、坐标树完整、车体方向与实际一致。
命令行可以进一步确认实际接口：
ros2 topic info /map --verbose
ros2 topic echo --once /map
ros2 run tf2_ros tf2_echo map base_link
ros2 run tf2_tools view_frames
/map 用于确认地图发布者和消息类型，tf2_echo map base_link 用于查看机器人在地图中的当前位姿，view_frames 用于检查坐标树是否断开或存在重复发布者。话题名和坐标名必须替换成当前系统的实际名称。
让机器人低速直行、缓慢转弯，再回到起始位置，观察扫描是否持续贴合地图中的固定墙面。若地图中的墙和当前扫描逐渐分开，优先检查第 29～31 章已经介绍的扫描、里程计、TF 和时间信息。
32.5 定位之后还能做什么¶
定位稳定后，/map 就可以交给后续节点使用：
- 导航框架使用地图和
map → base_link位姿建立全局、局部代价地图； - 规划器根据目标点计算可通行路径；
- 控制器根据局部障碍和路径输出
cmd_vel； - 多点导航、路径跟踪和自主探索复用同一套地图、定位、底盘和 TF。
因此，地图保存不是流程终点，而是把一次建图结果变成可重复加载、可定位、可导航的系统输入。进入第 33 章前，应记录地图文件、分辨率、原点、雷达 frame_id、定位参数和实际发布 map → odom 的节点。
32.6 常见问题¶
| 现象 | 优先检查 | 常见处理 |
|---|---|---|
| RViz2 没有地图 | /map 是否发布、Fixed Frame、地图 QoS |
启动地图服务器，Fixed Frame 设为实际 map |
| 地图加载失败 | yaml 的 image 路径和文件权限 |
修正路径，确认 .pgm 与 .yaml 在同一目录或路径正确 |
| 初始位姿后扫描仍错位 | 初始位置和方向、base_link → laser、地图是否过时 |
重新设置初始位姿，检查雷达安装 TF |
| 机器人位姿突然跳动 | 是否有多个节点发布 map → odom，时间戳和里程计方向 |
停止重复定位或 SLAM 节点，修复输入链路 |
| 行驶越远偏差越大 | 轮径轮距、车轮打滑、雷达频率和环境变化 | 先修正里程计和扫描质量，再调整定位参数 |
| 定位正常但导航不工作 | 地图话题、全局坐标名、TF 链和导航参数 | 先完成本章接口验证，再进入第 33 章 |