Livox激光雷达数据格式转换实战：从CustomMsg到ROS标准PointCloud2的保姆级教程

第一次接触Livox激光雷达时，我被它独特的数据格式搞得一头雾水。当时正在搭建一个多传感器融合系统，其他雷达都能正常输出标准的ROS PointCloud2消息，唯独Livox的数据让我的标定程序直接崩溃。经过一番折腾才发现，问题出在数据格式的兼容性上——Livox使用了自定义的CustomMsg格式，而大多数ROS工具链都默认使用sensor_msgs/PointCloud2。本文将带你彻底解决这个痛点，不仅教你如何转换格式，还会深入解析两种格式的差异和转换原理。

1. 为什么需要格式转换

在ROS生态中，点云数据的标准格式是sensor_msgs/PointCloud2。这个格式被PCL、rviz、TF等核心工具广泛支持。但Livox出于性能优化和特殊功能考虑，在官方驱动livox_ros_driver中使用了自定义的CustomMsg格式。

主要差异体现在三个方面：

• 字段结构：CustomMsg使用紧凑的二进制存储，而PointCloud2采用标准化的字段布局
• 数据类型：CustomMsg的反射率使用uint8，而PointCloud2通常用float32表示强度
• 扩展字段：CustomMsg包含独有的时间偏移、线号等专有属性

提示：当你在rviz中看不到点云，或者标定工具报"Unsupported message type"错误时，大概率就是格式不匹配的问题。

我曾经在一个SLAM项目中遇到过这样的错误日志：

bash复制[ERROR] [1625487362.345678]: Failed to process point cloud: expected sensor_msgs/PointCloud2, got livox_ros_driver/CustomMsg

2. 两种数据格式的深度解析

2.1 CustomMsg结构剖析

Livox的CustomMsg定义在livox_ros_driver包的CustomMsg.h中，核心结构包括：

cpp复制struct CustomPoint {
    float x, y, z;          // 坐标(m)
    uint8_t reflectivity;   // 反射率(0-255)
    uint8_t tag;            // 点属性标记
    uint8_t line;           // 激光线号
    uint64_t offset_time;   // 相对于时间戳的纳秒偏移
};

关键特性：

• 每个点占用20字节固定大小
• 时间戳精确到纳秒级
• 支持多线激光雷达的线号标识

2.2 PointCloud2标准格式

ROS标准点云消息的结构如下表所示：

典型的PointCloud2消息会使用如下字段定义：

python复制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)
]

3. 实战转换代码实现

3.1 基础转换节点

下面是一个完整的ROS节点实现，订阅CustomMsg并发布转换后的PointCloud2：

cpp复制#include <ros/ros.h>
#include <livox_ros_driver/CustomMsg.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

ros::Publisher pub;

void cloudCallback(const livox_ros_driver::CustomMsg::ConstPtr& msg) {
    pcl::PointCloud<pcl::PointXYZI> cloud;
    cloud.header.frame_id = msg->header.frame_id;
    cloud.width = msg->point_num;
    cloud.height = 1;
    cloud.is_dense = false;
    cloud.points.resize(msg->point_num);

    for (int i = 0; i < msg->point_num; ++i) {
        cloud.points[i].x = msg->points[i].x;
        cloud.points[i].y = msg->points[i].y;
        cloud.points[i].z = msg->points[i].z;
        cloud.points[i].intensity = msg->points[i].reflectivity / 255.0f;
    }

    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(cloud, output);
    output.header.stamp = msg->header.stamp;
    pub.publish(output);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "livox_converter");
    ros::NodeHandle nh;
    
    pub = nh.advertise<sensor_msgs::PointCloud2>("livox/points", 10);
    ros::Subscriber sub = nh.subscribe<livox_ros_driver::CustomMsg>(
        "/livox/lidar", 100, cloudCallback);
    
    ros::spin();
    return 0;
}

3.2 高级特性处理

如果需要保留Livox特有的线号和时间偏移信息，可以扩展PointCloud2的字段：

cpp复制// 在回调函数中添加
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(cloud, output);

// 添加自定义字段
output.fields.push_back(
    createPointField("line", 16, PointField::UINT8, 1));
output.fields.push_back(
    createPointField("offset_time", 17, PointField::UINT64, 1));

