参数简介

Velodyne VLP-16
测距精度 ±3.6 cm
转速 RPM 600
数据端口 2368
通讯协议 DUP
Python 3.6.8

数据包介绍

１、数据包结构参考VLP16手册，以下为激光雷达一个完整的数据包，在解析过程中底层会过滤掉42　字节的UDP报头。所以实际接收到的数据长度为1206字节。
２、每个数据包有12个数据块，每个数据块有2字节的0xFFEE数据标记，2个字节的方位角数据，2个数据列，每个数据列又包含总共48字节的16线的点距离信息（每个点距离信息占3个字节——2字节的距离信息，1字节的反射率信息）。

实际数据介绍

注意：在进行数据转换的时候注意将数据字节进行倒置

坐标转换关系

俯仰角：
单位：角度 / °（注意不是弧度）

vertical_angle_list = [-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15]

Z轴垂直高度修正值：
单位：毫米/mm

vertical_correction_list = [11.2, -0.7, 9.7, -2.2, 8.1, -3.7, 6.6, -5.1, 5.1, -6.6, 3.7, -8.1, 2.2, -9.7, 0.7, -11.2]

补偿半径

注意：在坐标转换的时候注意加上41.910mm的补偿半径。

运行结果

代码

import socket
import open3d as o3d
import numpy as np
from math import radians, cos, sin# 垂直角度 w 列表,单位:角度/° [来源请参考VLP16手册]
g_vertical_angle_list = [-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15]
# 垂直距离矫正列表,单位:毫米/mm [来源请参考VLP16手册]
g_vertical_correction_list = [11.2, -0.7, 9.7, -2.2, 8.1, -3.7, 6.6, -5.1, 5.1, -6.6, 3.7, -8.1, 2.2, -9.7, 0.7, -11.2]# 将点云保存到ply文件
def save_to_ply(save_file, point_list):length = len(point_list)with open(save_file, 'w') as f:f.writelines(("ply\n","format ascii 1.0\n","element vertex {}\n".format(length),"property float x\n","property float y\n","property float z\n","property uchar red\n","property uchar green\n","property uchar blue\n","element face 0\n","property list uint8 int32 vertex_index\n","end_header\n"))for i in range(length):f.writelines("%f %f %f %d %d %d\n" % (point_list[i][0], point_list[i][1], point_list[i][2],point_list[i][3], point_list[i][4], point_list[i][5]))class PointCloud:# @ distance：测出的点云距离，单位：m(米)# @ azimuth_angle：数据点与发射中心点的连线在XOY面的分向量与Y轴的夹角,即:方位角# @ w_angle：数据点与发射中心点的连线与平面的夹角,或激光的俯仰角# @ reflect：反射率，单位(百分比 % )#  [坐标换算请参考VLP16手册]def __init__(self, distance, azimuth_angle, w_angle, reflect, correction_index):# 根据手册提供的关系式求出 XYZ 坐标launch_radius = 41.910 / 1000  # 单位：m/米w_angle = radians(w_angle)  # 弧度转换azimuth_angle = radians(azimuth_angle)  # 弧度转换self.x = (distance * cos(w_angle) + launch_radius) * sin(azimuth_angle)self.y = (distance * cos(w_angle) + launch_radius) * cos(azimuth_angle)self.z = distance * sin(w_angle) + g_vertical_correction_list[correction_index] / 1000self.r = reflect  # 颜色以反射率为基准self.g = reflectself.b = reflectself.reflect = reflectdef to_string(self):return '(' + str(self.x) + 'm,' + str(self.y) + 'm,' + str(self.z) + 'm) 'def de_code_lidar_data(data):delta_count = 0delta_value = 0point_cloud_list = []azimuth_angle_list = []# 计算并存储12个 data block 的方位角(azimuth_angle) [详细来源请参考VLP16手册]for i in range(0, 12):block_index = i * 100azimuth_angle_index = block_index + 2azimuth_angle = float(int(data[azimuth_angle_index + 1] << 8) + data[azimuth_angle_index]) * 0.01azimuth_angle_list.append(azimuth_angle)# 计算出每个data block的第15-31个点的方位角并存储至 azimuth_angle_list [详细来源请参考VLP16手册]azimuth_angle_list_tmp = azimuth_angle_list.copy()for i in range(0, 11):a1 = azimuth_angle_list[i]a2 = azimuth_angle_list[i + 1]if a2 < a1:a2 += 360avg = (a1 + a2) / 2delta_value += (a2 - a1) / 2delta_count += 1azimuth_angle_list_tmp.insert(i * 2 + 1, avg)# 方位角平均增量delta_degrees_inc = delta_value / delta_count# 求出最后一个数据序列的方位角azimuth_angle_list_tmp.append(azimuth_angle_list_tmp[len(azimuth_angle_list_tmp) - 1] + delta_degrees_inc)azimuth_angle_list = azimuth_angle_list_tmp.copy()# 遍历12个 data block 的每个数据点  [详细来源请参考VLP16手册]for i in range(0, 12):  # 每个数据包的第 i 个block  [详细来源请参考VLP16手册]block_index = i * 100block_point_index = block_index + 4data_flag = int(data[block_index] << 8) + int(data[block_index + 1])if data_flag == 0xffee:for j in range(0, 2):  # 每个block的第 j 组数据  [详细来源请参考VLP16手册]azimuth_angle = azimuth_angle_list[i * 2 + j]if 0.0 <= azimuth_angle <= 1.0:  # 筛选方向角为 0 - 1 的数据点print('azimuth_angle = ' + str(azimuth_angle))for k in range(0, 16):  # 16线中的第 k 个线# 对应线路的俯仰角  [详细来源请参考VLP16手册]w_angle = g_vertical_angle_list[k]# 点到激光雷达中心的距离  [详细来源请参考VLP16手册]distance = (int(data[block_point_index + j * 48 + k * 3 + 1] << 8) + int(data[block_point_index + j * 48 + k * 3])) / 500# 点的反射率  [详细来源请参考VLP16手册]reflectivity = int(data[block_point_index + j * 48 + k * 3 + 2])print('w_angle = ' + str(w_angle) + ',distance = ' + str(distance) + ',degrees = ' + str(azimuth_angle))# 生成点云对象point = PointCloud(distance, azimuth_angle, w_angle, reflectivity, k)# 数据存入点云列表point_cloud_list.append([point.x, point.y, point.z, point.r, point.g, point.b])else:print('data error !')return point_cloud_listif __name__ == '__main__':np_points = []target_running_time = 5  # 单位:秒/sstart_record_time_flag = 0ip_config = ('', 2368)  # VLP16 的UDP协议端口号为 2368, 可不指定IP地址  [详细来源请参考VLP16手册]my_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)my_socket.bind(ip_config)while True:lidar_data, address = my_socket.recvfrom(1206)# 计算数据包的时间戳, 单位:微妙/us  [详细来源请参考VLP16手册]time_stamp = int(lidar_data[1203] << 24) + int(lidar_data[1202] << 16) + int(lidar_data[1201] << 8) + int(lidar_data[1200])if start_record_time_flag == 0:first_lidar_data_time_stamp = time_stamptime_stamp_old = time_stampstart_record_time_flag = 1np_points += de_code_lidar_data(lidar_data)running_time = (time_stamp - first_lidar_data_time_stamp) / 1000000if running_time >= target_running_time:breakfile_name = 'C:/lidar_point_cloud_01.ply'save_to_ply(file_name, np_points)# 利用 open3d 显示点云print(np_points)pcd = o3d.io.read_point_cloud(file_name)pcd.points = o3d.utility.Vector3dVector(pcd.points)print(pcd)print(np.asarray(pcd.points))o3d.visualization.draw_geometries([pcd])

