ROS 2中发布和可视化传感器数据实战系列之三：大疆览沃激光雷达点云数据发布与RViz2中显示彩色点云

【作者注】：本文将在介绍大疆激光雷达点云数据存储格式LVX数据组织结构的基础上，阐述如何将.lvx文件中的激光点云数据转换为ROS 2的sensor_msgs/PointCloud2消息，然后在ROS 2的话题上进行点云数据发布（文章附有完整的python源代码），并在RViz2中可视化。最后对如何将点云在RViz2中进行彩色显示划了重点，kenny wang在这里王婆卖瓜一下，个人认为这是目前讲解如何在RViz2显示彩色点云最清楚、最明白、最简单、最直接的一篇文章了。

走过路过，千万不要错过！！！撰文不易，广大知友若觉得本文有所帮助，还请多多点赞、收藏；如有不同意见或者看法，或者想要交流，请多多评论，或者联系作者，联系方式kennywangaigraphx@gmail.com，QQ-894625859。

另外作者在此郑重申明：本文为深圳季连科技有限公司（Aigraphx）kenny wang原创，知识产权归深圳季连所有，转载请一定要载明出处，否则深圳季连保留追究随意转载者法律责任的权利。

激光雷达传感器是自动驾驶、机器人和工业自动化等应用中重要且常用的传感器之一。除了像Velodyne（威力登）这样的全球激光雷达领航或头部企业之外，国内也已经有多家公司布局并量产激光雷达产品，包括速腾聚创、禾赛科技、镭神智能、华为、大疆览沃livox等。由于我公司（深圳季连科技有限公司）客户已经购置了大疆旗下览沃科技的傲览（Avia)、浩界（Horizon）和觅道-70（Mid-70）等多款激光雷达产品，拟安装应用于工业场景，主要用于进行3D目标检测和跟踪。由于疫情的影响，目前尚未安装这些激光雷达，因此需要先用览沃官网提供的激光雷达数据集提前开展相关算法研究。为此，需将览沃提供的*.lvx激光雷达数据文件中的点云数据发布为ROS 2话题并在RViz2中进行可视化。

沃览科技虽然在Github网站上发布了其面向ROS 1的激光雷达驱动程序livox_ros_driver和面向ROS 2的激光雷达驱动程序livox_ros2_driver，但由于kenny wang我手中没有其激光雷达固件，还无法使用其驱动程序软件包。为此，需要根据lvx文件格式自己动手编写其点云数据发布软件包。

首先来研究一下lvx文件格式。根据览沃官网，目前lvx文件格式的最新版本为v1.1（2020年5月发布），并且可以下载lvx格式说明书（LVX Specifications v1.1）。根据该说明书，LVX格式文件中包含的是二进制数据，由Public Header Block、Private Header Block、Device Info Block和Point Data Block这四个块组成。LVX文件中所有数据使用的都是小端序（little-endian），使用的数据类型包括字符型char和unsigned char（1字节）、短整型short和unsigned short（2字节）、整型int和unsigned int（4字节）、长整型long long和unsigned long long（8字节）、浮点型float（4字节IEEE浮点格式）、双精度浮点型double（8字节IEEE浮点格式）。

公用头块（Public Header Block）包含文件签名、版本和魔术码（magic code）等头信息（见下表），如果确认了*.lvx文件为v1.1版本的，则对于点云数据发布和可视化来说应该无关紧要。

私有头块（Private Header Block）包括帧时长（Frame Duration）和激光雷达设备台数（Device Count），如下表所示：

其中Frame Duration在v1.1版本中被设定为50ms且不能更改，因此如果点云数据发布频率设置为0.1s，则每调用一次回调函数时需要读取2帧点云数据；Device Count用于存储采集该点云数据时所使用的设备台数，此值越大，每帧所能覆盖的视野就越大，点云数据就越多或者越密集，点云文件也就越大，而且也决定了设备信息块（Device Info Block）的大小。

设备信息块（Device Info Block）用于存储采集该点云数据文件时所使用的所有设备信息，其大小可变，视Device Count不同而不同。Device Info Block的存储结构如下表所示：

字段Device Info用于存储每台设备的相关信息，字段大小为59字节，该字段的定义见下表：

由上表可知，Device Info字段会存储LiDAR广播码、Hub广播码、设备索引、设备类型、启用/禁用外参以及设备外参（3个欧拉角和XYZ的平移分量）。要注意的是，用户可以使用Device Index字段来从LVX文件中抽取每台设备的点云数据。

上面三个块都是元数据，真正的点云数据都存储在点云数据块（Point Cloud Data Block）中，因此这个块最为重要。点云数据块是由若干帧（frame）构成的（见下表），每帧是由若干个数据包（package)构成的，而每个数据包中又包含固定数量的点（point）。

每个帧中除了包含若干个package外，还包含帧的头信息（Frame Header）。帧的定义如下表所示：

Frame Header中包括3个字段，即当前帧偏移量（Current Offset）、下一帧偏移量（Next Offset）和帧索引（Frame Index），如下表所示：

