213 lines
7.4 KiB
Python
213 lines
7.4 KiB
Python
import ctypes
|
||
from ctypes import *
|
||
|
||
# 加载C++库
|
||
livox_sdk = ctypes.CDLL('./liblivox_sdk_wrapper.so')
|
||
|
||
# 预定义的常量
|
||
kBroadcastCodeSize = 16 # 广播码长度
|
||
# 定义枚举类型
|
||
class DeviceType(c_int):
|
||
kDeviceTypeHub = 0
|
||
kDeviceTypeLidarMid40 = 1
|
||
kDeviceTypeLidarTele = 2
|
||
kDeviceTypeLidarHorizon = 3
|
||
kDeviceTypeLidarMid70 = 6
|
||
kDeviceTypeLidarAvia = 7
|
||
|
||
class LidarState(c_int):
|
||
kLidarStateInit = 0
|
||
kLidarStateNormal = 1
|
||
kLidarStatePowerSaving = 2
|
||
kLidarStateStandBy = 3
|
||
kLidarStateError = 4
|
||
kLidarStateUnknown = 5
|
||
|
||
class LidarMode(c_int):
|
||
kLidarModeNormal = 1
|
||
kLidarModePowerSaving = 2
|
||
kLidarModeStandby = 3
|
||
|
||
class LidarFeature(c_int):
|
||
kLidarFeatureNone = 0
|
||
kLidarFeatureRainFog = 1
|
||
|
||
class LidarIpMode(c_int):
|
||
kLidarDynamicIpMode = 0
|
||
kLidarStaticIpMode = 1
|
||
|
||
class LidarScanPattern(c_int):
|
||
kNoneRepetitiveScanPattern = 0
|
||
kRepetitiveScanPattern = 1
|
||
|
||
class LivoxStatus(c_int):
|
||
kStatusSendFailed = -9
|
||
kStatusHandlerImplNotExist = -8
|
||
kStatusInvalidHandle = -7
|
||
kStatusChannelNotExist = -6
|
||
kStatusNotEnoughMemory = -5
|
||
kStatusTimeout = -4
|
||
kStatusNotSupported = -3
|
||
kStatusNotConnected = -2
|
||
kStatusFailure = -1
|
||
kStatusSuccess = 0
|
||
|
||
# 定义结构体和联合体
|
||
class LidarErrorCode(Structure):
|
||
_fields_ = [
|
||
("temp_status", c_uint32, 2),
|
||
("volt_status", c_uint32, 2),
|
||
("motor_status", c_uint32, 2),
|
||
("dirty_warn", c_uint32, 2),
|
||
("firmware_err", c_uint32, 1),
|
||
("pps_status", c_uint32, 1),
|
||
("device_status", c_uint32, 1),
|
||
("fan_status", c_uint32, 1),
|
||
("self_heating", c_uint32, 1),
|
||
("ptp_status", c_uint32, 1),
|
||
("time_sync_status", c_uint32, 3),
|
||
("rsvd", c_uint32, 13),
|
||
("system_status", c_uint32, 2),
|
||
]
|
||
|
||
class HubErrorCode(Structure):
|
||
_fields_ = [
|
||
("sync_status", c_uint32, 2),
|
||
("temp_status", c_uint32, 2),
|
||
("lidar_status", c_uint32, 1),
|
||
("lidar_link_status", c_uint32, 1),
|
||
("firmware_err", c_uint32, 1),
|
||
("rsvd", c_uint32, 23),
|
||
("system_status", c_uint32, 2),
|
||
]
|
||
|
||
class ErrorMessage(Union):
|
||
_fields_ = [
|
||
("error_code", c_uint32),
|
||
("lidar_error_code", LidarErrorCode),
|
||
("hub_error_code", HubErrorCode)
|
||
]
|
||
|
||
class StatusUnion(Union):
|
||
_fields_ = [
|
||
("error_code", c_uint32), # 假设这个字段为示例
|
||
("lidar_error_code", LidarErrorCode),
|
||
("hub_error_code", HubErrorCode)
|
||
]
|
||
|
||
# 定义 DeviceInfo 结构体
|
||
class DeviceInfo(Structure):
|
||
_fields_ = [
|
||
("broadcast_code", c_char * kBroadcastCodeSize), # 广播码，最大15个字符，带终止符
|
||
("handle", c_uint8), # 设备句柄
|
||
("slot", c_uint8), # 插槽编号
|
||
("id", c_uint8), # LiDAR id
|
||
("type", DeviceType), # 设备类型
|
||
("data_port", c_uint16), # 点云数据UDP端口
|
||
("cmd_port", c_uint16), # 控制命令UDP端口
|
||
("sensor_port", c_uint16), # IMU数据UDP端口
|
||
("ip", c_char * 16), # IP地址
|
||
("state", LidarState), # LiDAR状态
|
||
("feature", LidarFeature), # LiDAR特性
|
||
("status", StatusUnion), # LiDAR工作状态
|
||
("firmware_version", c_uint8 * 4) # 固件版本
|
||
]
|
||
|
||
# 定义 BroadcastDeviceInfo 结构体
|
||
class BroadcastDeviceInfo(Structure):
|
||
_fields_ = [
|
||
("broadcast_code", c_char * kBroadcastCodeSize), # 广播码，最多15个字符，带终止符
|
||
("dev_type", c_uint8), # 设备类型，参考 DeviceType
|
||
("reserved", c_uint16), # 保留字段
|
||
("ip", c_char * 16) # 设备 IP 地址
|
||
]
|
||
|
||
# 定义 LivoxEthPacket 结构体
|
||
class LivoxEthPacket(Structure):
|
||
_fields_ = [
|
||
("version", c_uint8), # Packet protocol version.
|
||
("slot", c_uint8), # Slot number used for connecting LiDAR.
|
||
("id", c_uint8), # LiDAR id.
|
||
("rsvd", c_uint8), # Reserved.
|
||
("err_code", c_uint32), # Device error status indicator information.
|
||
("timestamp_type", c_uint8), # Timestamp type.
|
||
("data_type", c_uint8), # Point cloud coordinate format.
|
||
("timestamp", c_uint8 * 8), # Nanosecond or UTC format timestamp.
|
||
("data", c_uint8 * 1) # Point cloud data (can be extended as needed).
|
||
]
|
||
|
||
# 定义 LivoxExtendRawPoint 结构体
|
||
class LivoxExtendRawPoint(Structure):
|
||
_fields_ = [
|
||
('x', c_int32), # X axis, Unit: mm
|
||
('y', c_int32), # Y axis, Unit: mm
|
||
('z', c_int32), # Z axis, Unit: mm
|
||
('reflectivity', c_uint8), # Reflectivity
|
||
('tag', c_uint8) # Tag
|
||
]
|
||
|
||
# 回调函数类型定义
|
||
DataCallbackType = CFUNCTYPE(None, c_uint8, POINTER(LivoxEthPacket), c_uint32)
|
||
ErrorCallbackType = CFUNCTYPE(None, c_uint8, c_uint8, POINTER(ErrorMessage))
|
||
DeviceChangeCallbackType = CFUNCTYPE(None, POINTER(DeviceInfo), c_uint8)
|
||
DeviceBroadcastCallbackType = CFUNCTYPE(None, POINTER(BroadcastDeviceInfo))
|
||
|
||
|
||
point_size = sizeof(LivoxExtendRawPoint)
|
||
|
||
# 数据回调函数
|
||
@DataCallbackType
|
||
def data_callback(handle, packet, data_num):
|
||
# print(f"Received data from handle {handle}, data num: {data_num}")
|
||
# 1. 计算数据的总字节数
|
||
total_data_size = data_num * point_size
|
||
# 2. 将 packet.data 转换为指向 LivoxExtendRawPoint 数组的指针
|
||
point_data_array_type = LivoxExtendRawPoint * data_num # 定义类型
|
||
point_data_pointer = cast(packet.data, POINTER(point_data_array_type)) # 将data转换为LivoxExtendRawPoint指针
|
||
# 3. 获取具体的点云数据
|
||
point_data_array = point_data_pointer.contents # 访问数组内容
|
||
# 4. 遍历每个 LivoxExtendRawPoint 并打印数据
|
||
for i in range(data_num):
|
||
point = point_data_array[i]
|
||
print(f"Point {i}: X={point.x}, Y={point.y}, Z={point.z}, Reflectivity={point.reflectivity}, Tag={point.tag}")
|
||
|
||
# 错误回调函数
|
||
@ErrorCallbackType
|
||
def error_callback(status, handle, error_message):
|
||
print(f"Error from handle {handle}, status: {status}")
|
||
|
||
# 设备状态变化回调函数
|
||
@DeviceChangeCallbackType
|
||
def device_change_callback(info, event_type):
|
||
print(f"Device {bytes(info.contents.broadcast_code).decode('utf-8')} changed state, event type: {event_type}")
|
||
|
||
# 设备广播回调函数
|
||
@DeviceBroadcastCallbackType
|
||
def device_broadcast_callback(info):
|
||
print(f"New device broadcast received: {bytes(info.contents.broadcast_code).decode('utf-8')}")
|
||
|
||
# 注册回调函数
|
||
livox_sdk.register_data_callback(data_callback)
|
||
livox_sdk.register_error_callback(error_callback)
|
||
livox_sdk.register_device_change_callback(device_change_callback)
|
||
livox_sdk.register_device_broadcast_callback(device_broadcast_callback)
|
||
|
||
# 调用初始化和开始发现设备的函数
|
||
if livox_sdk.init_sdk():
|
||
print("Livox SDK initialized.")
|
||
else:
|
||
print("Failed to initialize Livox SDK.")
|
||
|
||
if livox_sdk.start_discovery():
|
||
print("Started discovering Livox devices.")
|
||
else:
|
||
print("Failed to discover devices.")
|
||
|
||
# 等待数据回调
|
||
try:
|
||
while True:
|
||
pass
|
||
except KeyboardInterrupt:
|
||
livox_sdk.stop_sdk()
|
||
print("SDK stopped.")
|