FAST_LIO项目中的地图保存问题分析与解决方案

问题背景

在使用FAST_LIO项目进行激光雷达SLAM建图时，许多用户遇到了地图无法保存的问题。该问题主要出现在Ubuntu 22.04系统搭配ROS2 Humble版本的环境中，当用户按照配置文件设置了pcd_save_en为true后，系统仍然无法自动保存点云地图文件。

问题现象分析

从用户报告的情况来看，主要表现出以下几种现象：

1. 配置文件中的pcd_save_en参数已设置为true，但程序运行结束后没有生成预期的PCD文件
2. 部分用户在尝试手动调用map_save服务时，服务无响应或报错
3. 更严重的情况下，程序会直接崩溃并抛出pcl::IOException异常，提示"Input point cloud has no data"

根本原因探究

经过技术分析，这些问题主要源于以下几个方面：

1. 目录路径问题：FAST_LIO中使用的ROOT_DIR变量在某些非标准目录结构下无法正确解析，导致程序无法找到预期的保存路径

2. 资源竞争问题：ROS2的launch系统会在程序结束前发送SIGKILL信号，而地图保存操作可能还未完成就被强制终止

3. 点云数据验证缺失：在保存PCD文件前，程序没有充分验证点云数据是否有效，导致当点云为空时直接抛出异常

4. 代码注释问题：部分关键代码被注释掉，导致保存功能实际上并未启用

解决方案

针对上述问题，我们提供以下几种解决方案：

方案一：手动指定保存路径

对于目录路径问题，最简单的解决方案是直接修改源代码，将ROOT_DIR硬编码为你希望保存地图的绝对路径。这样可以避免路径解析错误导致的问题。

方案二：优化保存时机

为了避免被ROS2的launch系统提前终止，可以采用以下两种方式：

1. 在程序退出前增加延时，确保有足够时间完成保存操作
2. 使用独立的保存服务，在需要时手动触发保存操作

方案三：修复源代码

对于代码被注释导致的问题，可以按照以下步骤修复：

1. 找到laserMapping.cpp文件中的相关代码段（约513行处）
2. 取消以下关键代码的注释状态：

   if (pcd_save_en)
   {
       int size = feats_undistort->points.size();
       PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1));
       
       for (int i = 0; i < size; i++)
       {
           RGBpointBodyToWorld(&feats_undistort->points[i], &laserCloudWorld->points[i]); 
       }
       *pcl_wait_save += *laserCloudWorld;
       
       static int scan_wait_num = 0;
       scan_wait_num++;
       if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) 
       { 
           pcd_index ++;
           string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));
           pcl::PCDWriter pcd_writer;
           cout << "current scan saved to /PCD/" << all_points_dir << endl;
           pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
           pcl_wait_save->clear();
           scan_wait_num = 0;
       }
   }
   

方案四：增加数据验证

对于点云数据为空导致的异常，可以在保存前增加数据验证逻辑：

if(pcl_wait_save->size() == 0) {
    ROS_WARN("Attempted to save empty point cloud, skipping...");
    return;
}

最佳实践建议

1. 测试保存功能：在正式使用前，先用小范围数据测试地图保存功能是否正常工作
2. 监控保存过程：通过查看程序输出，确认是否出现"current scan saved to /PCD/"的提示信息
3. 合理设置保存间隔：根据硬件性能调整pcd_save_interval参数，避免频繁保存影响实时性能
4. 确保磁盘空间：大规模建图时需要确保有足够的磁盘空间保存点云数据

总结

FAST_LIO项目中的地图保存问题主要源于路径解析、资源竞争和代码实现三个方面。通过本文提供的解决方案，用户可以有效地解决地图无法保存的问题。建议用户根据自身环境选择最适合的解决方案，并在实际应用中注意监控保存过程，确保建图结果能够正确持久化。