3d激光雷达开发（lidar使用）

安装好了pcl库之后，下面就是需要想办法采集真实的雷达数据了。和camera不同，3d雷达一般需要单独购买。这方面，camera就强太多了，不仅手机有camera，电脑有camera，就算单独购买，也只要几百块钱。3d雷达则不同，最便宜的3d雷达也要3-4千块，多的可能大几万，这方面大家量力而行。有条件，可以自己购买；没有条件，可以申请单位购买；再不济，那就只能去看一下有没有别人不用的二手lidar了。

1、lidar品牌的选择

目前这方面，国内也是百花齐放，百家争鸣。不过，还是建议大家购买那些有一定知名度，自己也能负担得起的传感器雷达。对我们自己来说，就是购买了livox品牌的雷达。不为别的，主要是价格负担得起。

2、lidar的选择

目前来说，lidar的选择，还是来自于自己的需求。好的lidar视场角宽，看得远，但是价格也贵；便宜的lidar视场角一般，价格适中，比如说下面的mid40，视场角为38.4 * 38.4左右，

而mid70，视场角达到了70.4*70.4左右，

3、使用view工具显示雷达数据，

等从官方途径购买到了3d雷达之后，接着就是下载view工具，查看图像即可。

view工具下载地址如下所示，

https://www.livoxtech.com/cn/downloads

打开后，链接是这样的，

软件本身是绿色软件，只要把lidar和pc用网线直连在一起，网络配置成192.168.1.*即可。因为lidar默认就是这个网段的ip，pc配置成相同的一个网段后，就可以收到lidar发出来的广播包数据了。忘记说了，livox的雷达需要一个外接12v或者24v的电源，这一块需要准备下。

4、准备livox sdk

通过view软件确认可以，就可以准备sdk读取lidar数据了。这部分github上面有下载，地址如下，

https://github.com/Livox-SDK/Livox-SDK

可以把代码下载下来。

5、编译livox sdk

和pcl的编译差不多，livox sdk也是依赖于cmake才能完成的。

6、进一步编译livx_sdk.sln

打开sln工程后，可以看到很多的项目，这里面除了livox_sdk_static是静态库之外，其他都是exe程序。

编译没有问题的话，就会看到，所有的程序都已经编译成功，

7、雷达数据读取

所有sdk的示例代码中，可以重点关注lidar_sample、lidar_sample_cc这两个项目。前者是c语言，后者是cpp语言，就是这点区别，其他部分基本都是一样的。这里以lidar_sample举例，看一下官方提供的示例代码是这样的，

//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//#include <stdio.h>
#include <stdlib.h>
#ifdef WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif
#include <string.h>
#include "livox_sdk.h"typedef enum {kDeviceStateDisconnect = 0,kDeviceStateConnect = 1,kDeviceStateSampling = 2,
} DeviceState;typedef struct {uint8_t handle;DeviceState device_state;DeviceInfo info;
} DeviceItem;DeviceItem devices[kMaxLidarCount];
uint32_t data_recveive_count[kMaxLidarCount];/** Connect all the broadcast device. */
int lidar_count = 0;
char broadcast_code_list[kMaxLidarCount][kBroadcastCodeSize];/** Connect the broadcast device in list, please input the broadcast code and modify the BROADCAST_CODE_LIST_SIZE. */
/*#define BROADCAST_CODE_LIST_SIZE  3
int lidar_count = BROADCAST_CODE_LIST_SIZE;
char broadcast_code_list[kMaxLidarCount][kBroadcastCodeSize] = {"000000000000002","000000000000003","000000000000004"
};*//** Receiving error message from Livox Lidar. */
void OnLidarErrorStatusCallback(livox_status status, uint8_t handle, ErrorMessage *message) {static uint32_t error_message_count = 0;if (message != NULL) {++error_message_count;if (0 == (error_message_count % 100)) {printf("handle: %u\n", handle);printf("temp_status : %u\n", message->lidar_error_code.temp_status);printf("volt_status : %u\n", message->lidar_error_code.volt_status);printf("motor_status : %u\n", message->lidar_error_code.motor_status);printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);printf("pps_status : %u\n", message->lidar_error_code.device_status);printf("fan_status : %u\n", message->lidar_error_code.fan_status);printf("self_heating : %u\n", message->lidar_error_code.self_heating);printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);printf("system_status : %u\n", message->lidar_error_code.system_status);}}
}/** Receiving point cloud data from Livox LiDAR. */
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {if (data) {data_recveive_count[handle] ++ ;if (data_recveive_count[handle] % 100 == 0) {/** 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;}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 packet num %d\n", data->data_type, data_recveive_count[handle]);}}
}/** Callback function of starting sampling. */
void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {printf("OnSampleCallback statue %d handle %d response %d \n", status, handle, response);if (status == kStatusSuccess) {if (response != 0) {devices[handle].device_state = kDeviceStateConnect;}} else if (status == kStatusTimeout) {devices[handle].device_state = kDeviceStateConnect;}
}/** Callback function of stopping sampling. */
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;
}/** Callback function of changing of device state. */
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);}if (devices[handle].device_state == kDeviceStateConnect) {printf("Device Working State %d\n", devices[handle].info.state);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;}}
}/** Callback function when broadcast message received.* You need to add listening device broadcast code and set the point cloud data callback in this function.*/
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {if (info == NULL || info->dev_type == kDeviceTypeHub) {return;}printf("Receive Broadcast Code %s\n", info->broadcast_code);if (lidar_count > 0) {bool found = false;int i = 0;for (i = 0; i < lidar_count; ++i) {if (strncmp(info->broadcast_code, broadcast_code_list[i], kBroadcastCodeSize) == 0) {found = true;break;}}if (!found) {return;}}bool result = false;uint8_t handle = 0;result = AddLidarToConnect(info->broadcast_code, &handle);if (result == kStatusSuccess) {/** Set the point cloud data for a specific Livox LiDAR. */SetDataCallback(handle, GetLidarData, NULL);devices[handle].handle = handle;devices[handle].device_state = kDeviceStateDisconnect;}
}int main(int argc, const char *argv[]) {printf("Livox SDK initializing.\n");
/** Initialize Livox-SDK. */if (!Init()) {return -1;}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));memset(data_recveive_count, 0, sizeof(data_recveive_count));/** Set the callback function receiving broadcast message from Livox LiDAR. */SetBroadcastCallback(OnDeviceBroadcast);/** Set the callback function called when device state change,* which means connection/disconnection and changing of LiDAR state.*/SetDeviceStateUpdateCallback(OnDeviceInfoChange);/** Start the device discovering routine. */if (!Start()) {Uninit();return -1;}printf("Start discovering device.\n");#ifdef WIN32Sleep(30000);
#elsesleep(30);
#endifint i = 0;for (i = 0; i < kMaxLidarCount; ++i) {if (devices[i].device_state == kDeviceStateSampling) {
/** Stop the sampling of Livox LiDAR. */LidarStopSampling(devices[i].handle, OnStopSampleCallback, NULL);}}/** Uninitialize Livox-SDK. */Uninit();
}

如果用户需要自己读取数据，只需要修改一下上面这段代码，然后合入到自己的项目当中，略作调整即可。