数据包（package）被定义为SDK协议新类型的普通数据，包中除了固定数量的点（point）数据之外，还含有设备索引、版本、Slot ID、LiDAR ID、状态码、时间戳及其类型、数据类型等元数据，其结构如下表所示：

每个数据包中点的个数取决于数据类型（Data Type）或点云坐标格式字段值。除了当该字段值等于6时为IMU（惯性测量单元）数据外，其余数据类型（该字段值等于0、1、2、3、4、5）时均为点云数据。当该字段值等于0、2、4时为笛卡尔（Cartesian）坐标系的点云数据，而该字段值等于1、3、5时为球面（Spherical）坐标系的点云数据。具体来说：

（1）当数据类型字段值为0时，每个数据包中含有100个点的数据，每个点由x、y、z和reflectivity四个字段描述，每个点的大小为13个字节，点的具体定义如下表所示：

（2）当数据类型字段值为1时，每个数据包中也含有100个点的数据，但每个点由球面坐标系表示，即由depth、theta、phi和reflectivity四个字段描述，每个点的大小为9个字节，点的具体定义如下表所示：

（3）当数据类型字段值为2时，每个数据包中含有96个点的数据，每个点由x、y、z和reflectivity、tag五个字段描述，每个点的大小为14个字节，点的具体定义如下表所示：

（4）当数据类型字段值为3时，每个数据包中也含有96个点的数据，但每个点由球面坐标系表示，即由depth、theta、phi和reflectivity、tag五个字段描述，每个点的大小为10个字节，点的具体定义如下表所示：

（5）当数据类型字段值为4时，每个数据包中只含有48个点的数据，但由于是double return，每个点中实际含有2个点的数据，即由x1、y1、z1、reflectivity1、tag1和x2、y2、z2、reflectivity2、tag2十个字段描述，每个点的大小为28个字节，点的具体定义如下表所示：

（6）当数据类型字段值为5时，点也是用球面坐标系表示，每个数据包中也只含有48个点的数据，即由depth1、theta1、phi1、reflectivity1、tag1和depth2、theta2、phi2、reflectivity2、tag2十个字段描述，每个点的大小为20个字节，点的具体定义如下表所示：

（7）当数据类型字段值为6时，点中存储的实际上是IMU数据，每个包仅含有1个点数据，并由三个轴的角速度和加速度这6个字段构成，点的大小为24个字节，其具体定义如下表所示：

在掌握了LVX文件的组织结构后，就可以编写程序对LVX文件进行点云数据读取、转换成ROS 2的sensor_msgs/PointCloud2消息并对其进行发布，然后在RViz2中对其点云数据进行可视化。

从上面LVX文件组织结构的描述中可以看出，该格式文件的结构还是比较复杂的，其点云数据读取的程序也就相对比较复杂。庆幸的是，在Github网站上已经有几个软件包有模块专门用于读取LVX文件，如用Python编写的有Livox_lvx_parser、pylvx和OpenPyLivox等，可以直接利用或者稍加修改即可利用。在本实战文章中kenny wang使用的是pylvx并对其进行了稍加修改。由于在Github网站上已有该程序的python源代码，各位感兴趣的知友可以去上面的链接中查看其源代码，在此kenny wang考虑到产权问题不再附上。

上述程序可以将LVX文件中的点云数据读取到数组变量中，我们要做的工作就是新建一个ROS 2软件包，编写点云数据发布者节点代码（要调用上述开源程序的函数读取LVX文件并获得其返回的点云数据，并将其转换成PointCloud2消息），必要时创建launch文件和.rviz配置文件，并对软件包进行编译，这样就可以用ROS 2将LVX文件中大疆激光雷达点云数据发布到某个话题（topic）上，并在RViz2中进行可视化显示。有关如何创建、编辑和编译ROS 2软件包的具体方法，请参见本公司知乎专栏文章“ROS 2入门教程——2.2 创建您的第一个ROS 2软件包”和“ROS 2入门教程——2.4 编写一个简单的发布者和订阅者(Python)”。

下面是kenny wang编写的一个专用于大疆激光雷达LVX文件格式点云数据的ROS 2简单发布者节点的Python源代码：

import numpy as np

import rclpy
from rclpy.node import Node
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    # for creating point cloud
from .pylvx import *     # a single python script file for parse livox file in same directory

class PointCloudToPCL2(Node):
    def __init__(self):
        super().__init__('livox_pointcloud_publisher')
        lvx_file_path = \
            '/home/wangjg/datasets/livox_pointcloud_data/Horizon_poincloud_data.lvx'
        self.frames = []
        frames = []
        self.frame_index = 0
        index = 0
        timer_period = 0.1
        lf = LvxFile(lvx_file_path)
        duration = lf.private_header_block.frame_duration
        for frame in lf.point_data_block:
            frames.append(frame)
            if (index + 1) % int(timer_period * 1000 / duration) == 0:
                self.frames.append(frames)
                frames = []
            index += 1
        self.frames_number = len(self.frames)
        print('Number of Frames: %d'%self.frames_number)
 
        self._publisher = self.create_publisher(PCL2, 'pointcloud2', 10)
        self.timer = self.create_timer(timer_period, self.publish_pcl2)

    def publish_pcl2(self):
        """Callback to publish"""
        frames = self.frames[self.frame_index]
        points = read_points_from_lvxfile(frames)    # call the function defined after
        header = Header()
        header.frame_id = 'livox'
        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='intensity', offset=12, datatype=PointField.FLOAT32, count=1)    # must be 'intensity', noted by kenny wang
        ]    # the fourth field must be 'intensity' for color visualization in rviz2
        pcl2_msg = point_cloud2.create_cloud(header, fields, points)
        self._publisher.publish(pcl2_msg)    # publish the pointcloud messages
        self.get_logger().info("point cloud data published %d " % self.frame_index)
        self.frame_index += 1
        if self.frame_index == self.frames_number:    # for loop publishing purpose
            self.get_logger().info("no more velodyne points to publish. Loop again !")
            self.frame_index = 0

