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

      使用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.fields[3].datatype, pc2.fields[3].datatype);
    pc2.is_bigendian = pcl_pc2.is_bigendian;
    pc2.point_step = pcl_pc2.point_step;
    pc2.row_step = pcl_pc2.row_step;
    pc2.is_dense = pcl_pc2.is_dense;
  }


  inline
  void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
  {
    copyPCLPointCloud2MetaData(pcl_pc2, pc2);
    pc2.data.swap(pcl_pc2.data);
  }

  inline
  void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
  {
    copyPointCloud2MetaData(pc2, pcl_pc2);
    pcl_pc2.data = pc2.data;
  }

  inline
  void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
  {
    toPCL(pc2.header, pcl_pc2.header);
    pcl_pc2.height = pc2.height;
    pcl_pc2.width = pc2.width;
    toPCL(pc2.fields, pcl_pc2.fields);
    pcl_pc2.is_bigendian = pc2.is_bigendian;
    pcl_pc2.point_step = pc2.point_step;
    pcl_pc2.row_step = pc2.row_step;
    pcl_pc2.is_dense = pc2.is_dense;
  }
  
  inline
  void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf)
  {
    pcl_pf.name = pf.name;
    pcl_pf.offset = pf.offset;
    pcl_pf.datatype = pf.datatype;
    pcl_pf.count = pf.count;
  }

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

   inline
  void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header)
  {
    toPCL(header.stamp, pcl_header.stamp);
    pcl_header.seq = header.seq;
    pcl_header.frame_id = header.frame_id;
  }
  inline
  void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)
  {
    pcl_stamp = stamp.toNSec() / 1000ull;  // Convert from ns to us
  }
  ...
}

/usr/include/pcl-1.8/pcl/conversions.h

template<typename PointT> void
  toPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg)
  {
    // Ease the user's burden on specifying width/height for unorganized datasets
    if (cloud.width == 0 && cloud.height == 0)
    {
      msg.width  = static_cast<uint32_t>(cloud.points.size ());
      msg.height = 1;
    }
    else
    {
      assert (cloud.points.size () == cloud.width * cloud.height);
      msg.height = cloud.height;
      msg.width  = cloud.width;
    }

    // Fill point cloud binary data (padding and all)
    size_t data_size = sizeof (PointT) * cloud.points.size ();
    msg.data.resize (data_size);
    if (data_size)
    {
      memcpy(&msg.data[0], &cloud.points[0], data_size);
    }

    // Fill fields metadata
    msg.fields.clear ();
    for_each_type<typename traits::fieldList<PointT>::type> (detail::FieldAdder<PointT>(msg.fields));
    for(auto f: msg.fields) {
      printf("f.name %s, offset %d, datatype %d\n", f.name.c_str(), f.offset, f.datatype);
    }
    msg.header     = cloud.header;
    msg.point_step = sizeof (PointT);
    msg.row_step   = static_cast<uint32_t> (sizeof (PointT) * msg.width);
    msg.is_dense   = cloud.is_dense;
    /// @todo msg.is_bigendian = ?;
  }

 struct FieldAdder
    {
      FieldAdder (std::vector<pcl::PCLPointField>& fields) : fields_ (fields) {};

      template<typename U> void operator() ()
      {
        pcl::PCLPointField f;
        f.name = traits::name<PointT, U>::value;
        f.offset = traits::offset<PointT, U>::value;
        f.datatype = traits::datatype<PointT, U>::value;
        f.count = traits::datatype<PointT, U>::size;
        fields_.push_back (f);
      }

      std::vector<pcl::PCLPointField>& fields_;
    };

/usr/include/pcl-1.8/pcl/point_traits.h
// offset
    template<class PointT, typename Tag>
    struct offset : offset<typename POD<PointT>::type, Tag>
    {
      // Contents of specialization:
      // static const size_t value;

      // Avoid infinite compile-time recursion
      BOOST_MPL_ASSERT_MSG((!boost::is_same<PointT, typename POD<PointT>::type>::value),
                           POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));
    };

/usr/include/pcl-1.8/pcl/register_point_struct.h

#define POINT_CLOUD_REGISTER_FIELD_OFFSET(r, name, elem)                \
  template<> struct offset<name, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)> \
  {                                                                     \
    static const size_t value = offsetof(name, BOOST_PP_TUPLE_ELEM(3, 1, elem)); \
  };

#define POINT_CLOUD_REGISTER_POINT_STRUCT(name, fseq)               \
  POINT_CLOUD_REGISTER_POINT_STRUCT_I(name,                         \
    BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0))      \

/usr/include/pcl-1.8/pcl/point_types.h
POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZI,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
)
POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZI, pcl::_PointXYZI)




/usr/include/pcl-1.8/pcl/PCLPointField.h
namespace pcl
{
  struct PCLPointField
  {
    PCLPointField () : name (), offset (0), datatype (0), count (0)
    {}

