在ROS中订阅点云话题的时候，需要先将数据类型转换成PCL格式之后再做操作。
方式一：
直接调用pcl自带的函数
pcl::fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<t> &pcl_cloud)</t>
需要添加头文件 #include <pcl_conversions pcl_conversions.h></pcl_conversions>
使用举例：
void callback(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr colorcloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*in_cloud_ptr, *colorcloud);
｝
这个方式使用比较简单，一般都使用方式一。
方式二：
目前我使用realsense相机遇到一个问题，订阅得到的初始点云话题数据量过大，有307200个点云，使用pcl::fromROSMsg转成pcl点云，再进行降采样，这两步会用去大量的时间，在对程序运算速度有要求的时候并不好用。
因此采用mencpy的方式，直接从地址提取部分点云，可以有效节省时间。
memcpy函数用法：
void *memcpy(void *dest, const void *src, size_t n);
第一个参数是接收数据的参数，第二个参数是数据的地址，第三个参数是数据的大小。关键就在于第二和第三个参数，先看ros话题的消息格式：
再查看几个重要的参数：
sensor_msgs::PointCloud2 output_pc;
pcl::toROSMsg(cloud,output_pc);
std::cout<<"width: "<<(int)output_pc.width<<std::endl;
std::cout<<"height: "<<(int)output_pc.height<<std::endl;
std::cout<<"point_step: "<<(int)output_pc.point_step<<std::endl;
std::cout<<"row_step: "<<(int)output_pc.row_step<<std::endl;
std::cout<<"fields_size: "<<(int)output_pc.fields.size()<<std::endl;
以我这个为例，我是从深度图像转换成的点云，所有是640*480，point_step代表每个点存储了32个字节，
row_step就是width×point_step的大小，
fields_size表示每个点中有几个数据。
再查看fields里面的具体内容：
std::cout<<"fields0: "<<(int)output_pc.fields[0].datatype<<" "<<output_pc.fields[0].name<<std::endl;
std::cout<<"fields1: "<<(int)output_pc.fields[1].datatype<<" "<<output_pc.fields[1].name<<std::endl;
std::cout<<"fields2: "<<(int)output_pc.fields[2].datatype<<" "<<output_pc.fields[2].name<<std::endl;
std::cout<<"fields3: "<<(int)output_pc.fields[3].datatype<<" "<<output_pc.fields[3].name<<std::endl;
datatype是7，对应的是float32。也就是说每个点都有x,y,z,rgba四个数据，数据类型都是float32，占四个字节。这四个数据的存储方式如下图所示：
上面提到point_step是32，也就是4字节✖8，x,y,z占用前面12个字节，再空出4个字节后，存储rgba,再空出12个字节。
所有点云的数据是保存在 uint8[] data中的，我们只要每隔32个字节，把其中对应的数据取出，就完成了ROS话题转换pcl话题：
void callback(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pclcloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
for(int i=0;i<in_cloud_ptr->width*in_cloud_ptr->height;i++)
{
pcl::PointXYZRGBA p;
std::memcpy(&p.x,&in_cloud_ptr->data[32*i],4);
std::memcpy(&p.y,&in_cloud_ptr->data[32*i+4],4);
std::memcpy(&p.z,&in_cloud_ptr->data[32*i+8],4);
std::memcpy(&p.rgba,&in_cloud_ptr->data[32*i+16],4);
pclcloud->points.push_back(p);
}
}
这样就实现了从地址中读取ros点云数据。
为了省去之后的降采样的工作，可以只读取部分点云的数据：
void callback(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pclcloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
for(int i=0;i<in_cloud_ptr->width*in_cloud_ptr->height;i+=4)
{
if((i/in_cloud_ptr->width)%4!=0)
{
continue;
}
pcl::PointXYZRGBA p;
std::memcpy(&p.x,&in_cloud_ptr->data[32*i],4);
std::memcpy(&p.y,&in_cloud_ptr->data[32*i+4],4);
std::memcpy(&p.z,&in_cloud_ptr->data[32*i+8],4);
std::memcpy(&p.rgba,&in_cloud_ptr->data[32*i+16],4);
pclcloud->points.push_back(p);
}
}
这样就减少了原始的数据量，加快了ROS话题转换成PCL格式的速度。
Original: https://blog.csdn.net/luyuhangGray/article/details/122634340
Author: 陆宇杭
Title: ROS点云话题sensor_msgs::PointCloud2转pcl::PointCloud的两种方式
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/519669/
转载文章受原作者版权保护。转载请注明原作者出处！