Python+Open3D 解析Velodyne VLP-16激光雷达数据相关推荐

python json解析超过对象_json序列化数据超出最大值(maxJsonLength)
标签: 1.序列化: 以下代码在对象过大时会报错:进行序列化或反序列化时出错.字符串的长度超过了为 maxJsonLength 属性设置的值. //jsonObj比较大的时候会报错 var seria ...
python解析雷达数据_激光雷达数据解析（Python-lidar-data-analysis_V1.0）
2017年9月3号,杭州天气燥热,跑这来读研了,换专业了,一切从头开始. 上手的第一件事,解析激光雷达数据.(这是个半残废的小代码工程,最后不了了之,当做经验,也来写个半残废的博客) 语言: pyth ...
Python代码解析数据
{"jwotestProduct":null,"score":0,"comments":[{"id":130457723 ...
SLAM实操入门（六）：连接Velodyne的16线激光雷达并可视化
文章目录前言 1 Velodyne激光雷达 2 配置网络 3 创建ROS工程 4 启动并可视化 5 录包与播放总结前言好久没更新这部分了,最近在搞中期答辩的东西,简单补充一部分多线激光雷达建图 ...
python 处理服务器响应数据包,Python如何解析RADIUS服务器数据包？
我试图从RADIUS服务器解析UDP数据包,我试过不同的工具,包括Scapy,Pynids和pypcap.问题是一些半径属性没有正确解码,其中一些是.这可能是什么原因?Python如何解析RADIUS ...
Java 和 Python 解析动态 key 的 JSON 数据
一.概述解析JSON过程中,什么情况都可能遇到.遇到特殊的情况,不会怎么办?肯定不是设计的问题,一定是你的姿势不对. 有这样一种JSON需要解析: {"b3444533f6544" ...
python为什么closed_为什么Python无法解析此JSON数据？ [关闭] - Why can't Python parse this JSON data? [closed]...
问题: I have this JSON in a file: 我在文件中有此JSON: { "maps": [ { "id": "blabla&qu ...
Python深层解析json数据之JsonPath
我们在做接口自动化时,一般接口响应的都是json数据体,对响应数据进行提取使用或断言,当数据量很大或层级很深时,就会变得很麻烦,于是就可以用到jsonpath模块,解决json路径深取值难的问题. 一 ...
Python 解析爬取的车次数据（12306）
背景在完成数据分析.挖掘的过程中,通过各种途径获取到的源数据有时候是不能直接利用的,需要再做进一步加工.处理.解析等操作,以致得到最终的目标结果. 1 获取源数据使用 Python 从 12306 ...

Python+Open3D 解析Velodyne VLP-16激光雷达数据