使用LGSVL创建了激光雷达传感器,其中的原始点云PointCloud2类型的话题中含有intensity数据,并且在Rviz中可以将强度信息显示出来。
使用pcl::fromROSMsg()函数将ROS的PointCloud2类型的消息转成pcl::PointXYZI类型的数据后,出现了Failed to find match for field ‘intensity’ 的问题。
但是,在现实中使用pcl::fromROSMsg()处理激光雷达传感器的原始点云数据不会有问题。
在这里提供解决办法:
自己搭建转换函数,将PointCloud2类型的数据转换成pcl::PointXYZI类型
void callback(const sensor_msgs::PointCloud2ConstPtr &cloudIn)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);
// Get the field structure of this point cloud
int pointBytes = cloudIn->point_step;
int offset_x;
int offset_y;
int offset_z;
int offset_int;
for (int f=0; f<cloudIn->fields.size(); ++f)
{
if (cloudIn->fields[f].name == "x")
offset_x = cloudIn->fields[f].offset;
if (cloudIn->fields[f].name == "y")
offset_y = cloudIn->fields[f].offset;
if (cloudIn->fields[f].name == "z")
offset_z = cloudIn->fields[f].offset;
if (cloudIn->fields[f].name == "intensity")
offset_int = cloudIn->fields[f].offset;
}
// populate point cloud object
for (int p=0; p<cloudIn->width; ++p)
{
pcl::PointXYZI newPoint;
newPoint.x = *(float*)(&cloudIn->data[0] + (pointBytes*p) + offset_x);
newPoint.y = *(float*)(&cloudIn->data[0] + (pointBytes*p) + offset_y) ;
newPoint.z = *(float*)(&cloudIn->data[0] + (pointBytes*p) + offset_z);
newPoint.intensity = *(unsigned char*)(&cloudIn->data[0] + (pointBytes*p) + offset_int);
cloudPtr->points.push_back(newPoint);
}
sensor_msgs::PointCloud2 cloud_msg2;
pcl::toROSMsg(*cloudPtr,cloud_msg2);
}
参考:https://answers.ros.org/question/307881/failed-to-find-match-for-field-intensity-with-ouster-lidar/