点云格式转换：将 ros PointCloud2格式数据转为livox CustomMsg格式

前言

览沃科技有限公司（Livox）成立于2016年。为了革新激光雷达行业，Livox致力于提供高性能、低成本的激光雷达传感器。通过降低使用门槛和生产成本，Livox将激光雷达技术集成到更多产品和应用之中，从而为自动驾驶、智慧城市、测绘、移动机器人等行业带来创新性改变。Livox产品已销往包括美国、加拿大、中国、日本和欧盟在内的 26 个国家和地区。

面向智能移动机器人市场，Livox 推出最新一代 3D 激光雷达 Mid-360，开启混合固态激光雷达 360° 立体感知新篇章。凭借小巧体积，Mid-360 的安装布置更加灵活。同时，Mid-360 充分考虑了移动机器人对导航、避障等升维感知的需求，兼容室内外场景，赋能移动机器人进入空间智能感知新时代。 但是这款产品官网一直购买不到， 本篇博客 在 gazebo 中 可实现对该激光雷达的仿真 。

传统激光雷达普遍采用机械扫描方式，扫描路径随时间重复。而Livox 激光雷达采用了独特的扫描?式，扫描路径不会重复。在非重复扫描方式中，视场中被激光照射到的区域面积会随时间增大，这意味着视场覆盖率随时间推移而显著提高，可减小视场内物体被漏检的概率，有助于探测视场中的更多细节。

下图中给出了一个直观的例子。（a）图由于采用了非重复扫描方式，随着时间的积累，视场覆盖率逐渐升高。而（b）图，由于扫描每次都是重复的，视场覆盖率几乎没有提升。

gazebo中自带的laser插件是上图中b的方式，并不能代表livox的这种雷达。

livox 的仿真功能包(livox_laser_simulation)中 提供了 多种半固体的雷达仿真模型，可以在gazebo中进行该雷达的仿真与其扫描方式（非重复式扫描）点云发布。

但是经过测试发现 livox_laser_simulation 雷达仿真功能包发布的是 ros PointCloud2 点云格式数据。

而现在针对livox雷达的激光slam算法用的是 livox CustomMsg 点云格式数据，例如 Fast-Lio2 、Faster-Lio 、Lio-livox等。所以想在gazebo中仿真这些算法，需要将 ros PointCloud2格式数据转为livox CustomMsg格式 。

点云格式

PointCloud2 点云格式

PointCloud2 是ros的一种点云格式

具体官方数据 http://docs.ros.org/en/jade/api/sensor_msgs/html/msg/PointCloud2.html

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

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

header:  // 点云的头信息
  seq: 963 //
  stamp:  // 时间戳
    secs: 1541143772
    nsecs: 912011000
  frame_id: "/camera_init"
height: 1   // If the cloud is unordered, height is 1  如果cloud 是无序的 height 是 1
width: 852578  //点云的长度
fields:  //  sensor_msgs/PointField[] fields 
  - 
    name: "x"
    offset: 0
    datatype: 7 	// 	uint8 INT8    = 1
			//	uint8 UINT8   = 2
			//	uint8 INT16   = 3
			//	uint8 UINT16  = 4
			//	uint8 INT32   = 5
			//	uint8 UINT32  = 6
			//	uint8 FLOAT32 = 7
			//	uint8 FLOAT64 = 8
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "intensity"
    offset: 16
    datatype: 7
    count: 1
is_bigendian: False
point_step: 32 // Length of a point in bytes 一个点占的比特数 
row_step: 27282496 // Length of a row in bytes 一行的长度占用的比特数
data: [ .......................................................... ] //  Actual point data, size is (row_step*height)
is_dense: True // 没有非法数据点

livox CustomMsg 点云格式

CustomMsg 是 livox雷达的 专有的点云格式，其格式如下：

Header header             # ROS standard message header
uint64 timebase          # The time of first point
uint32 point_num      # Total number of pointclouds
uint8 lidar_id               # Lidar device id number
uint8[3] rsvd                 # Reserved use
CustomPoint[] points    # Pointcloud data

上述自定义数据包中的自定义点云（CustomPoint）格式 :

uint32 offset_time      # offset time relative to the base time
float32 x               # X axis, unit:m
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
uint8 reflectivity      # reflectivity, 0~255
uint8 tag               # livox tag
uint8 line              # laser number in lidar

• line
  最后一行line是激光雷达扫描的线数（livox avia为6线），从原始扫描点云可以看出，后期可以按线来对原始点处理、筛选
• tag
  tag: 主要指示多回波信息及噪点信息。是一个二进制表达，标记信息的格式如下：bit7 bit6 bit5 bit4 bit3 bit2 bit1 bit0

bit7 bit6 保留位

bit5 bit4 回波序号 : 00: 第 0 个回波 01: 第 1 个回波 10: 第 2 个回波 11: 第 3 个回波 由于Livox Avia 采用同轴光路，即使外部无被测物体，其内部的光学系统也会产生一个回波，该回波记为第 0个回波。随后，若激光出射方向存在可被探测的物体，则最先返回系统的激光回波记为第 1 个回波，随后为第 2个回波，以此类推。如果被探测物体距离过近（例如 1.5 m），第 1 个回波将会融合到第 0 个回波里，该回波记为第 0 个回波。

