Livox雷达ROS数据转换实战：从CustomMsg到PointCloud2的完整指南

刚拿到Livox雷达的开发者们，往往会在ROS环境中遇到一个令人沮丧的问题——明明设备连接正常，数据流也在传输，但Rviz中就是看不到点云显示。这不是你的操作失误，而是Livox采用的CustomMsg格式与ROS生态中广泛使用的sensor_msgs/PointCloud2格式存在兼容性问题。本文将带你深入理解这一技术痛点，并提供一套即插即用的解决方案。

1. 理解Livox数据格式的兼容性问题

Livox雷达在ROS驱动中默认使用的是自定义消息类型livox_ros_driver/CustomMsg，这与ROS标准点云消息sensor_msgs/PointCloud2在数据结构上存在显著差异。这种差异主要体现在三个方面：

1. 字段定义不同：CustomMsg包含了Livox特有的字段如reflectivity、tag和line，而PointCloud2采用更通用的x、y、z坐标加强度值的结构。

2. 数据组织方式不同：CustomMsg使用自定义的结构体数组存储点云，而PointCloud2采用更灵活的字段偏移机制。

3. 工具链支持差异：ROS生态中的大多数工具（如Rviz）和SLAM算法（如A-LOAM、LeGO-LOAM）都是基于PointCloud2设计的。

以下是一个简单的对比表格，展示了两种格式的关键区别：

2. 搭建转换节点的开发环境

在开始编写代码前，我们需要确保开发环境配置正确。以下是详细的准备工作：

2.1 系统与软件要求

• Ubuntu 18.04/20.04（推荐20.04）
• ROS Noetic/Melodic（与Ubuntu版本对应）
• Livox SDK和ROS驱动（已正确安装）
• CMake 3.0+
• C++11及以上编译器

2.2 创建ROS工作空间和包

bash复制# 创建catkin工作空间
mkdir -p ~/livox_ws/src
cd ~/livox_ws/
catkin_make

# 创建转换节点包
cd src
catkin_create_pkg livox_converter roscpp sensor_msgs livox_ros_driver

2.3 配置package.xml和CMakeLists.txt

在livox_converter包的package.xml中添加以下依赖：

xml复制<build_depend>livox_ros_driver</build_depend>
<exec_depend>livox_ros_driver</exec_depend>

在CMakeLists.txt中确保包含以下内容：

cmake复制find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  livox_ros_driver
)

add_executable(livox_to_pcl src/livox_to_pcl.cpp)
target_link_libraries(livox_to_pcl ${catkin_LIBRARIES})

add_executable(pcl_to_livox src/pcl_to_livox.cpp)
target_link_libraries(pcl_to_livox ${catkin_LIBRARIES})

3. 实现CustomMsg到PointCloud2的转换

这是最常用的转换方向，让Livox数据能够被ROS标准工具识别。以下是核心代码实现：

cpp复制#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <livox_ros_driver/CustomMsg.h>

ros::Publisher pcl_pub;

void livoxCallback(const livox_ros_driver::CustomMsg::ConstPtr& msg) {
    sensor_msgs::PointCloud2 cloud;
    cloud.header = msg->header;
    cloud.height = 1;
    cloud.width = msg->point_num;
    cloud.is_dense = false;
    
    // 设置字段
    sensor_msgs::PointCloud2Modifier modifier(cloud);
    modifier.setPointCloud2Fields(4,
        "x", 1, sensor_msgs::PointField::FLOAT32,
        "y", 1, sensor_msgs::PointField::FLOAT32,
        "z", 1, sensor_msgs::PointField::FLOAT32,
        "intensity", 1, sensor_msgs::PointField::FLOAT32);
    
    // 填充数据
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
    sensor_msgs::PointCloud2Iterator<float> iter_i(cloud, "intensity");
    
    for (size_t i = 0; i < msg->point_num; ++i) {
        *iter_x = msg->points[i].x;
        *iter_y = msg->points[i].y;
        *iter_z = msg->points[i].z;
        *iter_i = static_cast<float>(msg->points[i].reflectivity) / 255.0f;
        ++iter_x; ++iter_y; ++iter_z; ++iter_i;
    }
    
    pcl_pub.publish(cloud);
}

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

注意：reflectivity值在Livox中是0-255的uint8，转换为PointCloud2时需要归一化到0-1的float

4. 实现PointCloud2到CustomMsg的转换

虽然不常用，但某些场景下可能需要将处理后的点云数据转换回Livox格式：

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

ros::Publisher livox_pub;

void pclCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
    livox_ros_driver::CustomMsg livox_msg;
    livox_msg.header = msg->header;
    livox_msg.timebase = msg->header.stamp.toNSec();
    livox_msg.point_num = msg->width * msg->height;
    livox_msg.lidar_id = 0;
    
    // 查找字段偏移量
    int x_offset = -1, y_offset = -1, z_offset = -1, i_offset = -1;
    for (const auto& field : msg->fields) {
        if (field.name == "x") x_offset = field.offset;
        else if (field.name == "y") y_offset = field.offset;
        else if (field.name == "z") z_offset = field.offset;
        else if (field.name == "intensity") i_offset = field.offset;
    }
    
    // 填充点数据
    livox_msg.points.resize(livox_msg.point_num);
    for (size_t i = 0; i < livox_msg.point_num; ++i) {
        size_t data_offset = i * msg->point_step;
        livox_msg.points[i].x = *reinterpret_cast<const float*>(&msg->data[data_offset + x_offset]);
        livox_msg.points[i].y = *reinterpret_cast<const float*>(&msg->data[data_offset + y_offset]);
        livox_msg.points[i].z = *reinterpret_cast<const float*>(&msg->data[data_offset + z_offset]);
        
        if (i_offset != -1) {
            float intensity = *reinterpret_cast<const float*>(&msg->data[data_offset + i_offset]);
            livox_msg.points[i].reflectivity = static_cast<uint8_t>(intensity * 255.0f);
        } else {
            livox_msg.points[i].reflectivity = 0;
        }
        
        livox_msg.points[i].tag = 0;
        livox_msg.points[i].line = 0;
        livox_msg.points[i].offset_time = i * 100; // 假设100ns间隔
    }
    
    livox_pub.publish(livox_msg);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "pcl_to_livox");
    ros::NodeHandle nh;
    
    livox_pub = nh.advertise<livox_ros_driver::CustomMsg>("/livox/lidar_custom", 10);
    ros::Subscriber sub = nh.subscribe("/pointcloud_in", 10, pclCallback);
    
    ros::spin();
    return 0;
}

5. 高级应用与性能优化

基础转换实现后，我们可以进一步优化节点性能和功能：

5.1 使用多线程提高处理效率

cpp复制#include <ros/callback_queue.h>

// 在主函数中设置多线程
ros::NodeHandle nh;
nh.setCallbackQueue(&queue);

ros::AsyncSpinner spinner(4, &queue);
spinner.start();

5.2 添加动态参数配置

使用dynamic_reconfigure让用户可以在运行时调整参数：

1. 创建cfg/Converter.cfg文件：

python复制#!/usr/bin/env python
PACKAGE = "livox_converter"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
gen.add("intensity_scale", double_t, 0, "Intensity scaling factor", 1.0, 0.1, 10.0)
gen.add("publish_rate", int_t, 0, "Maximum publish rate (Hz)", 10, 1, 100)
exit(gen.generate(PACKAGE, "livox_converter", "Converter"))

2. 在CMakeLists.txt中添加：

cmake复制generate_dynamic_reconfigure_options(
  cfg/Converter.cfg
)

5.3 点云滤波与降采样

在转换过程中集成简单的滤波处理：

cpp复制#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>

void filteredLivoxCallback(const livox_ros_driver::CustomMsg::ConstPtr& msg) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    
    // 转换到PCL格式
    for (const auto& point : msg->points) {
        pcl::PointXYZI pcl_point;
        pcl_point.x = point.x;
        pcl_point.y = point.y;
        pcl_point.z = point.z;
        pcl_point.intensity = static_cast<float>(point.reflectivity) / 255.0f;
        cloud->push_back(pcl_point);
    }
    
    // 体素滤波
    pcl::VoxelGrid<pcl::PointXYZI> voxel;
    voxel.setInputCloud(cloud);
    voxel.setLeafSize(0.05f, 0.05f, 0.05f); // 5cm体素大小
    voxel.filter(*cloud);
    
    // 转回ROS格式
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*cloud, output);
    output.header = msg->header;
    
    pcl_pub.publish(output);
}

6. 实际部署与调试技巧

完成代码开发后，我们需要将其部署到实际系统中并确保稳定运行：

6.1 启动文件配置

创建launch/livox_converter.launch文件：

xml复制<launch>
    <!-- Livox驱动节点 -->
    <include file="$(find livox_ros_driver)/launch/livox_lidar.launch"/>
    
    <!-- 转换节点 -->
    <node pkg="livox_converter" type="livox_to_pcl" name="livox_to_pcl" output="screen">
        <remap from="/livox/lidar" to="/livox/lidar_raw"/>
        <remap from="/livox/points" to="/livox/points_cloud"/>
    </node>
    
    <!-- Rviz可视化 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find livox_converter)/rviz/livox.rviz"/>
</launch>

6.2 常见问题排查

1. 点云显示异常：

   • 检查Rviz中Global Options的Fixed Frame是否与点云消息的header.frame_id一致
   • 确认点云话题已添加到Rviz显示中并启用

2. 转换节点不工作：

   • 使用rostopic list确认输入话题是否有数据
   • 检查rosnode info /livox_to_pcl查看节点订阅和发布状态

3. 性能问题：

   • 使用top或htop监控CPU使用率
   • 考虑降低发布频率或启用点云降采样

6.3 性能监控脚本

创建一个简单的bash脚本监控节点性能：

bash复制#!/bin/bash

echo "CPU Usage:"
top -bn1 | grep "livox_to_pcl" | awk '{print $9}'

echo "Memory Usage:"
top -bn1 | grep "livox_to_pcl" | awk '{print $10}'

echo "Topic Frequency:"
rostopic hz /livox/points_cloud

在开发过程中，我发现Livox雷达的CustomMsg格式虽然提供了丰富的字段信息，但在实际SLAM应用中，大多数算法只需要基本的x,y,z和intensity数据。因此，转换节点不仅可以解决兼容性问题，还能作为数据预处理的重要环节，过滤掉不必要的字段，提高后续处理效率。