480 lines
16 KiB
C++
480 lines
16 KiB
C++
#ifndef _WIN32
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <unistd.h>
|
||
#include <string.h>
|
||
#include "livox_sdk.h"
|
||
#include "open3d/Open3D.h"
|
||
|
||
|
||
using namespace open3d;
|
||
|
||
typedef enum {
|
||
kDeviceStateDisconnect = 0,
|
||
kDeviceStateConnect = 1,
|
||
kDeviceStateSampling = 2,
|
||
} DeviceState;
|
||
|
||
typedef struct {
|
||
uint8_t handle;
|
||
DeviceState device_state;
|
||
DeviceInfo info;
|
||
} DeviceItem;
|
||
|
||
DeviceItem devices[kMaxLidarCount];
|
||
int lidar_count = 0;
|
||
|
||
/** 回调函数声明 */
|
||
void OnLidarErrorStatusCallback(livox_status status, uint8_t handle, ErrorMessage *message);
|
||
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data);
|
||
void MyGetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data);
|
||
void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
|
||
void OnCommonCommandCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
|
||
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
|
||
void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type);
|
||
void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
|
||
|
||
/** 用于Python的回调函数指针 */
|
||
void (*python_data_callback)(uint8_t, uint32_t *, uint32_t) = NULL;
|
||
void (*python_error_callback)(livox_status, uint8_t, ErrorMessage*) = NULL;
|
||
void (*python_device_change_callback)(const DeviceInfo*, DeviceEvent) = NULL;
|
||
void (*python_device_broadcast_callback)(const BroadcastDeviceInfo*) = NULL;
|
||
void (*python_common_command_callback)(livox_status, uint8_t, uint8_t) = NULL;
|
||
|
||
extern "C" {
|
||
|
||
/** 初始化Livox SDK */
|
||
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;
|
||
}
|
||
|
||
/** 启动设备扫描 */
|
||
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并清理 */
|
||
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();
|
||
}
|
||
|
||
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, GetLidarData, NULL);
|
||
SetDataCallback(handle, MyGetLidarData, NULL);
|
||
|
||
}
|
||
return result;
|
||
}
|
||
|
||
int start_sampling(uint8_t handle) {
|
||
livox_status result = LidarStartSampling(handle, OnSampleCallback, NULL);
|
||
devices[handle].device_state = kDeviceStateSampling;
|
||
return result;
|
||
}
|
||
|
||
int stop_sampling(uint8_t handle) {
|
||
livox_status result = LidarStopSampling(handle, OnSampleCallback, NULL);
|
||
devices[handle].device_state = kDeviceStateSampling;
|
||
return result;
|
||
}
|
||
|
||
int set_mode(LidarMode mode) {
|
||
return LidarSetMode(devices[0].handle, mode, OnCommonCommandCallback, NULL);
|
||
}
|
||
|
||
/** 注册数据回调函数给Python调用 */
|
||
void register_data_callback(void (*data_callback)(uint8_t handle, uint32_t *data, uint32_t data_num)) {
|
||
python_data_callback = data_callback;
|
||
}
|
||
|
||
/** 注册错误回调函数给Python调用 */
|
||
void register_error_callback(void (*error_callback)(livox_status status, uint8_t handle, ErrorMessage *message)) {
|
||
python_error_callback = error_callback;
|
||
}
|
||
|
||
/** 注册设备状态变化回调给Python调用 */
|
||
void register_device_change_callback(void (*device_change_callback)(const DeviceInfo* info, DeviceEvent type)) {
|
||
python_device_change_callback = device_change_callback;
|
||
}
|
||
|
||
/** 注册设备广播回调给Python调用 */
|
||
void register_device_broadcast_callback(void (*device_broadcast_callback)(const BroadcastDeviceInfo *info)) {
|
||
python_device_broadcast_callback = device_broadcast_callback;
|
||
}
|
||
|
||
/** 注册设备广播回调给Python调用 */
|
||
void register_common_command_callback(void (*common_command_callback)(livox_status status, uint8_t handle, uint8_t response)) {
|
||
python_common_command_callback = common_command_callback;
|
||
}
|
||
|
||
}
|
||
|
||
/** 回调函数的定义 */
|
||
void OnLidarErrorStatusCallback(livox_status status, uint8_t handle, ErrorMessage *message) {
|
||
if (python_error_callback) {
|
||
python_error_callback(status, handle, message);
|
||
}
|
||
}
|
||
|
||
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);
|
||
}
|
||
}
|
||
|
||
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;
|
||
LivoxExtendRawPoint point_array[data_num];
|
||
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);
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
|
||
// 停止采样时的回调处理
|
||
}
|
||
|
||
/** Query the firmware version of Livox LiDAR. */
|
||
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]);
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
void LidarDisConnect(const DeviceInfo *info) {
|
||
uint8_t handle = info->handle;
|
||
devices[handle].device_state = kDeviceStateDisconnect;
|
||
}
|
||
|
||
void LidarStateChange(const DeviceInfo *info) {
|
||
uint8_t handle = info->handle;
|
||
devices[handle].info = *info;
|
||
}
|
||
|
||
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);
|
||
}
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
static void delay_second(double second)
|
||
{
|
||
clock_t start_time;
|
||
start_time = clock();
|
||
for (; (clock() - start_time) < second * CLOCKS_PER_SEC;);
|
||
}
|
||
|
||
#ifndef _WIN32
|
||
static bool bCouldIsRecv = false;
|
||
static bool bCouldIsGet = false;
|
||
static uint32_t need_points_size = 200 * 10000;
|
||
static float points_data[240 * 10000 * 4];
|
||
//static uint32_t cur_point_num = 0;
|
||
static std::atomic<uint32_t> cur_point_num(0);
|
||
void MyGetLidarData(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));
|
||
if (!bCouldIsRecv)
|
||
{
|
||
uint32_t _cur_point_num = cur_point_num++;
|
||
//cur_point_num += data_num;
|
||
_cur_point_num = _cur_point_num * data_num;
|
||
// need_points_size = G(cloud_need_points_size);
|
||
uint32_t _offset = _cur_point_num * 4;
|
||
|
||
for (uint32_t i = 0; i < data_num; ++i)
|
||
{
|
||
_offset = _offset + i * 4;
|
||
points_data[_offset] = p_point_data[i].x / 1000.0;
|
||
points_data[_offset + 1] = p_point_data[i].y / 1000.0;
|
||
points_data[_offset + 2] = p_point_data[i].z / 1000.0;
|
||
points_data[_offset + 3] = (uint32_t)(p_point_data[i].reflectivity << 8 | p_point_data[i].tag);
|
||
}
|
||
|
||
uint32_t _cur_count = _cur_point_num + data_num;
|
||
if ((_cur_count % 100000) <= 100)
|
||
std::cout << _cur_count << " need data size : " << need_points_size << std::endl;
|
||
if (_cur_count < need_points_size)
|
||
return;
|
||
else
|
||
bCouldIsRecv = true;
|
||
}
|
||
else
|
||
return;
|
||
|
||
// 等待点云拷贝结束
|
||
//__p_point_data = p_point_data;
|
||
//__data_num = data_num;
|
||
while (true)
|
||
{
|
||
if (!bCouldIsGet)
|
||
{
|
||
delay_second(0.01);
|
||
continue;
|
||
}
|
||
cur_point_num = 0;
|
||
uint32_t* p = (uint32_t*)points_data;
|
||
memset(p , 0, need_points_size * 4);
|
||
bCouldIsGet = false;
|
||
break;
|
||
}
|
||
}
|
||
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);
|
||
}
|
||
}
|
||
#else
|
||
/** Extend cartesian coordinate format. */
|
||
typedef struct {
|
||
int32_t x; /**< X axis, Unit:mm */
|
||
int32_t y; /**< Y axis, Unit:mm */
|
||
int32_t z; /**< Z axis, Unit:mm */
|
||
uint8_t reflectivity; /**< Reflectivity */
|
||
uint8_t tag; /**< Tag */
|
||
} LivoxExtendRawPoint;
|
||
typedef struct {
|
||
uint8_t version; /**< Packet protocol version. */
|
||
uint8_t slot; /**< Slot number used for connecting LiDAR. */
|
||
uint8_t id; /**< LiDAR id. */
|
||
uint8_t rsvd; /**< Reserved. */
|
||
uint32_t err_code; /**< Device error status indicator information. */
|
||
uint8_t timestamp_type; /**< Timestamp type. */
|
||
/** Point cloud coordinate format, refer to \ref PointDataType . */
|
||
uint8_t data_type;
|
||
uint8_t timestamp[8]; /**< Nanosecond or UTC format timestamp. */
|
||
uint8_t data[1]; /**< Point cloud data. */
|
||
} LivoxEthPacket;
|
||
static bool bCouldIsRecv = false;
|
||
static bool bCouldIsGet = false;
|
||
static uint32_t need_points_size = 200 * 10000;
|
||
static float points_data[240 * 10000 * 4];
|
||
//static uint32_t cur_data_num = 0;
|
||
static std::atomic<uint32_t> cur_point_num(0);
|
||
typedef void (*FuncGetLidarData)(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data);
|
||
FuncGetLidarData GetLidarData;
|
||
#endif // !_WIN32
|
||
|
||
int main()
|
||
{
|
||
int ret = 0;
|
||
bool succ = init_sdk();
|
||
if (!succ)
|
||
{
|
||
printf("start_discovery init_sdk is %d \n", -1);
|
||
return -1;
|
||
}
|
||
|
||
succ = start_discovery();
|
||
if (!succ)
|
||
{
|
||
printf("start_discovery error is %d \n", -1);
|
||
return -1;
|
||
}
|
||
|
||
delay_second(2);
|
||
|
||
ret = start_sampling(0);
|
||
if (ret != 0)
|
||
{
|
||
printf("start_sampling error is %d \n", ret);
|
||
return -1;
|
||
}
|
||
|
||
|
||
// 写入点云对象
|
||
geometry::PointCloud* pCloud = new geometry::PointCloud();
|
||
|
||
while (true)
|
||
{
|
||
if (!bCouldIsRecv)
|
||
{
|
||
delay_second(0.05);
|
||
continue;
|
||
}
|
||
delay_second(0.01);
|
||
break;
|
||
}
|
||
pCloud->points_.resize(need_points_size);
|
||
for (uint32_t i = 0; i < need_points_size; ++i) {
|
||
pCloud->points_[i][0] = points_data[i * 4];
|
||
pCloud->points_[i][1] = points_data[i * 4 + 1];
|
||
pCloud->points_[i][2] = points_data[i * 4 + 2];
|
||
//pCloud->points_.push_back({ (float)point[0], (float)point[1], (float)point[2] });
|
||
}
|
||
|
||
io::WritePointCloud("output.ply", *pCloud);
|
||
|
||
bCouldIsGet = true;
|
||
delay_second(0.01);
|
||
bCouldIsRecv = false; //开始接收数据
|
||
|
||
|
||
ret = stop_sampling(0);
|
||
if (ret != 0)
|
||
{
|
||
printf("stop_sampling error is %d \n", ret);
|
||
return ret;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
#endif |