bit3 bit2 基于强度的点属性： 00：正常点 01：回波能量噪点置信度高 10：回波能量噪点置信度中 11：保留位
基于回波能量强度判断采样点是否为噪点。通常情况下，激光光束受到类似灰尘、雨雾、雪等干扰产生的噪点的回波能量很小。目前按照回波能量强度大小将噪点置信度分为二档：01表示回波能量很弱，这类采样点有较高概率为噪点，例如灰尘点；10表示回波能量中等，该类采样点有中等概率为噪点，例如雨雾噪点。噪点置信度越低，说明该点是噪点的可能性越低。

bit1 bit0 基于空间位置的点属性 : 00：正常点 01：空间噪点置信度高 10：空间噪点置信度中 11：空间噪点置信度低
基于采样点的空间位置判断是否为噪点。例如，激光探测测距仪在测量前后两个距离十分相近的物体时，两个物体之间可能会产生拉丝状的噪点。目前按照不同的噪点置信度分为三档，噪点置信度越低，说明该点是噪点的可能性越低。

• offset_time
  时间戳
  点云数据及 IMU 数据中都包含时间戳信息。

• reflectivity
  目标反射率
  目标反射率：以 0 至 255 表示。其中 0 至 150 对应反射率介于 0 至 100% 的漫散射物体；而 151 至255 对应全反射物体。

• x y z
  坐标信息：Livox Avia 的坐标信息可表示为直角坐标（x, y, z）或球坐标（r, θ, φ）。如果前方无被探测物体或者被探测物体超出量程范围（例如 600 m），在直角坐标系下，点云输出为（0，0，0）；在球坐标系下，点云输出为（0，θ, φ）。

将 ros PointCloud2格式数据转为livox CustomMsg格式

为了使 livox_laser_simulation 功能包可以输出 CustomMsg 格式数据用于 fast-lio仿真，需要对其代码进行修改。主要是livox_laser_simulation/src/livox_points_plugin.cpp 这个文件。

重新编写这个函数

void LivoxPointsPlugin::OnNewLaserScans()

前面的这些可以不变

    if (rayShape) 
    {
        std::vector<std::pair<int, AviaRotateInfo>> points_pair;
        InitializeRays(points_pair, rayShape);
        rayShape->Update();

        msgs::Set(laserMsg.mutable_time(), world->SimTime());
        msgs::LaserScan *scan = laserMsg.mutable_scan();
        InitializeScan(scan);

        SendRosTf(parentEntity->WorldPose(), world->Name(), raySensor->ParentName());

        auto rayCount = RayCount();
        auto verticalRayCount = VerticalRayCount();
        auto angle_min = AngleMin().Radian();
        auto angle_incre = AngleResolution();
        auto verticle_min = VerticalAngleMin().Radian();
        auto verticle_incre = VerticalAngleResolution();

声明要发布的 custom格式的livox 点云

        livox_ros_driver::CustomMsg pp_livox;//声明要发布的 custom格式的livox 点云

赋值点云帧头

        // 赋值点云帧头
        pp_livox.header.stamp = ros::Time::now();//时间
        pp_livox.header.frame_id = "livox";//坐标系
        int count = 0;

用 high_resolution_clock 记录 起始时间

        boost::chrono::high_resolution_clock::time_point start_time = boost::chrono::high_resolution_clock::now();

遍历每个点

        for (auto &pair : points_pair)
        {

获得点的距离

            auto range = rayShape->GetRange(pair.first);

获得点的强度

            auto intensity = rayShape->GetRetro(pair.first);

最大最小距离限制

            if (range >= RangeMax()) {
                range = 0;
            } else if (range <= RangeMin()) {
                range = 0;
            }

将极坐标转为x y z

            auto rotate_info = pair.second;
            // 将极坐标转为x y  z 
            ignition::math::Quaterniond ray;
            ray.Euler(ignition::math::Vector3d(0.0, rotate_info.zenith, rotate_info.azimuth));
            auto axis = ray * ignition::math::Vector3d(1.0, 0.0, 0.0);
            auto point = range * axis;

声明一个 Custom 点

livox_ros_driver::CustomPoint p;//声明一个 Custom 点

赋值坐标

            p.x = point.X();
            p.y = point.Y();
            p.z = point.Z();

赋值强度

 p.reflectivity = intensity;

计算当前点时间到起始时间差

            boost::chrono::high_resolution_clock::time_point end_time = boost::chrono::high_resolution_clock::now();// 当前点时间
            boost::chrono::nanoseconds elapsed_time = boost::chrono::duration_cast<boost::chrono::nanoseconds>(end_time - start_time);
           

赋值偏移时间

p.offset_time = elapsed_time.count();

将当前点存入点云中

pp_livox.points.push_back(p);
// 计点个数的计数器加1
count ++;
}//结束遍历每个点

赋值点云中点的个数

        pp_livox.point_num = count;

发布点云

        livox_pub.publish(pp_livox);
        ros::spinOnce();
    }

测试

将功能包进行编译，需要引入 livox_ros_driver 功能包

find_package(catkin REQUIRED COMPONENTS
  roscpp
  tf
  livox_ros_driver  # 新增
)

包含头文件

#include <livox_ros_driver/CustomMsg.h>

完成编译后启动仿真环境

产看当前话题名
可以看到这个 /livox/lidar 话题名

查看这个话题信息

rostopic info /livox/lidar

jk-jone@JKKC:~$ rostopic info /livox/lidar
Type: livox_ros_driver/CustomMsg
Publishers:
/gazebo (http://JKKC:38793/)
Subscribers: None

消息类型改成了 livox_ros_driver/CustomMsg