123 lines
4.5 KiB
Python
123 lines
4.5 KiB
Python
import time
|
|
import ctypes
|
|
from datetime import datetime
|
|
import livox_sdk
|
|
|
|
# ---------------------------------------------------------
|
|
# Ajuste isso pros seus valores conhecidos
|
|
# ---------------------------------------------------------
|
|
LIDAR_BROADCAST_CODE = None # se você quiser forçar um SN específico
|
|
HOST_IP = "192.168.1.100" # IP do PC (Points IP no viewer)
|
|
LIDAR_IP = "192.168.1.169" # IP do LiDAR
|
|
POINT_PORT = 56301 # Point Cloud Port
|
|
CMD_PORT = 56100 # Porta de comando (já vimos no log)
|
|
IMU_PORT = 56401 # IMU Port
|
|
INFO_PORT = 56201 # Lidar Info Port
|
|
# ---------------------------------------------------------
|
|
|
|
# A estrutura de ponto que vem do Livox SDK2 normalmente é algo tipo:
|
|
# typedef struct {
|
|
# uint32_t timestamp;
|
|
# float x;
|
|
# float y;
|
|
# float z;
|
|
# uint8_t reflectivity;
|
|
# uint8_t tag;
|
|
# } LivoxPoint;
|
|
#
|
|
# O wrapper Python já converte isso pra você ou expõe um ponteiro pra array.
|
|
# Vamos supor que ele chama nosso callback com (data_ptr, data_num, timestamp).
|
|
|
|
# Callback chamado quando chegam pontos do LiDAR
|
|
@ctypes.CFUNCTYPE(None, ctypes.c_void_p, ctypes.c_uint32, ctypes.c_uint64)
|
|
def point_cloud_callback(data_ptr, data_num, packet_ts):
|
|
# data_ptr = ponteiro pra array de pontos (LivoxPoint)
|
|
# data_num = quantos pontos vieram nesse pacote
|
|
# packet_ts = timestamp do pacote em nanossegundos ou microssegundos (depende da build)
|
|
|
|
now_local = datetime.now().strftime("%H:%M:%S.%f")[:-3]
|
|
|
|
# Só log leve pra testar leitura:
|
|
print(f"[{now_local}] pacote recebido: {data_num} pontos | ts_sensor={packet_ts}")
|
|
|
|
# Se você quiser inspecionar o primeiro ponto:
|
|
# cria uma view python em cima da struct
|
|
# Exemplo genérico, pode precisar adaptar:
|
|
# first_point = livox_sdk.get_point(data_ptr, 0)
|
|
# print(" primeiro ponto:", first_point.x, first_point.y, first_point.z, first_point.reflectivity)
|
|
|
|
def main():
|
|
print("=== Mid-360 Reader - Inicializando SDK Livox ===")
|
|
|
|
# 1) Inicializa o core do SDK
|
|
ok = livox_sdk.init()
|
|
if not ok:
|
|
print("Falha ao inicializar Livox SDK")
|
|
return
|
|
|
|
# 2) Configura os canais de rede (host_ip, lidar_ip, portas)
|
|
# Nem todo wrapper expõe isso direto; alguns assumem discovery automático.
|
|
# Então a gente tenta primeiro discovery normal e depois bind manual.
|
|
#
|
|
# Se seu livox_sdk.py tiver um método tipo 'set_network_params' / 'setup_lidar',
|
|
# você chama aqui. Exemplo (ajuste pro que existir no seu arquivo):
|
|
try:
|
|
livox_sdk.set_network_params(
|
|
host_ip=HOST_IP,
|
|
lidar_ip=LIDAR_IP,
|
|
cmd_port=CMD_PORT,
|
|
data_port=POINT_PORT,
|
|
imu_port=IMU_PORT,
|
|
info_port=INFO_PORT
|
|
)
|
|
except AttributeError:
|
|
# Wrapper sem essa call explícita -> beleza, seguimos com discovery
|
|
pass
|
|
|
|
# 3) Registrar callback de ponto
|
|
# A API costuma ter algo como 'register_point_cloud_callback'
|
|
if hasattr(livox_sdk, "register_point_cloud_callback"):
|
|
livox_sdk.register_point_cloud_callback(point_cloud_callback)
|
|
else:
|
|
print("⚠ Seu livox_sdk.py não tem register_point_cloud_callback; ajustar nomes depois.")
|
|
print(" Procura por algo tipo 'SetPointCloudCallback' dentro do arquivo livox_sdk.py")
|
|
# a partir daqui ele pode não receber pontos, mas vamos continuar pra ver se inicializa ok
|
|
|
|
# 4) Conectar ao LiDAR
|
|
# Há dois jeitos comuns nos samples:
|
|
# - via broadcast code (número de série tipo 47MDN7K0030169)
|
|
# - via IP direto
|
|
#
|
|
# Você tem SN: 47MDN7K0030169 no seu log.
|
|
# Se o wrapper expõe add_lidar(), usa:
|
|
connected = False
|
|
if hasattr(livox_sdk, "add_lidar"):
|
|
connected = livox_sdk.add_lidar("47MDN7K0030169", LIDAR_IP)
|
|
elif hasattr(livox_sdk, "connect_lidar"):
|
|
connected = livox_sdk.connect_lidar(LIDAR_IP, CMD_PORT)
|
|
else:
|
|
# fallback "vamos ver se já está conectado só por init()"
|
|
connected = True
|
|
|
|
if not connected:
|
|
print("Não conseguiu conectar no LiDAR pelo wrapper. Checar função correta de conexão.")
|
|
return
|
|
|
|
print("✅ Conectado. Lendo pacotes... CTRL+C pra sair.\n")
|
|
|
|
try:
|
|
while True:
|
|
# alguns wrappers exigem uma call de 'poll()' pra processar eventos C -> Python.
|
|
if hasattr(livox_sdk, "poll"):
|
|
livox_sdk.poll()
|
|
|
|
time.sleep(0.05)
|
|
except KeyboardInterrupt:
|
|
print("Encerrando leitura...")
|
|
finally:
|
|
livox_sdk.uninit()
|
|
print("SDK encerrado. Bye.")
|
|
|
|
if __name__ == "__main__":
|
|
main()
|