Livox ROS Driver 2，录制/livox/lidar话题为rosbag，并将PointCloud2点云数据合并保存为pcd

首先创建功能包，也可以使用已有功能包

catkin_create_pkg pcd_packge rospy roscpp std_msgs geometry_msgs

然后，新建pcd_get.py，完整python代码如下

#! /usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import rosbag
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import open3d as o3d
import numpy as np
import os

# 回调函数：收到第一个点云数据后开始录制 rosbag
def point_cloud_callback(msg, bag):
    # 将收到的点云消息写入 rosbag 文件
    bag.write("/livox/lidar", msg)

# 录制 rosbag
def record_and_save_pcd(time_record, bag_save_path):
    rospy.init_node('lidar_record_node', anonymous=True)

    # 设置循环频率
    rate = rospy.Rate(10)  # 10 Hz，即每秒检查10次

    # 创建 rosbag 文件对象
    with rosbag.Bag(bag_save_path, 'w') as bag:
        # 订阅 /livox/lidar 点云话题
        lidar_subscriber = rospy.Subscriber("/livox/lidar", PointCloud2, point_cloud_callback, callback_args=bag)

        # 等待第一次接收到点云数据，才开始录制
        rospy.loginfo("Waiting for first point cloud message...")

        # 用于记录开始时间
        start_time = None
        recording_started = False

        # 继续运行直到录制时间达到 time_record
        while not rospy.is_shutdown():
            if not recording_started:
                # 如果尚未开始录制，则记录开始时间
                start_time = rospy.get_time()
                recording_started = True
                rospy.loginfo("Recording started.")

            # 检查是否已经录制了足够的时间
            if rospy.get_time() - start_time >= time_record:
                rospy.loginfo("Recording finished. Stopping rosbag...")
                # 停止订阅话题
                lidar_subscriber.unregister()
                break

            rate.sleep()  # 按照设定频率休眠

# 从 rosbag 文件读取数据并保存为 pcd
def bag_to_pcd(bag_path, pcd_path, voxel_size):
    """
    将 .bag 文件中的 PointCloud2 数据转换为 PCD 文件并保存，
    先进行体素下采样，再保存为 PCD 文件。
    
    参数:
    bag_path: .bag 文件路径
    pcd_path: 保存 PCD 文件的路径
    voxel_size: 体素下采样的尺寸（体素大小）
    """
    # 读取 .bag 文件
    bag_file = rosbag.Bag(bag_path, 'r')

    # 获取 '/livox/lidar' 话题的数据
    lidar_data = bag_file.read_messages(topics=['/livox/lidar'])

    # 创建一个空的点云对象
    pcd = o3d.geometry.PointCloud()

    # 解析 PointCloud2 消息并提取点云
    def parse_point_cloud2(msg):
        # 使用 sensor_msgs 的 point_cloud2 来解析消息
        pc_data = point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
        # 将点云数据转换为 numpy 数组
        pc_array = np.array(list(pc_data))
        return pc_array

    # 遍历所有消息并合并点云
    for _, msg, _ in lidar_data:
        # 将 PointCloud2 消息解析为 numpy 数组
        points = parse_point_cloud2(msg)
        
        # 将点云数据添加到 open3d 点云对象中
        pcd.points.extend(o3d.utility.Vector3dVector(points))

    # 进行体素下采样
    pcd_downsampled = pcd.voxel_down_sample(voxel_size)

    # 可视化下采样后的点云
    # o3d.visualization.draw_geometries([pcd_downsampled])

    # 保存下采样后的点云为 PCD 文件
    o3d.io.write_point_cloud(pcd_path, pcd_downsampled)

    # 关闭 bag 文件
    bag_file.close()

if __name__ == "__main__":
    try:
        time_record = 2.0  # 录制时间，单位为秒
        bag_save_path = "../file/livox_bag.bag"  # rosbag 保存路径
        pcd_save_path = "../file/livox_pointcloud.pcd"
        
        # 确保文件目录存在
        dir_path = os.path.dirname(bag_save_path)  # 获取文件路径的目录部分
        if not os.path.exists(dir_path):
            os.makedirs(dir_path)  # 创建文件夹（包括必要的中间目录）
            print(f"Directory {dir_path} created.")

        # 录制并保存为 pcd
        record_and_save_pcd(time_record, bag_save_path)
        bag_to_pcd(bag_save_path, pcd_save_path, voxel_size=0.01)  # 转换为 pcd
        
    except rospy.ROSInterruptException:
        pass

该代码可以将多帧的点云数据（因为livox雷达是非重复式扫描）合并到一个pcd文件。
运行：

sudo chmod +x pcd_get.py
python3 pcd_get.py  
# 或者使用rosrun运行也可
rosrun pcd_packge pcd_get.py