【学习记录】镭神32线激光雷达ROS下运行fromRosMsg()报错 Failed to find match for field “intensity“ 问题

今日采用ROS的fromRosMsg()将ros下的点云数据转成PCL时，报错 “Failed to find match for field ‘intensity’”。调试半天，确定问题并解决。

1. RVIZ查看发布数据是否包含Intensity

首先采用rviz看了下镭神的ros下驱动发布的数据是否包含了intensity数据。
虽然是显然应该包括的，但调试了半天一直收不到数据我都怀疑它没有发出来。
打开rviz订阅点云数据，然后将颜色显示按照intensity进行显示，如果按照intensity字段能够显示出不同的的颜色，那么就是本身包含了intensity数据的。

2. 查看发布的 RosMsg 数据

网上一般解释，这个报错往往发生在自定义的点云数据类型。但我这里转化的是将 PointCloud2 转成标准的 pcl::PointXYZI，所以一开始完全没有往数据格式上考虑。
不会真的是数据格式不正确吧？于是我输出了此时的点云：

rostopic echo /lslidar_points_cloud

部分输出如下：

header: xxx
height: 32
width: 2244
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "intensity"
    offset: 16
    datatype: 2
    count: 1
is_bigendian: False
point_step: 32
row_step: 71808
data: [0, 0, 0, 0, 0, 0, 0, ...]
is_dense: False

由于这个信息是编码后的，所以无法直接从data中读取出来是否含有intensity。但注意到intensity下的datatype是2，这个含义是啥呢？进一步输出PointCloud2的message：

rosmsg info sensor_msgs/PointCloud2

输出内容如下：

[sensor_msgs/PointCloud2]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

可以发现，2 表示的是uint8，也就是说，此时镭神雷达发出的点云中的intensity是一个uint8类型，而非常见的float类型。是否是这个原因导致无法match呢？

3. 自定义点云格式

推测默认的pcl::PointXYZI是float类型接收的intensity，因此需要重新定义一种点云类型。定义方式如下：

// header.h
struct lslidarPointXYZI{
	PCL_ADD_POINT4D;
	double timestamp;
	uint8_t intensity;		// lslidar's intensity is a `uint8_type` (type=2)
	float range;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
POINT_CLOUD_REGISTER_POINT_STRUCT(lslidarPointXYZI,
					(float, x, x)(float, y, y)(float, z, z)
					(uint8_t, intensity, intensity)(double, timestamp, timestamp))
typedef pcl::PointCloud<lslidarPointXYZI> lslidarPointCloud;

其中在struct字段中，将intensity定义为uint8_t，并且需要在下面加一行点云类型的宏注册。关于这行没有详细查找含义，但推测是把上面struct中定义的字段和实际pcl中默认的xyz/intensity等进行对应。

之后再代码测试：

pcl::PointCloud<lslidarPointXYZI> point_cloud_ls;
fromROSMsg(*msg_pc, point_cloud_ls);

这次可以成功转化，没有报错。
只不过此时的intensity是0~255，如果按照通用的情况，我们需要将其转为float类型再进行后续运算。

总结

真是一个坑爹的设定。