RViz2可视化传感器数据系列教程之二：在RViz2中显示激光雷达点云数据

【编者注】本文由深圳季连科技有限公司kenny wang创作，知识产权归深圳季连科技有限公司所有。如需转载，敬请注明原文出处。

在RViz2中，可以对激光雷达或者深度相机的三维点云数据进行可视化显示，但前提是必须以话题这种ROS的通信方式对点云数据进行了发布。因此，首先需要通过ROS 2节点发布激光雷达点云数据。

目前，在ROS 2中还没有内建的三维点云数据发布者专用软件包或者节点。在Github网站上有一些点云数据发布软件包，但大多数是ROS 1的，仅有很少几个ROS 2的点云数据发布软件包，包括ros2_kitti_publishers、kitti_ros_publisher、my_ros_kitti，均为针对点云公开数据集kitti的ROS 2软件包。

在ROS 2中，目前点云数据话题的消息接口类型为sensor_msgs/PointCloud2。为了将激光雷达、深度相机等的点云数据发布为ROS 2话题，必须将点云数据转换为该接口类型的消息。因此，必须了解该消息接口的定义。其定义如下：

从该消息接口定义中可以看出，点云消息PointCloud2包括：

（1）消息头定义，包括时间戳stamp和坐标系frame_id；

（2）二维结构定义，即点云的高度height（每列点云的个数，若为无序点云，其高为1）和宽度width（每行点云的个数）；

（3）字段或通道定义，即每个点的字段或通道（如x、y、z、r），其类型为PointField；

（4）数据及其布局定义，包括是否为大序端is_bigendian、是否有无效数据点is_dense、每个点占用的比特数point_step、每行占用的比特数row_step，以及真正的序列化后数据data。

其中字段或通道的消息类型为PointField，该消息接口定义如下：

# This message holds the description of one point entry in the

# PointCloud2 message format.

uint8 INT8 = 1

uint8 UINT8 = 2

uint8 INT16 = 3

uint8 UINT16 = 4

uint8 INT32 = 5

uint8 UINT32 = 6

uint8 FLOAT32 = 7

uint8 FLOAT64 = 8

# Common PointField names are x, y, z, intensity, rgb, rgba

string name # Name of field

uint32 offset # Offset from start of point struct

uint8 datatype # Datatype enumeration, see above

uint32 count # How many elements in the field

PointField消息接口定义了点云的字段或通道名称name、对点结构起始位置的偏移量offset、数据类型（6种整型和2种浮点型之一）和元素个数。

下面以具体的点云数据为例来进行进一步的详细说明。如果没有激光雷达和深度相机，则只能从开放的点云数据集下载获得示例点云数据。这里我们从目前国际上最大、最流行的自动驾驶数据集Kitti中下载一个原始数据格式（raw data）点云示例数据，其具体网址为http://www.cvlibs.net/datasets/kitti/raw_data.php。这里我们下载的是city场景之下的2011_09_26_drive_0093数据集（包括4个摄像头的图像以及点云和里程计数据），我们下载了纠正后的同步数据（synced+rectified data）、标定配准（calibration）数据和目标三维标注tracklets数据（见下图）。

解压缩后该数据集的文档目录结构如下图所示。其中imgage_00、imgage_01、imgage_02、imgage_03目录中存储的分别是左侧黑白相机、右侧黑白相机、左侧彩色相机和右侧彩色相机采集的图像数据及其时间戳信息（timestamps.txt）；oxts目录中存储的是里程计采集的位姿相关数据及其时间戳和数据格式说明信息；velodyne_points目录中存储的是velodyne激光雷达采集的点云数据；在根目录下还有3个标定配准txt文件，存储的是相机之间、IMU与激光雷达、激光雷达与相机之间标定配准所需的相关参数数据；而tracklets_labels.xml文件中存储的则是跟踪目标的三维标注（labels）数据。

激光雷达点云数据存储在velodyne_points目录下。打开该目录的data子目录，可以看到里面存储了所有的点云数据文件（见下图），其中每个点云数据文件的名称均为10位数字，后缀为.bin，文件名从0000000000.bin开始，由于是以每秒10帧的频率采集点云数据，因此此子目录中的点云数据文件个数约为该数据集采集时间长度（单位为秒）的10倍，这里下载的示例数据集采集时间长度为43秒，而该子目录中共有433个点云数据文件，最后一个文件的名称为0000000432.bin。

这些点云数据文件均为二进制文件，如果直接用Sublime Text之类软件打开这种点云数据文件，只能看到一堆十六进制的数字，并不能从中看出任何内容，如下图所示：

