使用Open3D库直接从激光雷达中读取点云信息并显示

本文的目的是直接从雷达的端口读取到数据，并将数据解析成点云的形式然后显示出来。

使用的Python库

import numpy as np
import open3d as o3d
import threading
from socket import *
from queue import Queue

文章中使用的雷达是robosense16，不同厂家的雷达和线束会带来参数上的差异。

准备工作
使用udp的方法，绑定对应的端口。

udpsocket = socket(AF_INET, SOCK_DGRAM)
udpsocket.bind(("", 6699))
q = Queue(maxsize=1) # 队列的长度为1，来达到实时

第一个函数根据不同的线束来映射相对应的角度，16线雷达的线束ID为1到8对应-15到-1，9到16对应15到1。
第二个函数根据收到的每个block中的数据和当前水平角度，来计算点的x,y,z和反射值。（在本方法中过滤了距离大于120的点，方向为任意方向。）

def channelmaptheta(c):
    theta = np.array([-15, -13, -11, -9, -7, -5, -3, -1, 15, 13, 11, 9, 7, 5, 3, 1])
    theta = theta * np.pi / 180
    return theta[c]

def distance(lidardata, alpha):
    points = []
    for i in range(0, 96, 6):
        omega = channelmaptheta(int(i / 6))
        # print(alpha, omega)
        dis = int(lidardata[i + 0:i + 4], 16) / 100
        if (dis > 120):
            continue
        reflex = int(lidardata[i