mid360激光雷达fast_lio 源码解析（一）

公布的主要topic：

1.话题Topic："/cloud_registered"，配准后的单帧点云，消息类型：sensor_msgs::PointCloud2，以IMU为基准坐标系，当前帧往第一帧配准。

ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered", 100000);

  if (scan_pub_en || pcd_save_en)      publish_frame_world(pubLaserCloudFull);//调用公布函数

2.公布函数：

void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
{
    if(scan_pub_en)  //是否公布，在/FAST_LIO-main/config/mid360.yaml文件中设置
    {
        PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);//是否公布体素网格降采样后的点云，dense_pub_en在mid360.yaml文件中设置，采样参数在/FAST_LIO-main/launch/mapping_mid360.launch文件中设置参数filter_size_surf
        int size = laserCloudFullRes->points.size();
        PointCloudXYZI::Ptr laserCloudWorld( \
                        new PointCloudXYZI(size, 1));//配准后的点云

        for (int i = 0; i < size; i++)
        {
            RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
                                &laserCloudWorld->points[i]);
        }//雷达点云配准并变换至IMU坐标系

        sensor_msgs::PointCloud2 laserCloudmsg;
        pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
        laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
        laserCloudmsg.header.frame_id = "camera_init";//点云转换为sensor_msgs::PointCloud2类型
        pubLaserCloudFull.publish(laserCloudmsg);//公布
        publish_count -= PUBFRAME_PERIOD;
    }

    /**************** save map ****************/
    /* 1. make sure you have enough memories
    /* 2. noted that pcd save will influence the real-time performences **/
    if (pcd_save_en)//是否保存PCD文件，在mid360.yaml文件中设置
    {
        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]);//雷达点云配准并变换至IMU坐标系
        }
        *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文件，pcd_save_interval在mid360.yaml文件中设置
        {
            pcd_index ++;
            string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));//pcd文件保存路径
            pcl::PCDWriter pcd_writer;
            cout << "current scan saved to /PCD/" << all_points_dir << endl;
            pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);//写pcd文件
            pcl_wait_save->clear();//清空内存中历史点云
            scan_wait_num = 0;
        }
    }
}