但根据kitti数据集自述文档可以得知这种二进制点云数据文件的组织方式和数据内容。Kitti数据集原始数据raw data中的点云数据是以二进制文件格式存储的，二进制文件中的每行包含8个数据，每个数据由四位十六进制数表示（浮点数），每个数据通过空格隔开。即一个点云数据由四个浮点数（32位浮点型）数据构成，分别表示点云的三维空间坐标分量x、y、z和强度或者反射值r。具体地，kitti数据集点云的存储方式如下图所示：

在Sublime Text软件中打开二进制点云数据文件并拖动至文件末尾，就可以知道该点云数据文件中存储的点云个数。从下图所示的示例二进制点云数据文件可以知道，该文件存储了约12.4万个点云数据。

为了在ROS 2话题上发布激光点云数据，需先将二进制文件格式的点云数据转换成PointCloud2消息。在ROS 2的common_interfaces中已经内建有sensor_msgs_py软件包及其point_cloud2模块，通过调用该模块中的create_cloud()方法可以基于这种点云数据二进制文件创建PointCloud2接口类型的消息，然后再对该消息进行发布即可。

为此，需要使用Python或者C++等编程语言编写自己的点云数据发布专用ROS 2软件包和节点，或者从Github网站上下载现有的点云数据发布专用ROS 2软件包并进行必要的修改和编译。下面是这样一个仅有60行代码的最简单示例点云数据发布专用ROS 2软件包Python源代码：

import rclpy
from rclpy.node import Node

import numpy as np
import glob 
import os

from std_msgs.msg import Header
from sensor_msgs.msg import PointCloud2 as PCL2
from sensor_msgs.msg import PointField
from sensor_msgs_py import point_cloud2

class PointCloudToPCL2(Node):
    def __init__(self):
        super().__init__('point_cloud_publisher')
        self._publisher = self.create_publisher(PCL2, 'pointcloud2', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.publish_pcl2)
        self.velodyne_file_paths = \
            '/home/wangjg/datasets/kitti_raw_data/2011_09_26_drive_0093_sync/velodyne_points/data/'
        self.files_number = len(os.listdir(self.velodyne_file_paths))
        self.frame = 0

    def publish_pcl2(self):
        """Callback to publish"""
        file_name = os.path.join(self.velodyne_file_paths, '%010d.bin' % self.frame)
        cloud = np.fromfile(file_name, np.float32)
        cloud = cloud.reshape((-1, 4))
        header = Header()
        header.frame_id = 'velodyne_lidar'
        header.stamp = self.get_clock().now().to_msg()
        fields = [
            PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
            PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
            PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
            PointField(name='r', offset=12, datatype=PointField.FLOAT32, count=1)
        ]
        pcl2_msg = point_cloud2.create_cloud(header, fields, cloud)
        self._publisher.publish(pcl2_msg)
        self.get_logger().info("point cloud data published %d " % self.frame)
        self.frame += 1

        if self.frame == self.files_number:
            self.get_logger().info("no more velodyne points to publish. Loop again !")
            self.frame = 0

def main(args=None):
    rclpy.init(args=args)
    point_cloud_to_pcl2 = PointCloudToPCL2()
    try:
        rclpy.spin(point_cloud_to_pcl2)
    except KeyboardInterrupt:
        point_cloud_to_pcl2.get_logger().debug("Keyboard interrupt")
    # destroy node explicity
    point_cloud_to_pcl2.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在您的ROS 2工作空间的src子目录下可以创建一个ROS 2软件包，运行如下命令可以完成这样一个任务：

ros2 pkg create --build-type ament_python point_cloud2_publisher

然后将上述示例python源代码保存到该软件包存放源代码的子目录下，并对package.xml和setup.py文件进行必要的修改，然后再编译该软件包，这样就可以运行该ROS 2软件包的节点（可执行文件）来发布示例点云数据了。有关如何创建、编辑和编译ROS 2软件包的具体方法，请参见本专栏文章“ROS 2入门教程——2.2 创建您的第一个ROS 2软件包”和“ROS 2入门教程——2.4 编写一个简单的发布者和订阅者(Python)”。

在运行这个软件包的发布者节点发布激光雷达点云数据话题后，就可以运行rviz2并在对该点云数据话题的固定坐标系进行正确地配置后，就会在rviz2中可视化显示该点云数据了。当然，为了方便，也可以参考本专栏本系列教程中的上一篇“RViz2可视化传感器数据系列教程之一：在RViz2中显示相机视频图像数据”，创建一个launch文件以同时运行启动这个软件包的点云数据发布者节点和rviz2，并用一个.rviz配置文件直接配置好rviz2，这样启动该launch文件就可以直接发布点云数据，同时在rviz2中对该点云数据进行可视化显示，如下图所示。

同时对激光点云数据在RViz2中的播放过程进行了录屏，视频已发布在本专栏中（见“kitti点云数据在rviz2中的可视化”视频）。