关于揽沃Livox激光雷达python版SDK

简介

前不久跟着导师做项目时，需要使用激光雷达，选择品牌为Livox揽沃。在揽沃官网上有着C++版sdk，但导师需要基于python环境进行开发，因此看着雷达官网的通信协议手码了python版本的sdk。

 

---

资料

揽沃通信协议中文版连接

揽沃通信协议英文版链接

 

一、关于雷达工作过程

在通信的过程中，雷达或中心板被定义为从设备，接收点云数据的电脑被被定义为主设备。

主设备与从设备通信的流程如下:

1.当从设备上电后，将会从65000端口向局域网中的55000端口广播设备信息数据。

2.接收到广播数据后，主设备向65000端口发送数据请求，请求数据中包含源端口。

3.从设备接收到连接请求后向这个源端口回复ACK数据。

4.当用户接收到ACK数据，握手完成。可以开始发送心跳维持连接，并进行命令和数据的传输。

注意：如果心跳超时，Livox雷达/Hub将会重新回到广播状态。

以上为官方协议中内容，我们需要监听55000端口获取雷达广播到的数据，在python中使用socket创建udp进行等待数据到来，数据到来后解析数据，判断是否收到的为雷达发出的广播信息，如果是则由55000端口向65000端口发送打包好的数据，数据中包含了本地ip，以及接收命令的端口号，接收点云数据的端口号，接收imu传感器数据的端口号，发送完成后等待回应，这步就是所谓的握手。

当握手成功后，以1hz频率向雷达发送打包好的数据（0x00，0x03类型）------心跳信号来维持雷达与主机的连接。在维持的过程中可以继续通过握手时设定的端口向65000发送其他请求。

二、关于通信协议分析

1.协议帧

在所有收到的数据与发送的数据，所采用的都是基于该协议帧发送的，其中data字段为要发送的请求类型。

在我实际编写代码时，需要对发送的数据打包，也就是只需要知道length与data，其他的都是固定值，再根据不同的length、data与固定值分别算出crc16与crc32。

2.代码架构

一切数据都是通过协议帧为基础的，而数据有打包与解包两种操作，还需要计算crc，因此创建一个基础类里面包含了这些相同部分，接着写出每一个命令的类，让他们继承该基础类，这样大致相同的可以不重写打包算法，而不相同的只需要重写一下函数即可。在使用时，需要哪个命令就声明哪一个类即可。代码如下：

import crcmod
import struct



class BaseFrameCMD_ACK:
	def __init__(self):
		self.cmd_frame_data = None
		self.ack_frame_data = [None,  None,    None,     None,    None,    None,   None,     None]
		self.struct_pack_mode = ['<BBHBH', 'H',  'I']
		self.name = None
		self.lenght = None
		self.cmd_frame = None


	#crc16计算
	def calculate_crc16(self, data):
		crc16_func = crcmod.mkCrcFun(0x11021, rev=True, initCrc=0x4C49)
		crc16_value = crc16_func(data)
		return crc16_value

	#crc32计算
	def calculate_crc32(self, data):
		crc32_func = crcmod.mkCrcFun(0x104C11DB7, rev = True, initCrc=0x564F580A, xorOut=0xFFFFFFFF)
		crc32_value = crc32_func(data)
		return crc32_value
	
	#设置cmd_frame_data
	def _set_cmd_data(self, param):

		try:
			head = struct.pack(self.cmd_frame[0], *[0xaa, 0x01, self.lenght, 0x00, 0x00])
			crc_16 = struct.pack(self.cmd_frame[1], self.calculate_crc16(head))
			if type(param) == list:
				data = struct.pack(self.cmd_frame[2], *self.id, *param)
			elif param == None:
				data = struct.pack(self.cmd_frame[2], *self.id)
			else:
				data = struct.pack(self.cmd_frame[2], *self.id, param)

			crc_32 = struct.pack(self.cmd_frame[3], self.calculate_crc32(head + crc_16 + data))

			self.cmd_frame_data = head + crc_16 + data + crc_32
		except:
			print(f'打包{self.name}命令时出错')

	#获取ack_frame_data
	def get_ack_data(self, pack):
		try:
			unpack = struct.unpack(''.join(self.struct_pack_mode), pack)			
		except:
			print(f'解析{self.name}命令时出错')
			return None 

		for i in range(6):
			self.ack_frame_data[i] = unpack[i]
		self.ack_frame_data[6] = unpack[6:-1]
		self.ack_frame_data[7] = unpack[-1]
	
		return self.ack_frame_data

	#将协议帧中的数据段解析为元组格式，每个格式为对应列表索引
	def data_pack_assemble(self):
		try:
			data_pack_assemble_table = []
			pass_num = 0
			for num in self.struct_pack_mode[2]:
				if pass_num > 0:
					pass_num -= 1
					continue
				if num in ['B', 'f', 'i', 'H', 'I', 'L', 's', 'h']:
					data_pack_assemble_table.append(1)
				elif num == '1':
					pass_num = 2
					data_pack_assemble_table.append(16)
				else:
					pass_num = 1 
					data_pack_assemble_table.append(int(num))


			t_list, idx = [], 0
			for num in data_pack_assemble_table:
				if num > 1:
					t_list.append(self.ack_frame_data[6][idx: idx+num])
				else:
					t_list.append(self.ack_frame_data[6][idx])
				idx += num

			self.ack_frame_data[6] = t_list
			return t_list
		except:
			print(f'解析{self.name}数据段失败')
			return None