    std::string name;

    pcl::uint32_t offset;
    pcl::uint8_t datatype;
    pcl::uint32_t count;

    enum PointFieldTypes { INT8 = 1,
                           UINT8 = 2,
                           INT16 = 3,
                           UINT16 = 4,
                           INT32 = 5,
                           UINT32 = 6,
                           FLOAT32 = 7,
                           FLOAT64 = 8 };

/usr/include/pcl-1.8/pcl/register_point_struct.h

自己实现类似fromROSMsg()和toROSMsg()函数功能的代码如下：

void ROSMsg2PCL(sensor_msgs::PointCloud2& ros_msgs, cloud::Ptr cloud_ptr) {
  cloud_ptr->header.seq = ros_msgs.header.seq;
  cloud_ptr->header.frame_id = ros_msgs.header.frame_id;
  cloud_ptr->header.stamp.fromNSec(ros_msgs->header.stamp / 1000ull); // ns => us
  cloud_ptr->height = ros_msgs.height;
  cloud_ptr->width = ros_msgs.width;
  cloud_ptr->is_dense = ros_msgs.is_dense;
  
  int points_num = ros_msgs.height * ros_msgs.width;
  cloud_ptr->points.resize(points_num * 4 * 4);  // XYZI
  float * pdata = reinterpret_cast<float*>(&ros_msgs.data[0]);
  for (auto i = 0; i < points_num; ++i) {
      cloud_ptr->points[i*4 + 0] = pdata[i*4 + 0];
      cloud_ptr->points[i*4 + 1] = pdata[i*4 + 1];
      cloud_ptr->points[i*4 + 2] = pdata[i*4 + 2];
      if (ros_msgs.point_step == 16) // 4+4+4+4
        cloud_ptr->points[i*4 + 3] = pdata[i*4 + 3];
      /*有的雷达输出的rosmsg在fields的排放顺序有特殊安排，需要使用POINT_CLOUD_REGISTER_POINT_STRUCT注册特定的struct来读取XYZI，例如:
         POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
           (float, x, x)
           (float, y, y)
           (float, z, z)
           (float, intensity, intensity))
      即便是保证了XYZI的排放顺序,intensity可能占了8个字节，前4个字节为0后4个字节才是intensity值，不同雷达处理可能不一样，所以这里的处理只是示例，不同雷达是情况而定
      */
      else if (ros_msgs.point_step == 20) //4+4+4+8
        cloud_ptr->points[i*4 + 3] = pdata[i*4 + 4];
  }
}

void initRosMsgFields(std::vector<sensor_msgs::PointField>& fields) {
  fields.resize(4);
  fields[0].name = "x";
  fields[0].offset = 0;
  fields[0].datatype = 7;
  fields[0].count = 1;
  fields[1].name = "y";
  fields[1].offset = 4;
  fields[1].datatype = 7;
  fields[1].count = 1;
  fields[2].name = "z";
  fields[2].offset = 8;
  fields[2].datatype = 7;
  fields[2].count = 1;
  fields[3].name = "intensity";
  fields[3].offset = 12;
  fields[3].datatype = 7;
  fields[3].count = 1;
}

void PCL2ROSMSg(cloud::Ptr cloud_ptr, sensor_msgs::PointCloud2& ros_msgs) {
    //set header with PCL Cloud header for common use. 
    ros_msgs.header.stamp.fromNSec(cloud_ptr->header.stamp * 1000ull); // us => ns
    ros_msgs.header.seq = cloud_ptr->header.seq;
    ros_msgs.header.frame_id = cloud_ptr->header.frame_id;

    initRosMsgFields(ros_msgs.fields);

    ros_msgs.width = cloud_ptr->width;   // 1
    ros_msgs.height = cloud_ptr->height; // point_num
    ros_msgs.point_step = ros_msgs.fields.size() * sizeof(float); // 4 * 4
    ros_msgs.row_step   = static_cast<uint32_t> (ros_msgs.point_step * ros_msgs.width);
    ros_msgs.is_dense   = cloud_ptr->is_dense;

    int data_size = cloud_ptr->size() * ros_msgs.point_step;
    if (data_size > 0) {
       ros_msgs.data.resize(data_size);
       //memcpy(&ros_msgs.data[0], &cloud_ptr->points[0], data_size);
       float * pdata = reinterpret_cast<float*>(&ros_msgs.data[0]);
       for (int i = 0; i < cloud_ptr->size(); i++ ) {
          pcl::PointXYZI pt = cloud_ptr->points[i];
	  pdata[i*4 + 0] = pt.x;
	  pdata[i*4 + 1] = pt.y;
	  pdata[i*4 + 2] = pt.z;
	  pdata[i*4+ 3] = pt.intensity;
       }
    }
}