342 lines
11 KiB
C++
342 lines
11 KiB
C++
|
#ifndef _WIN32
|
|||
|
|
#include <stdio.h>
|
|||
|
|
#include <stdlib.h>
|
|||
|
|
#ifndef _WIN32
|
|||
|
|
#include <unistd.h>
|
|||
|
|
#endif
|
|||
|
|
#include <string.h>
|
|||
|
|
|
|||
|
|
#include "LidarHelper.h"
|
|||
|
|
|
|||
|
|
CLidarHelper* CLidarHelper::m_instance;
|
|||
|
|
extern "C" {
|
|||
|
|
|
|||
|
|
/** 回调函数声明 */
|
|||
|
|
static void OnLidarErrorStatusCallback(livox_status status, uint8_t handle, ErrorMessage* message);
|
|||
|
|
static void GetLidarData(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data);
|
|||
|
|
static void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void* data);
|
|||
|
|
static void OnCommonCommandCallback(livox_status status, uint8_t handle, uint8_t response, void* data);
|
|||
|
|
static void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void* data);
|
|||
|
|
static void OnDeviceInfoChange(const DeviceInfo* info, DeviceEvent type);
|
|||
|
|
static void OnDeviceBroadcast(const BroadcastDeviceInfo* info);
|
|||
|
|
|
|||
|
|
/** 用于Python的回调函数指针 */
|
|||
|
|
//static void (*python_data_callback)(uint8_t, uint32_t*, uint32_t) = NULL;
|
|||
|
|
//static void (*python_error_callback)(livox_status, uint8_t, ErrorMessage*) = NULL;
|
|||
|
|
//static void (*python_device_change_callback)(const DeviceInfo*, DeviceEvent) = NULL;
|
|||
|
|
//static void (*python_device_broadcast_callback)(const BroadcastDeviceInfo*) = NULL;
|
|||
|
|
//static void (*python_common_command_callback)(livox_status, uint8_t, uint8_t) = NULL;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
typedef enum {
|
|||
|
|
kDeviceStateDisconnect = 0,
|
|||
|
|
kDeviceStateConnect = 1,
|
|||
|
|
kDeviceStateSampling = 2,
|
|||
|
|
} DeviceState;
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
uint8_t handle;
|
|||
|
|
DeviceState device_state;
|
|||
|
|
DeviceInfo info;
|
|||
|
|
} DeviceItem;
|
|||
|
|
|
|||
|
|
static DeviceItem devices[kMaxLidarCount];
|
|||
|
|
static int lidar_count = 0;
|
|||
|
|
static FuncGetLidarData pfuncGetLidarData = nullptr;
|
|||
|
|
|
|||
|
|
static bool init_sdk() {
|
|||
|
|
printf("Livox SDK initializing.\n");
|
|||
|
|
if (!Init()) {
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
printf("Livox SDK has been initialized.\n");
|
|||
|
|
LivoxSdkVersion _sdkversion;
|
|||
|
|
GetLivoxSdkVersion(&_sdkversion);
|
|||
|
|
printf("Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
|
|||
|
|
|
|||
|
|
memset(devices, 0, sizeof(devices));
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/** 启动设备扫描 */
|
|||
|
|
static bool start_discovery() {
|
|||
|
|
SetBroadcastCallback(OnDeviceBroadcast);
|
|||
|
|
SetDeviceStateUpdateCallback(OnDeviceInfoChange);
|
|||
|
|
|
|||
|
|
if (!Start()) {
|
|||
|
|
printf("Failed to start device discovery.\n");
|
|||
|
|
Uninit();
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
printf("Started device discovery.\n");
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/** 停止SDK并清理 */
|
|||
|
|
static void stop_sdk() {
|
|||
|
|
printf("Stopping Livox SDK.\n");
|
|||
|
|
for (int i = 0; i < kMaxLidarCount; ++i) {
|
|||
|
|
if (devices[i].device_state == kDeviceStateSampling) {
|
|||
|
|
LidarStopSampling(devices[i].handle, OnStopSampleCallback, NULL);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
Uninit();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static int connect(const char* broadcast_code) {
|
|||
|
|
uint8_t handle = 0;
|
|||
|
|
livox_status result = AddLidarToConnect(broadcast_code, &handle);
|
|||
|
|
if (result == kStatusSuccess) {
|
|||
|
|
/** Set the point cloud data for a specific Livox LiDAR. */
|
|||
|
|
SetDataCallback(handle, pfuncGetLidarData, NULL);
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static int start_sampling(uint8_t handle) {
|
|||
|
|
livox_status result = LidarStartSampling(handle, OnSampleCallback, NULL);
|
|||
|
|
devices[handle].device_state = kDeviceStateSampling;
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static int stop_sampling(uint8_t handle) {
|
|||
|
|
livox_status result = LidarStopSampling(handle, OnSampleCallback, NULL);
|
|||
|
|
devices[handle].device_state = kDeviceStateSampling;
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static int set_mode(LidarMode mode) {
|
|||
|
|
return LidarSetMode(devices[0].handle, mode, OnCommonCommandCallback, NULL);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/** 回调函数的定义 */
|
|||
|
|
static void OnLidarErrorStatusCallback(livox_status status, uint8_t handle, ErrorMessage* message) {
|
|||
|
|
//if (python_error_callback) {
|
|||
|
|
// python_error_callback(status, handle, message);
|
|||
|
|
//}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void OnCommonCommandCallback(livox_status status, uint8_t handle, uint8_t response, void* data) {
|
|||
|
|
//if (python_common_command_callback) {
|
|||
|
|
// python_common_command_callback(status, handle, response);
|
|||
|
|
//}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void GetLidarData(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data) {
|
|||
|
|
if (data) {
|
|||
|
|
/** Parsing the timestamp and the point cloud data. */
|
|||
|
|
uint64_t cur_timestamp = *((uint64_t*)(data->timestamp));
|
|||
|
|
if (data->data_type == kCartesian) {
|
|||
|
|
LivoxRawPoint* p_point_data = (LivoxRawPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kSpherical) {
|
|||
|
|
LivoxSpherPoint* p_point_data = (LivoxSpherPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kExtendCartesian) {
|
|||
|
|
LivoxExtendRawPoint* p_point_data = (LivoxExtendRawPoint*)data->data;
|
|||
|
|
#ifdef _WIN32
|
|||
|
|
LivoxExtendRawPoint point_array[10*10000];
|
|||
|
|
#else
|
|||
|
|
LivoxExtendRawPoint point_array[data_num];
|
|||
|
|
#endif
|
|||
|
|
uint32_t* converted_data = (uint32_t*)malloc(data_num * 4 * sizeof(uint32_t));
|
|||
|
|
for (uint32_t i = 0; i < data_num; ++i) {
|
|||
|
|
LivoxExtendRawPoint* point = &p_point_data[i];
|
|||
|
|
// 将结构体的 x, y, z 放入 uint32_t 数组中
|
|||
|
|
converted_data[i * 4] = point->x;
|
|||
|
|
converted_data[i * 4 + 1] = point->y;
|
|||
|
|
converted_data[i * 4 + 2] = point->z;
|
|||
|
|
// 将 reflectivity 和 tag 合并成一个 uint32_t（高16位和低16位）
|
|||
|
|
converted_data[i * 4 + 3] = (uint32_t)(point->reflectivity << 8 | point->tag);
|
|||
|
|
}
|
|||
|
|
//// 调用 Python 回调函数
|
|||
|
|
//if (python_data_callback) {
|
|||
|
|
// python_data_callback(handle, converted_data, data_num);
|
|||
|
|
//}
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kExtendSpherical) {
|
|||
|
|
LivoxExtendSpherPoint* p_point_data = (LivoxExtendSpherPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kDualExtendCartesian) {
|
|||
|
|
LivoxDualExtendRawPoint* p_point_data = (LivoxDualExtendRawPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kDualExtendSpherical) {
|
|||
|
|
LivoxDualExtendSpherPoint* p_point_data = (LivoxDualExtendSpherPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kImu) {
|
|||
|
|
LivoxImuPoint* p_point_data = (LivoxImuPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kTripleExtendCartesian) {
|
|||
|
|
LivoxTripleExtendRawPoint* p_point_data = (LivoxTripleExtendRawPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
else if (data->data_type == kTripleExtendSpherical) {
|
|||
|
|
LivoxTripleExtendSpherPoint* p_point_data = (LivoxTripleExtendSpherPoint*)data->data;
|
|||
|
|
}
|
|||
|
|
// printf("data_type %d\n", data->data_type);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void* data) {
|
|||
|
|
if (status == kStatusSuccess && response != 0) {
|
|||
|
|
devices[handle].device_state = kDeviceStateConnect;
|
|||
|
|
}
|
|||
|
|
else if (status == kStatusTimeout) {
|
|||
|
|
devices[handle].device_state = kDeviceStateConnect;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void* data) {
|
|||
|
|
// 停止采样时的回调处理
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/** Query the firmware version of Livox LiDAR. */
|
|||
|
|
static void OnDeviceInformation(livox_status status, uint8_t handle, DeviceInformationResponse* ack, void* data) {
|
|||
|
|
if (status != kStatusSuccess) {
|
|||
|
|
printf("Device Query Informations Failed %d\n", status);
|
|||
|
|
}
|
|||
|
|
if (ack) {
|
|||
|
|
printf("firm ver: %d.%d.%d.%d\n",
|
|||
|
|
ack->firmware_version[0],
|
|||
|
|
ack->firmware_version[1],
|
|||
|
|
ack->firmware_version[2],
|
|||
|
|
ack->firmware_version[3]);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void LidarConnect(const DeviceInfo* info) {
|
|||
|
|
uint8_t handle = info->handle;
|
|||
|
|
QueryDeviceInformation(handle, OnDeviceInformation, NULL);
|
|||
|
|
if (devices[handle].device_state == kDeviceStateDisconnect) {
|
|||
|
|
devices[handle].device_state = kDeviceStateConnect;
|
|||
|
|
devices[handle].info = *info;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void LidarDisConnect(const DeviceInfo* info) {
|
|||
|
|
uint8_t handle = info->handle;
|
|||
|
|
devices[handle].device_state = kDeviceStateDisconnect;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void LidarStateChange(const DeviceInfo* info) {
|
|||
|
|
uint8_t handle = info->handle;
|
|||
|
|
devices[handle].info = *info;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void OnDeviceInfoChange(const DeviceInfo* info, DeviceEvent type) {
|
|||
|
|
if (info == NULL) {
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
uint8_t handle = info->handle;
|
|||
|
|
if (handle >= kMaxLidarCount) {
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (type == kEventConnect) {
|
|||
|
|
LidarConnect(info);
|
|||
|
|
printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
|
|||
|
|
}
|
|||
|
|
else if (type == kEventDisconnect) {
|
|||
|
|
LidarDisConnect(info);
|
|||
|
|
printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
|
|||
|
|
}
|
|||
|
|
else if (type == kEventStateChange) {
|
|||
|
|
LidarStateChange(info);
|
|||
|
|
printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
|
|||
|
|
}
|
|||
|
|
printf("Device Working State %d\n", devices[handle].info.state);
|
|||
|
|
if (devices[handle].device_state == kDeviceStateConnect) {
|
|||
|
|
if (devices[handle].info.state == kLidarStateInit) {
|
|||
|
|
printf("Device State Change Progress %u\n", devices[handle].info.status.progress);
|
|||
|
|
}
|
|||
|
|
else {
|
|||
|
|
printf("Device State Error Code 0X%08x\n", devices[handle].info.status.status_code.error_code);
|
|||
|
|
}
|
|||
|
|
printf("Device feature %d\n", devices[handle].info.feature);
|
|||
|
|
SetErrorMessageCallback(handle, OnLidarErrorStatusCallback);
|
|||
|
|
if (devices[handle].info.state == kLidarStateNormal) {
|
|||
|
|
LidarStartSampling(handle, OnSampleCallback, NULL);
|
|||
|
|
devices[handle].device_state = kDeviceStateSampling;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//if (python_device_change_callback) {
|
|||
|
|
// python_device_change_callback(info, type);
|
|||
|
|
//}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void OnDeviceBroadcast(const BroadcastDeviceInfo* info) {
|
|||
|
|
//if (python_device_broadcast_callback) {
|
|||
|
|
// python_device_broadcast_callback(info);
|
|||
|
|
//}
|
|||
|
|
printf("connect to device %d \n", info->broadcast_code);
|
|||
|
|
connect(info->broadcast_code);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
CLidarHelper* CLidarHelper::Get()
|
|||
|
|
{
|
|||
|
|
if (CLidarHelper::m_instance == nullptr) {
|
|||
|
|
CLidarHelper::m_instance = new CLidarHelper();
|
|||
|
|
// 待优化
|
|||
|
|
}
|
|||
|
|
return CLidarHelper::m_instance;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
CLidarHelper::CLidarHelper()
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
CLidarHelper::~CLidarHelper()
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int CLidarHelper::Init(void* pfunGetdate)
|
|||
|
|
{
|
|||
|
|
pfuncGetLidarData = (FuncGetLidarData)pfunGetdate;
|
|||
|
|
|
|||
|
|
bool succ = init_sdk();
|
|||
|
|
if (!succ)
|
|||
|
|
return LIDAR_INIT_SDK_FAILED;
|
|||
|
|
succ = start_discovery();
|
|||
|
|
if (!succ)
|
|||
|
|
return LIDAR_START_DISCOVERY_FAILD;
|
|||
|
|
|
|||
|
|
return LIDAR_OK;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int CLidarHelper::Start(int epx_time)
|
|||
|
|
{
|
|||
|
|
int ret = start_sampling(0);
|
|||
|
|
if (ret != 0)
|
|||
|
|
{
|
|||
|
|
printf("start_sampling error is %d \n", ret);
|
|||
|
|
return LIDAR_START_SAMPLING_FAILD;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return LIDAR_OK;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int CLidarHelper::Stop()
|
|||
|
|
{
|
|||
|
|
int ret = stop_sampling(0);
|
|||
|
|
if (ret != 0)
|
|||
|
|
{
|
|||
|
|
printf("stop_sampling error is %d \n", ret);
|
|||
|
|
return LIDAR_STOP_SAMPLING_FAILD;
|
|||
|
|
}
|
|||
|
|
return LIDAR_OK;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int CLidarHelper::Cap()
|
|||
|
|
{
|
|||
|
|
return LIDAR_OK;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} // extern "C"
|
|||
|
|
#else
|
|||
|
|
#include "LidarHelper.h"
|
|||
|
|
CLidarHelper* CLidarHelper::m_instance;
|
|||
|
|
#endif //_WIN32
|
|||
|
|
|