PCL的fromROSMsg()和toROSMsg()不能正确处理xyz之外其他field的数据长度

Arnold-FY-Chen

已于 2024-02-17 23:27:28 修改

阅读量1.5k

点赞数 11

CC 4.0 BY-SA版权

文章标签： PCL ROS Point Cloud 点云 intensity

于 2024-02-17 22:09:57 首次发布

本文链接：https://blog.csdn.net/XCCCCZ/article/details/136142235

文章讲述了如何在PCL中正确处理从ROS传感器消息转换为PCL点云类型（如PointXYZI）时的数据类型长度问题，以及自定义fromROSMsg和toROSMsg函数以适应不同数据结构的需求。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

使用PCL的PCL的fromROSMsg()函数将ROS的sensor_msgs::PointCloud2类型数据转换为PCL的pcl::PointCloud<T>类型数据时，假如T只是PointXYZ没问题，假如是PointXYZI，intensity这个field的数据类型是float，但是数据长度就是不对的，数据占8个字节而不是float的4个字节！toROSMsg()将PCL的pcl::PointCloud<T>类型数据转换为ROS的sensor_msgs::PointCloud2类型数据时则对数据类型和长度不作检查，完全照搬PCL里的PointXYZI的数据类型和长度，intensity占8个字节这样的有问题数据，rviz可以正常解释和播放展示，自己开发的软件在读取pcl库调用toROSMsg()转换生成的bag时通常根据intensity是float类型而认为数据长度是4个字节而读取到不正常的intensity数据而不能正常播放展示，这时需要自己实现类似fromROSMsg()和toROSMsg()这样的功能函数。

翻看了一下PCL相关源码，里面fromROSMsg()和toROSMsg()函数相关的代码如下：

/opt/ros/melodic/include/pcl_conversions/pcl_conversions.h

namespace pcl {
...
template<typename T>
  void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
  {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
    pcl_conversions::moveFromPCL(pcl_pc2, cloud);
  }

template<typename T>
  void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
  {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(cloud, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
  }
  ...
}




namespace pcl_conversions {
  ...
  inline
  void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf)
  {
    pf.name = pcl_pf.name;
    pf.offset = pcl_pf.offset;
    pf.datatype = pcl_pf.datatype;
    pf.count = pcl_pf.count;
  }

  inline
  void fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::PointField> &pfs)
  {
    pfs.resize(pcl_pfs.size());
    std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
    int i = 0;
    for(; it != pcl_pfs.end(); ++it, ++i) {
      fromPCL(*(it), pfs[i]);
    }
  }

  inline
  void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
  {
    fromPCL(pcl_pc2.header, pc2.header);
    pc2.height = pcl_pc2.height;
    pc2.width = pcl_pc2.width;
    fromPCL(pcl_pc2.fields, pc2.fields);
    //printf("pc2.fields.size() %d, ", pc2.fields.size());
    //printf("pcl_pc2.fields/pc2.fields: offset %d/%d, datatype %d/%d\n", pcl_pc2.fields[3].offset, pc2.fields[3].offset, pcl_pc2.field