Go2w：FAST_LIO2 及建图

type

Post

status

Published

date

Mar 30, 2026

slug

summary

FAST_LIO2 及 2d地图生成

一、FAST_LIO2_foxy

1. 移植ROS2版本的FAST_LIO2

FAST_LIO2 的 ROS2 版本代码库（非唯一）

FAST_LIO_ROS2/include/ikd-Tree部分代码需要手动重新git

进行foxy版本的适配

在 nano 编辑器里，按 Ctrl + W，输入 save_map，然后按 Enter 键，就能直接跳转到报错的地方（大约在 945 行）。
注释下述代码
在大约950行补充地图保存代码

编译运行

2. 修改 xfer_format 参数为 livox custom format

修改文件代码livox_ros_driver2/launch_ROS2/msg_MID360_launch.py

3. 测试代码

终端 1：启动激光雷达节点

显示如下信息基本说明雷达节点正常启动，且输出imu和点云数据。

终端 2：启动FAST-LIO2 3D 建图引擎

Ctrl+C保存点云，点云保存位置见文件ros2_ws/src/FAST_LIO_ROS2/config/mid360.yaml
点云保存后可使用pcl_viewer查看。

4. 建图效果

二、地图保存

方法一：实时投影切片法 (Octomap)

1. 原理

利用 octomap_server 节点订阅 FAST-LIO2 实时发布的 /cloud_registered 话题。该节点内部维护一个八叉树地图，并将其垂直投影到平面，生成占用栅格地图（Occupancy Grid）。

2. 核心参数与调试策略

在你的实验中，我们总结出的重要参数如下如下：

参数	推荐值	作用说明
resolution	0.02 ~ 0.05	决定地图的精细度。0.02 时墙壁更平直，锯齿感少。
pointcloud_min_z	0.1	防噪关键。设为正数以彻底过滤机器狗行走产生的地面噪点。
pointcloud_max_z	0.5	限制高度，避免扫到食堂天花板或悬挂装饰物。
sensor_model.max_range	8.0 ~ 10.0	限制雷达视距，减少远端玻璃反光或散乱点云对地图边缘的干扰。

3. 实际效果

方法二：离线转换法 (PCD 转 PGM)

克隆pcd转pgm的库（不唯一）

修改参数文件ros2_ws/src/pcd2pgm/config/pcd2pgm.yaml

编译运行

转换效果

方法三：使用 `slam_toolbox` 联合建图

核心架构逻辑

FAST-LIO2（腿）：继续跑，但只用它提供的精准 TF 坐标变换（里程计），保证机器狗知道自己走了多远。

pointcloud_to_laserscan（眼睛）：加一个中间节点，像激光剑一样，在机器狗水平高度横切一刀，把 Mid-360 的 3D 点云变成传统的 2D 激光雷达数据（LaserScan）。

slam_toolbox（大脑）：吃进 2D 激光数据和里程计，画出完美的 2D 栅格地图。

🟢 1.启动雷达驱动

检查点：启动后稍微等几秒，让雷达完成内部的初始化和对时。

🟢 2.启动 3D 核心 FAST-LIO2

检查点：切到 RViz 里看一眼，确认点云是不是又正常出来了，并且终端里出现了 IMU Initial Done。

状态：此时，高精度的 3D 里程计（从 camera_init 到 body）已经在后台稳定发送了。

🟢 3.启动“降维切片”

现在，我们把 FAST-LIO2 洗干净的 3D 点云切成 2D 激光扫描。

检查点：这个节点运行后通常不会疯狂打印日志。你可以在 RViz 里点击 Add -> By topic，看看是不是多了一个 /scan 的话题。

🟢 4.启动 SLAM Toolbox 大脑

为了避免配置文件互相打架，我们直接用带参数的 ros2 run 命令强制它匹配 FAST-LIO2 的坐标系：

🟢 5.在 RViz 里见证 2D 地图生长

现在回到 FAST-LIO2 自动弹出的那个 RViz 窗口，做两件事：

把左侧面板最上方的 Fixed Frame 从 camera_init 改成 map。（这代表我们将以全局 2D 上帝视角来观察）。

点击左下角 Add -> By topic，找到 /map，双击把它加进来。

建图效果如下所示

保存2d地图

🟢 6.一键启动脚本

保存地图和pcd文件

三、注意事项

当雷达（Mid-360）还没完全启动好、时间戳没对齐时，如果你提前启动了 FAST-LIO2，终端里会上演一场“连环车祸”。

以后如果你在终端看到以下 3 个标志性报错，不用怀疑，100% 是时间没对齐（启动太快了）：

🚩 核心罪魁祸首

在运行 fast_lio 的那个终端里，你会看到这两行疯狂刷屏或紧挨着出现：

黑话翻译：time diff is -2.98... 意思是算法发现 IMU 的时间和雷达点云的时间差了将近 3 秒（通常是因为 IMU 瞬间供电发数据，而雷达电机转起来花了 3 秒）。

系统反应：clear buffer 意思是算法判定时间发生了“时光倒流（loop back）”的严重错误，为了防止计算出满天乱飞的错误地图，它直接把接收到的数据全部扔进了垃圾桶（清空缓存）。

🚩 连带并发症 1：RViz 找不到北

因为 FAST-LIO2 把数据都扔了，它就无法计算出机器狗的位置，自然也就不会向 ROS 2 系统发布坐标系。此时你的 RViz 终端（或者界面上）会报这个错：

黑话翻译：RViz 说：“喂！我要画图了，但地图的起点（camera_init）在哪里？机器狗的身体（body）在哪里？谁来给我发个坐标系啊！”

🚩 连带并发症 2：保存时的绝望崩溃

当你发现没画面，无奈按下 Ctrl + C 想要结束程序时，终端会给你表演一个“当场去世”：

黑话翻译：因为算法全程都在 clear buffer，内存里一滴点云都没有。当你按下退出键触发保存地图代码时，点云库（PCL）发现你要保存一个完全空的空气，直接抛出 has no data 异常并强制崩溃退出。

💡

有关问题，欢迎您在底部评论区留言，一起交流~