解析sensor_msgs::PointCloud2 ROS点云数据

一、消息结构

sensor_msgs::PointCloud2是一类点云数据结构，消息定义如下

$ rosmsg info 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

一个仿真的点云消息长这样

$ rostopic echo /airsim_node/Drone0/lidar/LidarSensor1
header: 
  seq: 2116
  stamp: 
    secs: 1586919439
    nsecs: 448866652
  frame_id: "LidarSensor1"
height: 1
width: 3
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
is_bigendian: False
point_step: 12
row_step: 36
data: [143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64, 143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64, 143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64]
is_dense: True

PointCloud2的data是序列化后的数据，直接看不到物理意义。写了两种解析sensor_msgs::PointCloud2 数据的程序。

二、Python

#!/usr/bin/env python2
# coding:utf-8

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2

def callback_pointcloud(data):
    assert isinstance(data, PointCloud2)
    gen = point_cloud2.read_points(data)
    print type(gen)
    for p in gen:
        print p 

def listener():
    rospy.init_node('pylistener', anonymous=True)
    #Subscriber函数第一个参数是topic的名称，第二个参数是接受的数据类型 第三个参数是回调函数的名称
    rospy.Subscriber('/airsim_node/Drone0/lidar/LidarSensor1', PointCloud2, callback_pointcloud)
    rospy.spin()

if __name__ == '__main__':
    listener()

解析上面数据输出如下

<type 'generator'>
(1.5258788153005298e-06, 1.220703097715159e-06, 5.185408592224121)
(9.155273232863692e-07, 1.220703097715159e-06, 5.185408592224121)
(9.155273232863692e-07, 1.220703097715159e-06, 5.185408592224121)

三、C++

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

double pointCloud2ToZ(const sensor_msgs::PointCloud2 &msg)
{
	sensor_msgs::PointCloud out_pointcloud;
	sensor_msgs::convertPointCloud2ToPointCloud(msg, out_pointcloud);
	for (int i=0; i<out_pointcloud.points.size(); i++) {
		cout << out_pointcloud.points[i].x << ", " << out_pointcloud.points[i].y << ", " << out_pointcloud.points[i].z << endl;
	}
	cout << "------" << endl;
	return out_pointcloud.points[0].z;
}