镭神雷达在lio-sam中使用的数据格式修改

本文主要是介绍镭神雷达在lio-sam中使用的数据格式修改，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

有两点修改，一、重新定义点云类型，二、修改点云时间

重新定义点云类型

// leishen lidar
struct LeiShenPointXYZIRT {PCL_ADD_POINT4DPCL_ADD_INTENSITY;uint16_t ring;double time;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (LeiShenPointXYZIRT,(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)(uint16_t, ring, ring) (double, time, time)
)

// using PointXYZIRT = VelodynePointXYZIRT;
using PointXYZIRT = LeiShenPointXYZIRT;

重新定义cachePointCloud（）函数

bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{      // cache point cloudcloudQueue.push_back(*laserCloudMsg);   //点云数据加入队列if (cloudQueue.size() <= 2)             //检查点云队列是否小于等于2,满足就直接返回false，数据量太少return false;// convert cloud//取出队列中第一帧数据currentCloudMsg = std::move(cloudQueue.front());currentCloudMsg.header.frame_id = lidarFrame; // "velodyne";//第一帧数据从队列中删除cloudQueue.pop_front();// void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)// 首先由ROS的sensor_msgs::PointCloud2类型转换为PCL的pcl::PCLPointCloud2类型，// 然后再由PCL的pcl::PCLPointCloud2类型转换为PCL的pcl::PointCloud<T>pcl::moveFromROSMsg(currentCloudMsg, *tmpCloudIn);// laserCloudIn->points.resize(tmpCloudIn->size());  //points容器重新划分大小 64512laserCloudIn->is_dense = true;laserCloudIn->clear();double timestamp = currentCloudMsg.header.stamp.toSec();for (size_t i = 0; i < tmpCloudIn->size(); i++) {if (std::isnan(tmpCloudIn->points[i].x) ||std::isnan(tmpCloudIn->points[i].y) ||std::isnan(tmpCloudIn->points[i].z)) {continue;}PointXYZIRT tmpCloudPoint;auto &src = tmpCloudIn->points[i];if(abs(src.x) > 0.000000 || abs(src.y > 0.000000) || abs(src.z) > 0.000000)   //去除无效点作用{tmpCloudPoint.x = src.x;tmpCloudPoint.y = src.y;tmpCloudPoint.z = src.z;tmpCloudPoint.intensity = src.intensity;tmpCloudPoint.ring = src.ring;// dst.time = timestamp - src.time / 1e9;tmpCloudPoint.time = src.time;    //VELODYNE时间为每个点相对第一个点的时间差，越往后越大，正数;雷神时间为每个点与最后一个点的时间差，越往后越接近0,负数laserCloudIn->push_back(tmpCloudPoint);   //将该点加入容器中// if(abs(tmpCloudPoint.time) > 0.000000)//     laserCloudIn->push_back(tmpCloudPoint);}}if((int)laserCloudIn->size() > 0){double begintime = laserCloudIn->points.front().time;for(size_t i = 0; i < laserCloudIn->size(); i++)laserCloudIn->points[i].time = laserCloudIn->points[i].time - begintime;        //转换雷达时间戳，与velodyne格式相同// get timestampcloudHeader = currentCloudMsg.header;timeScanCur = cloudHeader.stamp.toSec();  //一帧数据起始时间timeScanEnd = timeScanCur + laserCloudIn->points.back().time;   }// check dense flag// 判断点云数据是否是密集模式     不是密集模式的话请移除空值点if (laserCloudIn->is_dense == false){ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");ros::shutdown();}// check ring channelstatic int ringFlag = 0;if (ringFlag == 0){ringFlag = -1;for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i){if (currentCloudMsg.fields[i].name == "ring")  //检查fields中是否存在ring{ringFlag = 1;break;}}if (ringFlag == -1){ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");ros::shutdown();}}// check point timeif (deskewFlag == 0){deskewFlag = -1;for (auto &field : currentCloudMsg.fields){if (field.name == "time" || field.name == "t") //检查fields中是否存在time或者t{deskewFlag = 1;break;}}if (deskewFlag == -1)ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");}return true;
}