Apollo CyberRT 固态激光雷达数据录制脚本 (PCD格式)

订阅/apollo/sensor/lidar/scan话题，接收点云数据并保存为PCD格式。

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

import os
import time
import argparse
from datetime import datetime
import numpy as np
from cyber.python.cyber_py3 import cyber
from cyber.python.cyber_py3 import cyber_time
from modules.drivers.proto.pointcloud_pb2 import PointCloud

def write_pcd(filename, points):
"""
将点云数据写入PCD文件

参数:
filename: 输出文件名
points: 点云数据(Nx3或Nx4 numpy数组)
"""
header = """# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH {}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {}
DATA ascii
""".format(len(points), len(points))

with open(filename, 'w') as f:
f.write(header)
for point in points:
if point.shape[0] == 3:# 只有xyz
f.write("{:.6f} {:.6f} {:.6f} {:.6f}\n".format(
point[0], point[1], point[2], 0.0))
else:# 包含强度
f.write("{:.6f} {:.6f} {:.6f} {:.6f}\n".format(
point[0], point[1], point[2], point[3]))

class LidarRecorder:
def __init__(self, output_dir="lidar_pcd", duration=None):
"""
初始化激光雷达录制器

参数:
output_dir: 输出目录
duration: 录制持续时间(秒)，None表示无限录制
"""
self.output_dir = output_dir
self.duration = duration
self.is_recording = False
self.start_time = None
self.frame_count = 0

# 创建输出目录
os.makedirs(self.output_dir, exist_ok=True)

# 初始化CyberRT
cyber.init()
self.node = cyber.Node("lidar_recorder")

# 订阅激光雷达话题
self.lidar_reader = self.node.create_reader(
"/apollo/sensor/lidar/scan",
PointCloud,
self.lidar_callback)

print(f"激光雷达录制器初始化完成，数据将保存到: {os.path.abspath(self.output_dir)}")

def lidar_callback(self, point_cloud_pb):
"""
激光雷达数据回调函数

参数:
point_cloud_pb: PointCloud protobuf消息
"""
if not self.is_recording:
return

# 将protobuf消息转换为numpy数组
points = np.array([
(point.x, point.y, point.z, point.intensity)
for point in point_cloud_pb.point
])

# 生成时间戳和文件名
timestamp = point_cloud_pb.header.timestamp_sec
dt = datetime.fromtimestamp(timestamp)
time_str = dt.strftime("%Y%m%d_%H%M%S_%f")[:-3]
filename = os.path.join(
self.output_dir,
f"lidar_{time_str}_{self.frame_count:06d}.pcd"
)

# 保存为PCD文件
write_pcd(filename, points)
self.frame_count += 1

# 打印进度
if self.frame_count % 10 == 0:
print(f"已保存 {self.frame_count} 帧点云数据")

# 检查录制时长
if self.duration is not None and (time.time() - self.start_time) >= self.duration:
self.stop_recording()

def start_recording(self):
"""开始录制"""
if self.is_recording:
print("已经在录制中")
return

self.is_recording = True
self.start_time = time.time()
self.frame_count = 0
print(f"开始录制激光雷达数据到 {self.output_dir}")

# 如果是无限录制，需要手动停止
if self.duration is None:
print("无限录制模式，按Ctrl+C停止...")

def stop_recording(self):
"""停止录制"""
if not self.is_recording:
print("当前没有在录制")
return

self.is_recording = False
elapsed = time.time() - self.start_time
print(f"\n录制已停止，共录制 {elapsed:.2f} 秒")
print(f"共保存 {self.frame_count} 帧点云数据")
print(f"数据已保存到: {os.path.abspath(self.output_dir)}")

def spin(self):
"""运行节点"""
try:
self.node.spin()
except KeyboardInterrupt:
self.stop_recording()
cyber.shutdown()

def main():
parser = argparse.ArgumentParser(description="Apollo CyberRT 激光雷达数据录制工具")
parser.add_argument("--output", "-o", default="lidar_pcd",
help="输出目录 (默认: lidar_pcd)")
parser.add_argument("--duration", "-d", type=float,
help="录制持续时间(秒)，不指定则为无限录制")

args = parser.parse_args()

recorder = LidarRecorder(
output_dir=args.output,
duration=args.duration
)

# 开始录制
recorder.start_recording()

# 运行节点
recorder.spin()

if __name__ == "__main__":
main()

使用说明

1. 依赖安装:

• 确保已安装Apollo CyberRT Python环境
• 需要numpy库

2. 运行脚本:

python lidar_recorder.py --output /path/to/output --duration 60

• --output: 指定输出目录（默认为当前目录下的lidar_pcd）
• --duration: 指定录制时长（秒），不指定则为无限录制

3. 停止录制:

• 如果设置了--duration参数，录制会在指定时间后自动停止
• 无限录制模式下，按Ctrl+C手动停止

4. 输出文件:

• 每个点云帧会保存为一个单独的.pcd文件
• 文件名包含时间戳和帧序号

注意事项

1. 脚本假设点云数据包含x, y, z和intensity字段，如果您的雷达数据格式不同，需要调整write_pcd函数

2. 在Apollo环境中运行时，确保:

• CyberRT已正确初始化
• 激光雷达驱动节点已启动并发布/apollo/sensor/lidar/scan话题

3. 对于大规模录制，建议使用更高效的二进制格式（如.bin）替代ASCII格式的PCD文件