class SetBroadcastMessage(BaseFrameCMD_ACK):
	def __init__(self):
		super(SetBroadcastMessage, self).__init__()
		self.struct_pack_mode.insert(2, 'BB16BBH')
		self.name = 'SetBroadcastMessage 广播消息'
		self.id = (0x00, 0x00)

	
class Handshake(BaseFrameCMD_ACK):
	def __init__(self, user_ip:str, data_port, cmd_port, imu_port):
		super(Handshake, self).__init__()		
		self.struct_pack_mode.insert(2, 'BBB')
		self.id = (0x00, 0x01)
		self.name = 'Handshake 网络握手确认指令'
		self.cmd_frame = ['<BBHBH','<H','<BB4BHHH','<I'] 
		self.lenght = 25
		params = [int(x) for x in user_ip.split('.')] + [data_port, cmd_port, imu_port]
		self._set_cmd_data(params)
		
class QueryDeviceInformation(BaseFrameCMD_ACK):
	def __init__(self):
		super(QueryDeviceInformation, self).__init__()		
		self.struct_pack_mode.insert(2, 'BBB4B')
		self.id = (0x00, 0x02)
		self.name = 'QueryDeviceInformation 设备信息查询'
		self.lenght = 15
		self.cmd_frame = ['<BBHBH','<H','<BB','<I']
		self._set_cmd_data(None)
		

以上展示的代码只是部分，目前完成了所有命令的解包与打包，但还有一些特定的功能没有实现，如采样数据包协议,标签信息,状态码。对于采样数据包我使用了numpy，利用numpy解析bytes速度可以大大提升。

while True:
            if not self.flag:
                break
            else:
                data, addr = self.socket_data_port.recvfrom(1500)
                databytes += data[18:]
                if len(databytes) == 1344*500:   
                    pointcloud = np.frombuffer(databytes, dtype=np.int8).reshape((-1,14))
                    pointcloud = (pointcloud[:, 0:12].reshape(-1)).tobytes()
                    pointcloud = np.frombuffer(pointcloud, dtype=np.int32).reshape((-1,3))
                    self.send_data.emit(pointcloud)
                    del databytes 
                    databytes = bytes()

该部分比较简陋，还没有时间重写，但可以作为一个思路帮助大家批量获取点云数据。

代码内容：获取到数据，将数据累加，由于我才用的为双回波直角坐标系，所以每100ms可以得到48000个点，每次收到的数据为48个，根据官网中定义的类型可以计算出48000个点需要1344*1000字节，由于我只需要24000个xyz，所以将标签与反射率丢掉了，1000也变成了500。（就是在这里发射了信号，传输了一帧点云24000个点）。

以下为视频演示

 

 

---