107 lines
3.6 KiB
Python
107 lines
3.6 KiB
Python
|
import ctypes
|
||
|
|
from ctypes import c_char_p, c_void_p, c_uint32, c_uint64, c_float, c_uint8, POINTER
|
||
|
|
|
||
|
|
# Ajusta aqui pro nome REAL da DLL gerada no seu build
|
||
|
|
_DLL_PATH = r"C:\\ZendionInc\\agrobot_base\\Python\\livox\\livox_lidar_sdk_shared.dll"
|
||
|
|
|
||
|
|
# Carrega a DLL
|
||
|
|
_lvx = ctypes.WinDLL(_DLL_PATH)
|
||
|
|
|
||
|
|
# -------------------------------------------------------------------
|
||
|
|
# Definições de estruturas que vêm da SDK
|
||
|
|
# Isso baseia nos headers livox_lidar_api.h / livox_lidar_def.h
|
||
|
|
# -------------------------------------------------------------------
|
||
|
|
|
||
|
|
class LivoxPoint(ctypes.Structure):
|
||
|
|
_fields_ = [
|
||
|
|
("x", c_float),
|
||
|
|
("y", c_float),
|
||
|
|
("z", c_float),
|
||
|
|
("reflectivity", c_uint8),
|
||
|
|
("tag", c_uint8),
|
||
|
|
("timestamp", c_uint32),
|
||
|
|
]
|
||
|
|
|
||
|
|
# callback C:
|
||
|
|
# void OnPointCloud(const uint32_t handle,
|
||
|
|
# const LivoxPoint* data,
|
||
|
|
# uint32_t data_num,
|
||
|
|
# uint64_t timestamp,
|
||
|
|
# void* client_data);
|
||
|
|
PointCloudCallbackType = ctypes.CFUNCTYPE(
|
||
|
|
None,
|
||
|
|
c_uint32, # handle do lidar
|
||
|
|
POINTER(LivoxPoint), # array de pontos
|
||
|
|
c_uint32, # quantidade de pontos
|
||
|
|
c_uint64, # timestamp do pacote
|
||
|
|
c_void_p # client_data (userdata)
|
||
|
|
)
|
||
|
|
|
||
|
|
# -------------------------------------------------------------------
|
||
|
|
# Mapeia as funções principais da DLL
|
||
|
|
# (nomes podem mudar um pouquinho dependendo da versão)
|
||
|
|
# -------------------------------------------------------------------
|
||
|
|
|
||
|
|
# int LivoxLidarSdkInit(const char* config_path);
|
||
|
|
_lvx.LivoxLidarSdkInit.argtypes = [c_char_p]
|
||
|
|
_lvx.LivoxLidarSdkInit.restype = ctypes.c_int
|
||
|
|
|
||
|
|
# void LivoxLidarSdkUninit(void);
|
||
|
|
_lvx.LivoxLidarSdkUninit.argtypes = []
|
||
|
|
_lvx.LivoxLidarSdkUninit.restype = None
|
||
|
|
|
||
|
|
# int LivoxLidarStart(void);
|
||
|
|
# (em algumas versões é LivoxLidarStartSampling / LivoxLidarStartCallBack etc)
|
||
|
|
try:
|
||
|
|
_lvx.LivoxLidarStart.argtypes = []
|
||
|
|
_lvx.LivoxLidarStart.restype = ctypes.c_int
|
||
|
|
_HAS_START = True
|
||
|
|
except AttributeError:
|
||
|
|
_HAS_START = False
|
||
|
|
|
||
|
|
# int LivoxLidarStop(void);
|
||
|
|
try:
|
||
|
|
_lvx.LivoxLidarStop.argtypes = []
|
||
|
|
_lvx.LivoxLidarStop.restype = ctypes.c_int
|
||
|
|
_HAS_STOP = True
|
||
|
|
except AttributeError:
|
||
|
|
_HAS_STOP = False
|
||
|
|
|
||
|
|
# int SetLivoxLidarPointCloudCallBack(PointCloudCallbackType cb, void* client_data);
|
||
|
|
_lvx.SetLivoxLidarPointCloudCallBack.argtypes = [PointCloudCallbackType, c_void_p]
|
||
|
|
_lvx.SetLivoxLidarPointCloudCallBack.restype = ctypes.c_int
|
||
|
|
|
||
|
|
# -------------------------------------------------------------------
|
||
|
|
# Funções Python que você vai chamar do seu script
|
||
|
|
# -------------------------------------------------------------------
|
||
|
|
|
||
|
|
def init(config_path: str) -> bool:
|
||
|
|
"""Inicializa o SDK passando o caminho do config.json"""
|
||
|
|
res = _lvx.LivoxLidarSdkInit(config_path.encode("utf-8"))
|
||
|
|
return res == 0 # normalmente 0 == sucesso
|
||
|
|
|
||
|
|
def uninit():
|
||
|
|
"""Finaliza SDK"""
|
||
|
|
_lvx.LivoxLidarSdkUninit()
|
||
|
|
|
||
|
|
def set_point_cloud_callback(cb_func):
|
||
|
|
"""Registra callback de nuvem de pontos."""
|
||
|
|
# guarda referência global pra callback não ser coletada
|
||
|
|
global _POINT_CB_REF
|
||
|
|
_POINT_CB_REF = PointCloudCallbackType(cb_func)
|
||
|
|
# passa null pro client_data por enquanto
|
||
|
|
ret = _lvx.SetLivoxLidarPointCloudCallBack(_POINT_CB_REF, None)
|
||
|
|
return ret == 0
|
||
|
|
|
||
|
|
def start():
|
||
|
|
"""Manda começar a stream de dados, se a DLL expuser LivoxLidarStart."""
|
||
|
|
if _HAS_START:
|
||
|
|
return _lvx.LivoxLidarStart() == 0
|
||
|
|
return True # se não existir, assume que já começou automático
|
||
|
|
|
||
|
|
def stop():
|
||
|
|
"""Para stream."""
|
||
|
|
if _HAS_STOP:
|
||
|
|
return _lvx.LivoxLidarStop() == 0
|
||
|
|
return True
|