使用pcl::fromROSMsg()函数报错Failed to find match for field ‘intensity‘

使用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/

你可能感兴趣的:(LGSVL,无人驾驶,PCL,自动驾驶)