// 调整point_step和row_step
output.point_step += 9; // 1(byte) + 8(bytes)
output.row_step = output.point_step * output.width;

// 填充额外数据
for (size_t i = 0; i < msg->point_num; ++i) {
    *reinterpret_cast<uint8_t*>(&output.data[i * output.point_step + 16]) = msg->points[i].line;
    *reinterpret_cast<uint64_t*>(&output.data[i * output.point_step + 17]) = msg->points[i].offset_time;
}

4. 性能优化与实用技巧

4.1 零拷贝转换

对于高频激光雷达(如Livox Mid-40的100,000点/秒)，内存拷贝可能成为瓶颈。可以使用ROS的message_filters和共享指针优化：

cpp复制#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

typedef message_filters::sync_policies::ApproximateTime<
    livox_ros_driver::CustomMsg, sensor_msgs::Imu> SyncPolicy;

void callback(const livox_ros_driver::CustomMsg::ConstPtr& cloud_msg,
              const sensor_msgs::Imu::ConstPtr& imu_msg) {
    // 直接操作原始数据指针...
}

int main() {
    message_filters::Subscriber<livox_ros_driver::CustomMsg> cloud_sub(nh, "/livox/lidar", 10);
    message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh, "/livox/imu", 100);
    message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), cloud_sub, imu_sub);
    sync.registerCallback(boost::bind(&callback, _1, _2));
}

4.2 常见问题排查

问题1：转换后的点云在rviz中显示异常

• 检查frame_id是否设置正确
• 确认point_step计算是否准确
• 验证字节序(is_bigendian)设置

问题2：数据丢失或错位

• 确保point_num与实际点数一致
• 检查offset计算是否正确
• 验证数据类型转换(特别是反射率的归一化)

问题3：性能低下

• 使用ros::Publisher::getNumSubscribers()控制转换频率
• 考虑使用nodelet减少序列化开销
• 对静态场景可以启用点云降采样

5. 工程化部署方案

5.1 创建独立功能包

建议将转换器封装为独立ROS包，便于复用：

bash复制catkin_create_pkg livox_converter roscpp livox_ros_driver sensor_msgs pcl_conversions

目录结构示例：

code复制livox_converter/
├── CMakeLists.txt
├── package.xml
├── include/
└── src/
    ├── converter_node.cpp
    └── livox_converter.cpp  # 核心转换逻辑

5.2 参数配置化

通过ROS参数服务器使转换器更灵活：

yaml复制# params.yaml
livox_converter:
  input_topic: "/livox/lidar"
  output_topic: "/points"
  frame_id: "livox_frame"
  publish_rate: 10.0
  keep_line_info: false

在节点中加载参数：

cpp复制ros::NodeHandle private_nh("~");
std::string input_topic, output_topic;
private_nh.param("input_topic", input_topic, std::string("/livox/lidar"));
private_nh.param("output_topic", output_topic, std::string("/points"));

5.3 集成到launch文件

创建标准启动文件：

xml复制<launch>
    <node pkg="livox_converter" type="converter_node" name="livox_converter" output="screen">
        <rosparam command="load" file="$(find livox_converter)/params.yaml" />
        <param name="frame_id" value="$(arg frame_id)" />
    </node>
</launch>

6. 实际项目中的经验分享

在最近的一个自动驾驶项目中，我们使用三台Livox Horizon激光雷达组建感知系统。最初直接使用原始CustomMsg格式，导致以下问题：

1. 标定工具无法识别点云
2. 点云配准算法需要重写适配层
3. 点云可视化需要特殊配置

通过实现本文介绍的转换器，我们获得了以下收益：

• 兼容性：所有标准ROS工具链即插即用
• 性能：经过优化的转换器仅增加<1ms延迟
• 灵活性：通过参数配置支持不同型号Livox设备

一个特别有用的技巧是在转换时添加设备序列号作为字段，这样在多雷达系统中可以轻松区分点云来源：

cpp复制// 在转换回调中添加
output.fields.push_back(createPointField("device_id", 16, PointField::UINT8, 1));
// ...
*reinterpret_cast<uint8_t*>(&output.data[i * output.point_step + 16]) = device_id_;