最近一直在处理点云的数据，所以简单了解了一下Velodyne LiDAR相关的内容。这篇博客简单记录一下。
1.VeloView软件简介
VeloView是一个专门用来可视化Velodyne LiDAR数据的软件，官网地址是这里。找到适合自己的版本下载安装即可，如下。
软件十分方便好用，整体界面如下所示。
软件可以打开记录好的pcap文件或者读取LiDAR传感器实时的数据流。目前VeloView支持如下传感器的数据：
包括常见的VLP16、VLP32等。
1.1 打开pcap文件
pcap文件是网络数据抓包保存的常用格式，全称是Package Capture。所有的pcap可以用Wireshark软件打开，如下。
可以看到整个pcap文件由许多packet组成。而这些packet是什么意思，就由各自定义的数据结构组成。比如VLP16官方手册里可以看到，如下。
每个长度为1248的packet是就是用来实际存储测量数据的。VeloView可以直接打开由Velodyne观测而保存的点云数据，如下。
这里稍微需要注意一下的是，VeloView打开的pcap文件路径不能有中文，不然会报错。
1.2 pcap文件信息查看
VeloView可以查看非常详细的点云信息，我们可以点击Tools-Spreadsheet，然后就会弹出SpreadSheet界面。可以看到，LiDAR观测到的每个点的三维坐标、回波强度、垂直角度等都可以查看。
另外，这里需要稍微介绍一下的是。其实LiDAR的原始数据本身是没有所谓的“帧”的概念的。我们看到的一帧一帧的LiDAR点云，其实都是软件根据一定的规则对原始数据划分的结果。真正的原始数据是pcap里存储的一个个packet。那么你可能问，如何划分帧或者说如何定义帧呢？这其实有很多方法。一个比较简单的想法就是：我们读取pacp中第一个有效数据packet中的第一个三维点，记录下它的旋转角度。然后依次遍历packet，直到找到和这个旋转角度最相近的那个packet。这样我们就认为，这两个packet之间存储的数据就是扫描了360度，一圈，以此作为一帧。之后同理，逐个遍历划分。当然了，这部分内容其实和pcap的数据存储结构密切相关，感兴趣可以详细阅读上面的VLP16的手册，里面都有介绍，这里不再赘述。
1.3 pcap文件输出
VeloView不仅仅可以可视化点云，更重要的一个功能是可以将pcap文件转换、输出成csv文件。
每一个csv文件表示一帧数据，如下所示。
在csv文件中，每一行则表示一个观测点，包含该点的回波强度、三维信息、垂直角度、水平旋转角度等信息。这里简单解释一下timestamp和adjustedtime的区别。timestamp是指一个小时以内的累积秒数(以一个小时为周期，超过了就变为0)，而adjustedtime则是包含小时的累积秒数，可以看作是timestamp的累加结果。而timestamp是从哪里来呢？根据VLP16的数据结构，从数据packet中解析出来的。有了每一帧的csv文件，我们就可以恢复出所有的三维点云。但需要注意的是，尽管timestamp是从packet中直接解析出来的。但是并不是绝对时间戳。如果要获得绝对时间戳，需要结合长度为554的packet中包含的GPRMC数据进行解析。GPRMC是GPS提供的年月日时分秒。我们可以先将GPS时间转换为GPS累积秒，然后再将GPS累计秒转化为计算机的Unix累积秒。转换过程可以参考这个网页。当然，在实际转换中如果出现和期望值相差较大的情况，请确认时区是否正确(中国默认是东八区，比格林尼治时间差8小时)。
2.点云数据转ROS Bag
在之前的这篇博客中，我们介绍了利用ROS的相关工具，将VLP16的pcap文件转换为ROS Bag的方法。当时说，这个方法什么都好，但就是有个问题：无法保存数据的绝对时间戳，转换之后，数据都变成了转换时刻的时间戳，而非采集时刻。当时这个问题没有解决办法。那么这里，我们给出一种解决办法，也就是根据VLP16数据结构，完全自己解析pcap文件，将数据以VeloView的格式保存为csv文件(csv文件名称即为该帧时间戳，单位为纳秒)。最后，编写脚本，利用ROS提供的API，将多个csv文件合并为一个ROS Bag文件。
对于pcap文件解析，这里就不再赘述。这一部分的代码我也没过多深究，也比较底层(有空可以简单介绍)。不管怎么解析，最后你可以得到多个csv文件。然后我们编写了代码，如下。
# coding=utf-8
import os
import sys
import numpy as np
import cv2
import rosbag
import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
def findFiles(root_dir, filter_type, reverse=False):
"""
在指定目录查找指定类型文件 -> paths, names, files
:param root_dir: 查找目录
:param filter_type: 文件类型
:param reverse: 是否返回倒序文件列表，默认为False
:return: 路径、名称、文件全路径
"""
separator = os.path.sep
paths = []
names = []
files = []
for parent, dirname, filenames in os.walk(root_dir):
for filename in filenames:
if filename.endswith(filter_type):
paths.append(parent + separator)
names.append(filename)
for i in range(paths.__len__()):
files.append(paths[i] + names[i])
print(names.__len__().__str__() + " files have been found.")
paths = np.array(paths)
names = np.array(names)
files = np.array(files)
index = np.argsort(files)
paths = paths[index]
names = names[index]
files = files[index]
paths = list(paths)
names = list(names)
files = list(files)
if reverse:
paths.reverse()
names.reverse()
files.reverse()
return paths, names, files
def readLidarCSV(file_path):
point_list = []
fin = open(file_path, 'r')
fin.readline()
line = fin.readline().strip()
while line:
parts = line.split(",")
intensity = float(parts[0])
azimuth = float(parts[2])
distance = float(parts[3])
vertical_angle = float(parts[5])
pos_x = float(parts[6])
pos_y = float(parts[7])
pos_z = float(parts[8])
point_list.append([pos_x, pos_y, pos_z, intensity, distance, azimuth, vertical_angle])
line = fin.readline().strip()
return point_list
if __name__ == '__main__':
input_dir = sys.argv[1]
out_dir = sys.argv[2]
# lidar参数
lidar_dir = input_dir
lidar_type = ".csv"
lidar_topic_name = "/VLP/pointcloud"
# 统计信息
fout = open(out_dir + "/summary.txt", 'w')
# Bag输出路径
bag_path = out_dir + "/combine.bag"
# 新建ROS Bag输出
bag_out = rosbag.Bag(bag_path, 'w')
# LiDAR数据转换
# ----------------------------------------------------------
paths, names, files = findFiles(lidar_dir, lidar_type)
for i in range(len(files)):
ts = float(names[i].split(".")[0]) / 1e9
tmp_path = files[i]
point_list = readLidarCSV(tmp_path)
points = np.array(point_list)
lidar_msg = PointCloud2()
lidar_msg.header.frame_id = "map"
lidar_ts_ros = rospy.rostime.Time.from_sec(ts)
lidar_msg.header.stamp = lidar_ts_ros
if len(points.shape) == 3:
lidar_msg.height = points.shape[1]
lidar_msg.width = points.shape[0]
else:
lidar_msg.height = 1
lidar_msg.width = len(points)
lidar_msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1),
PointField('distance', 16, PointField.FLOAT32, 1),
PointField('azimuth', 20, PointField.FLOAT32, 1),
PointField('vertical_angle', 24, PointField.FLOAT32, 1),
]
lidar_msg.is_bigendian = False
lidar_msg.point_step = 28
lidar_msg.row_step = lidar_msg.point_step * points.shape[0]
lidar_msg.is_dense = False
lidar_msg.data = np.asarray(points, np.float32).tostring()
bag_out.write(lidar_topic_name, lidar_msg, lidar_ts_ros)
print("lidar", i + 1, "/", len(files),',',ts)
fout.write("LiDAR start timestamp(unit:s)\t"+str(int(names[0].split(".")[0]) / 1e9)+"\n")
fout.write("LiDAR end timestamp(unit:s)\t"+str(int(names[-1].split(".")[0]) / 1e9)+"\n")
# ----------------------------------------------------------
bag_out.close()
fout.close()
利用上述代码，我们就可以将多个csv文件转换成一个ROS Bag文件。比如我们运行了脚本以后，一个转换好的ROS Bag信息如下。
可以看到，ROS Bag中包含461个帧，采集时间范围为1338893122.09-1338893168.09(单位：秒)。这样，我们就实现了最初的需求，通过pcap->csv->ROS Bag。当然，这里需要说明的是，你csv的结构和我这里不同的话，那么就需要做相应的修改，否则会报错。该脚本也上传到了这个Github项目中，对应genBagFromLidar.py文件，感兴趣可以查看。
3.多源数据转ROS Bag
在之前这篇博客中，我们分别实现了影像数据、IMU数据、LiDAR数据以及影像和IMU数据转换ROS Bag的功能。在本篇博客的第二部分，也实现了LiDAR数据转ROS Bag的功能。在这里，我们整理一个“终极”版本，也就是同时将相机、IMU、LiDAR转换到一个ROS Bag中。代码如下。
# coding=utf-8
import os
import sys
import numpy as np
import cv2
import rosbag
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, Imu
from geometry_msgs.msg import Vector3
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
def findFiles(root_dir, filter_type, reverse=False):
"""
在指定目录查找指定类型文件 -> paths, names, files
:param root_dir: 查找目录
:param filter_type: 文件类型
:param reverse: 是否返回倒序文件列表，默认为False
:return: 路径、名称、文件全路径
"""
separator = os.path.sep
paths = []
names = []
files = []
for parent, dirname, filenames in os.walk(root_dir):
for filename in filenames:
if filename.endswith(filter_type):
paths.append(parent + separator)
names.append(filename)
for i in range(paths.__len__()):
files.append(paths[i] + names[i])
print(names.__len__().__str__() + " files have been found.")
paths = np.array(paths)
names = np.array(names)
files = np.array(files)
index = np.argsort(files)
paths = paths[index]
names = names[index]
files = files[index]
paths = list(paths)
names = list(names)
files = list(files)
if reverse:
paths.reverse()
names.reverse()
files.reverse()
return paths, names, files
def readIMU(imu_path):
timestamps = []
wxs = []
wys = []
wzs = []
axs = []
ays = []
azs = []
fin = open(imu_path, 'r')
fin.readline()
line = fin.readline().strip()
while line:
parts = line.split(",")
ts = float(parts[0]) / 1e9
wx = float(parts[1])
wy = float(parts[2])
wz = float(parts[3])
ax = float(parts[4])
ay = float(parts[5])
az = float(parts[6])
timestamps.append(ts)
wxs.append(wx)
wys.append(wy)
wzs.append(wz)
axs.append(ax)
ays.append(ay)
azs.append(az)
line = fin.readline().strip()
return timestamps, wxs, wys, wzs, axs, ays, azs
def readLidarCSV(file_path):
point_list = []
fin = open(file_path, 'r')
fin.readline()
line = fin.readline().strip()
while line:
parts = line.split(",")
intensity = float(parts[0])
azimuth = float(parts[2])
distance = float(parts[3])
vertical_angle = float(parts[5])
pos_x = float(parts[6])
pos_y = float(parts[7])
pos_z = float(parts[8])
point_list.append([pos_x, pos_y, pos_z, intensity, distance, azimuth, vertical_angle])
line = fin.readline().strip()
return point_list
if __name__ == '__main__':
project_dir = sys.argv[1]
out_dir = sys.argv[2]
if len(sys.argv) != 3:
print("Wrong params, input params: project_dir out_bag_path")
exit()
# camera参数
img_dir = project_dir + "/Camera"
img_type = ".jpg"
img_topic_name = "/Camera/image_raw"
# imu参数
imu_path = project_dir + "/Synchronise/IMU.csv"
imu_topic_name = "/IMU/imu_raw"
# lidar参数
lidar_dir = project_dir + "/VLP16"
lidar_type = ".csv"
lidar_topic_name = "/VLP/pointcloud"
# 统计信息
fout = open(out_dir + "/summary.txt", 'w')
# Bag输出路径
bag_path = out_dir + "/combine.bag"
# 新建ROS Bag输出
bag_out = rosbag.Bag(bag_path, 'w')
# IMU数据转换
# ----------------------------------------------------------
imu_ts, wxs, wys, wzs, axs, ays, azs = readIMU(imu_path)
imu_msg = Imu()
angular_v = Vector3()
linear_a = Vector3()
for i in range(len(imu_ts)):
imu_ts_ros = rospy.rostime.Time.from_sec(imu_ts[i])
imu_msg.header.stamp = imu_ts_ros
imu_msg.header.frame_id = "map"
angular_v.x = wxs[i]
angular_v.y = wys[i]
angular_v.z = wzs[i]
linear_a.x = axs[i]
linear_a.y = ays[i]
linear_a.z = azs[i]
imu_msg.angular_velocity = angular_v
imu_msg.linear_acceleration = linear_a
bag_out.write(imu_topic_name, imu_msg, imu_ts_ros)
print('imu:', i, '/', len(imu_ts),',',imu_ts[i])
fout.write("IMU start timestamp(unit:s)\t"+str(imu_ts[0])+"\n")
fout.write("IMU end timestamp(unit:s)\t"+str(imu_ts[-1])+"\n")
# ----------------------------------------------------------
# Camera数据转换
# ----------------------------------------------------------
paths, names, files = findFiles(img_dir, img_type)
cb = CvBridge()
for i in range(len(files)):
frame_img = cv2.imread(files[i])
timestamp = int(names[i].split(".")[0]) / 1e9
ros_ts = rospy.rostime.Time.from_sec(timestamp)
ros_img = cb.cv2_to_imgmsg(frame_img, encoding='bgr8')
ros_img.header.stamp = ros_ts
bag_out.write(img_topic_name, ros_img, ros_ts)
print('image:', i, '/', len(files),',',timestamp)
fout.write("Camera start timestamp(unit:s)\t"+str(int(names[0].split(".")[0]) / 1e9)+"\n")
fout.write("Camera end timestamp(unit:s)\t"+str(int(names[-1].split(".")[0]) / 1e9)+"\n")
# ----------------------------------------------------------
# LiDAR数据转换
# ----------------------------------------------------------
paths, names, files = findFiles(lidar_dir, lidar_type)
for i in range(len(files)):
ts = float(names[i].split(".")[0]) / 1e9
tmp_path = files[i]
point_list = readLidarCSV(tmp_path)
points = np.array(point_list)
lidar_msg = PointCloud2()
lidar_msg.header.frame_id = "map"
lidar_ts_ros = rospy.rostime.Time.from_sec(ts)
lidar_msg.header.stamp = lidar_ts_ros
if len(points.shape) == 3:
lidar_msg.height = points.shape[1]
lidar_msg.width = points.shape[0]
else:
lidar_msg.height = 1
lidar_msg.width = len(points)
lidar_msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1),
PointField('distance', 16, PointField.FLOAT32, 1),
PointField('azimuth', 20, PointField.FLOAT32, 1),
PointField('vertical_angle', 24, PointField.FLOAT32, 1),
]
lidar_msg.is_bigendian = False
lidar_msg.point_step = 28
lidar_msg.row_step = lidar_msg.point_step * points.shape[0]
lidar_msg.is_dense = False
lidar_msg.data = np.asarray(points, np.float32).tostring()
bag_out.write(lidar_topic_name, lidar_msg, lidar_ts_ros)
print("lidar", i + 1, "/", len(files),',',ts)
fout.write("LiDAR start timestamp(unit:s)\t"+str(int(names[0].split(".")[0]) / 1e9)+"\n")
fout.write("LiDAR end timestamp(unit:s)\t"+str(int(names[-1].split(".")[0]) / 1e9)+"\n")
# ----------------------------------------------------------
bag_out.close()
fout.close()
代码还是放到了GitHub项目中，点击查看，对应genBagFromCamImuLidar.py文件。首先说明一下我们现有的数据组织形式，如下。
-
Camera：以EuRoC数据集为标准，以jpg格式保存图像，图像名称为拍摄时刻的时间戳(单位：纳秒)。
-
IMU：以EuRoC数据集为标准，以csv格式保存数据，文件中的每一行代表一次观测，每次观测包含时间戳(纳秒)、角速度、加速度。
-
LiDAR：以Velodyne官方的VeloView软件输出的csv文件为标准，以csv格式保存每帧数据，文件名为当前帧时间戳(纳秒)。文件中的每一行代表一个观测点，每个观测点包含时间戳(纳秒)、三维坐标、垂直角度、距离、强度信息。
因此，我们脚本的输入为数据采集软件保存数据的根目录(如./ 20220610_184503_abcdef)以及期望的输出目录(如./out)。脚本会在输出目录下输出一个combine.bag文件以及一个summary.txt文件。combine.bag文件即为融合以后的数据，summary.txt文件里存放了各个传感器采集数据的起始与终止时间。当然了，上述代码如果需要，也可以按照你自己的需求进行修改。ROS Bag文件中相关Topic说明如下。
最终，我们合并以后的ROS Bag相关信息如下。
用RViz可视化效果如下。相机数据展示在左下角，LiDAR点云数据展示在中间部分，IMU数据以紫色箭头展示，可以看到，各个传感器的数据均正常转换。
ROS Play节点发布的Topic展示如下，可以看到，发布的Topic名称是和上面提到的表格对应的。
在播放过程中，对Topic进行监听如下，可以看到，相机、IMU、LiDAR的帧率均正常，且在合理范围内。
最后，为了验证我们生成的ROS Bag的完整性，利用rqt_bag工具可视化ROS Bag内容，如下所示。可以看到，经过合并的多传感器数据都在同一时间框架内，起始与终止时间基本一致，说明数据转换过程是正确的。
为了更加方便了解代码和测试，整理了一小部分数据放在百度云盘上。链接：https://pan.baidu.com/s/1Ip-oskL9LsZqJ5uJygek2A 提取码：1517。
4.参考资料
- [1] https://www.paraview.org/VeloView#overview
- [2] https://www.doc88.com/p-99829253299188.html
本文作者原创,未经许可不得转载,谢谢配合