def read_points_from_lvxfile(frames):    # function definition to read points from *.lvx file
    points = []
    data_type = None
    for frame in frames:
        for package in frame.packages:
            package: Package
            if data_type is None and package.data_type != DataType.IMU_INFO:
                data_type = package.data_type
            for point in package.points:
                if package.data_type == data_type:
                    if data_type == DataType.CARTESIAN_MID or \
                        data_type == DataType.CARTESIAN_SINGLE:    # for single return
                        fields = 'x y z reflectivity'.split(' ')
                        values = [getattr(point, field) for field in fields]
                        pt = livox_pointcloud_mm2m(values)
                        points.append(np.array(pt))
                    elif data_type == DataType.CARTESIAN_DOUBLE:    # for double return
                        fields = 'x y z reflectivity'.split(' ')
                        values = [getattr(point, field + '1') for field in fields]
                        pt = livox_pointcloud_mm2m(values)
                        points.append(np.array(pt))
                        fields = 'x y z reflectivity'.split(' ')
                        values = [getattr(point, field + '2') for field in fields]
                        pt = livox_pointcloud_mm2m(values)
                        points.append(np.array(pt))
                    else:
                       return
    if data_type not in [DataType.CARTESIAN_MID, DataType.CARTESIAN_SINGLE, \
        DataType.CARTESIAN_DOUBLE]:
        print(data_type)
        return
    return points

def livox_pointcloud_mm2m(values):    # function to transform coordinate unit from mm to m
    pt = [0,0,0,0]
    pt[0] = values[0]/1000.
    pt[1] = values[1]/1000.
    pt[2] = values[2]/1000.
    pt[3] = values[3]/1.
    return pt

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().info("User Keyboard interrupt, exit !")
    # destroy node explicity
    point_cloud_to_pcl2.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

编译软件包后，运行这个软件包的发布者节点发布激光雷达点云数据话题后，就可以运行rviz2并在对该点云数据话题的固定坐标系进行正确地配置后，就会在rviz2中可视化显示该点云数据了。大家记得要把代码中.lvx文件路径替换成你们自己的路径哦，而不可能与kenny wang我的.lvx文件路径完全相同。

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

注意：最后我kenny wang要划重点了。最初在网上看到相关源代码中将points的第四个PointField的name设置为r，代表反射的意思，当时也没多想，就拿来用了，但发现点云在RViz2中显示为黑白颜色。那么如何让激光点云在RViz2显示为彩色而不是黑白（灰度）的？这个问题折腾了我两三天。在网上查看了很多相关文章和很多源代码，都没有交代清楚，或者人家清楚但没有说。我也尝试了各种方法，如将LVX文件中的所有帧一帧一帧的输出为*.pcd文件，再用python-pcl库的导入方法load()、load_XYZI()，用open3d库的io.read_point_cloud()方法再读取点云数据，或者将点云的强度或反射值映射成颜色，等等，好一通折腾，但都不起作用。

最后kenny wang我准备放弃（kenny wang建议大家在经过长时间努力而未成功、准备放弃某件事时最好要回头看看，因为你已经试错了很多，那么成功可能就离你不远了，要从不断试错中找到成功之路）的时候，偶然（很多伟大的发明都是“偶然”产生的，但偶然之中蕴含着必然，因为发明者已经试错无数次了）在RViz2中的“显示”面板中看到PointCloud2话题下的Color Transformer字段值默认是Intensity，而Channel Name只能从x、y、z、r中选一个，kenny就想会不会是这两个字段值要相同才行。然后将sensor_msgs/PointCloud2消息中如下所示字段定义代码

        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)
        ]

最后一个字段定义的name由'r'修改成'intensity'，即：

            PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1)

彩色点云出现了！然后我又试了一下将其它点云数据发布软件包也进行相同的修改，发现也起作用，终于明白真的就是如kenny wang我所想的那样。所以这里要划的重点就是：

sensor_msgs/PointCloud2消息中点云强度或者反射字段（xyz三个字段之后的那个字段）的名称name必须为'intensity'才能在RViz2中显示出彩色点云，否则其名称如为'r'、'rgb'、'rgba'、'i'等都只能显示出黑白点云。

参考资料：

LIVOX. LVX Specifications v1.1, 2020.05

Livox Viewer文件格式解析工具(github.com/Jaesirky/pyl)