前言

在移动机器人开发中，让机器人“看见”环境并“知道”自己在哪，是所有高级功能（如导航、避障）的基础。这就涉及到 SLAM (Simultaneous Localization and Mapping)。

本文记录了我基于 ROS 2 Humble、ESP32 (Micro-ROS) 和 YDLidar 搭建 SLAM 系统的完整过程。从最底层的串口通信，到 TF 坐标变换树的构建，再到最终使用 SLAM Toolbox 完成建图。

这不仅仅是一份操作指南，更是一次对 ROS 2 核心概念（话题、TF、时间同步）的深度复盘。

---

1. 系统架构：它们是如何协作的？

一个完整的 SLAM 系统就像一个团队，各个节点各司其职。为了让 SLAM 算法跑起来，我们需要打通以下数据链路：

1. 底盘 (ESP32 + Micro-ROS)：负责运动控制和里程计计算。它告诉系统“我走了多远”。

2. 雷达 (YDLidar)：机器人的眼睛，负责扫描周围障碍物的距离。

3. 坐标变换节点 (Gulugulu Node)：关键中间件。它负责将底盘的里程计数据转换成 ROS 的 TF 坐标系。

4. 模型描述 (URDF)：定义机器人的身体结构（如雷达安装在底盘什么位置）。

5. 大脑 (SLAM Toolbox)：融合里程计和雷达数据，修正误差，绘制地图。

---

2. 核心步骤详解

第一步：打通底盘通信 (Micro-ROS Agent)

底盘使用 ESP32 主控，通过 WiFi 与电脑通信。

• 发布话题：/odom (nav_msgs/Odometry) —— 包含位置(x, y)和姿态(四元数)。

• 订阅话题：/cmd_vel (geometry_msgs/Twist) —— 接收速度指令。

遇到的坑与解决：

• 问题：启动 Agent 后没反应，话题无数据。

• 解决：Micro-ROS 依赖 Session 机制。启动 Agent 后，必须按下 ESP32 的 RST (复位) 键，强制重新建立连接，直到看到 Session established。

第二步：雷达驱动与“TF 冲突”之谜

雷达通过 WiFi 透传板连接电脑，我们使用 ydlidar_ros2_driver 发布 /scan 话题。

遇到的坑与解决（重点）：

• 问题：雷达正常转，但地图上的点云乱跳，无法建图。

• 原因：双重发布冲突。

  • 雷达驱动里自带了一个 static_transform_publisher，默认发布 base_link -> laser_frame。

  • 我的 URDF 模型里也定义了 base_link -> laser_frame。

  • ROS 系统同时收到两个不同的坐标变换，导致 TF 树震荡。

• 解决：修改 ydlidar_launch.py，注释掉驱动自带的 TF 发布节点，只保留数据驱动。

Python

# ydlidar_launch.py 修改片段
return LaunchDescription([
    params_declare,
    driver_node,
    # tf2_node,   <--- 必须注释掉这一行，把 TF 权限交给 URDF
])

第三步：手写坐标变换节点 (Gulugulu Node)

这是整个系统中最关键的“桥梁”。SLAM 算法不直接读取 /odom 话题，它依赖 TF 树。我们需要写一个 C++ 节点，做以下转换：

• 输入：订阅 /odom 话题。

• 输出：广播 odom -> base_footprint 的 TF 变换。

核心代码逻辑 (C++)：

C++

// 关键点：时间戳必须同步！
// 不能用 this->now()，必须用 msg->header.stamp
// 否则会导致雷达数据和里程计数据时间对不齐，地图产生重影。
transform.header.stamp = msg->header.stamp; 
transform.header.frame_id = "odom";
transform.child_frame_id = "base_footprint";
tf_broadcaster_->sendTransform(transform);

第四步：机器人的身体 (URDF)

我们需要告诉 SLAM 算法雷达安装在哪里。通过 robot_state_publisher 加载 URDF 文件，发布静态 TF：

• base_footprint -> base_link (底盘高度)

• base_link -> laser_frame (雷达安装位置)

至此，一棵完整的 TF 树建立完毕： odom -> base_footprint -> base_link -> laser_frame

第五步：启动大脑 (SLAM Toolbox)

一切准备就绪，启动 SLAM Toolbox。

• 命令：ros2 launch slam_toolbox online_async_launch.py use_sim_time:=False

• 注意：在真机上运行，use_sim_time 必须设为 False。如果设为 True，节点会等待仿真时间信号，导致程序卡死。

SLAM 算法工作后，会发布最顶层的变换：map -> odom，此时 TF 树完全闭合。

---

3. 最终启动流程 (Cheatsheet)

为了让系统跑起来，我们需要开启 5 个终端，顺序如下：

1. 底盘连接：

   sudo docker run ... microros/micro-ros-agent ...
   # 启动后按 ESP32 复位键！
   

2. 雷达数据透传：

   python3 ~/fishbot_ws/wifi_laser.py
   

3. 雷达驱动：

   ros2 launch ydlidar ydlidar_launch.py
   

4. 模型与坐标变换 (Gulugulu)：

   ros2 launch gulugulu fishbot_bringup.launch.py
   

5. SLAM 建图：

   ros2 launch slam_toolbox online_async_launch.py use_sim_time:=False
   

---

4. 成果与总结

在 RViz2 中将 Fixed Frame 设置为 map，添加 RobotModel 和 LaserScan。

现象：

• 蓝色的机器人模型出现在屏幕中央。

• 红色的雷达点云描绘出了房间的轮廓。

• 随着遥控机器人移动 (teleop_twist_keyboard)，地图逐渐变大、变清晰。

关于“漂移”的思考： 刚开始建图时，可能会发现走久了地图会“歪”。这是因为单纯依赖轮子编码器（里程计）会有累计误差（轮子打滑、地面不平）。 SLAM Toolbox 的强大之处在于 回环检测 (Loop Closure)。当你控制机器人绕一圈回到原点时，算法会识别出“我来过这里”，并瞬间修正之前的累积误差，把地图“拉直”。