mmr/Livox-SDK
Archived
1
0
Fork 0
Code Issues 105 Pull Requests 11 Actions Packages Projects Releases 9 Wiki Activity

feature:support for conneting horizon

Browse Source
This commit is contained in:
Livox-SDK
2019-12-19 15:25:48 +08:00
parent 83def61d80
commit da19f31bc4
1442 changed files with 52220 additions and 16430 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sample/hub/main.c
+125 -76
View File
@@ -59,22 +59,55 @@ char broadcast_code_list[BROADCAST_CODE_LIST_SIZE][kBroadcastCodeSize] = {
"00000000000001"
};*/
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {
static uint32_t receive_packet_count = 0;
if (data) {
++receive_packet_count;
if (0 == (receive_packet_count % 10000)) {
printf("receive packet count %d %d\n", data->id, receive_packet_count);
/** Parsing the timestamp and the point cloud data. */
uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
/** Receiving error message from Livox Hub. */
void OnHubErrorStatusCallback(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("sync_status : %u\n", message->hub_error_code.sync_status);
printf("temp_status : %u\n", message->hub_error_code.temp_status);
printf("lidar_status :%u\n", message->hub_error_code.lidar_status);
printf("lidar_link_status : %u\n", message->hub_error_code.lidar_link_status);
printf("firmware_err : %u\n", message->hub_error_code.firmware_err);
printf("system_status : %u\n", message->hub_error_code.system_status);
}
printf("receive packet from %d \n", (uint16_t) HubGetLidarHandle(data->slot, data->id));
}
}
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
/** Receiving point cloud data from Livox Hub. */
void GetHubData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {
static uint32_t receive_packet_count = 0;
if (data) {
++receive_packet_count;
if (0 == (receive_packet_count % 1000)) {
printf("receive packet handle: %d count: %d\n", (uint16_t) HubGetLidarHandle(data->slot, data->id), receive_packet_count);
/** 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;
}
printf("data_type %d data num %d\n", data->data_type, data_num);
}
}
}
/** 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) {
@@ -85,24 +118,13 @@ void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *da
}
}
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
/** Callback function of stopping sampling. */
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
}
void OnDeviceInformation(uint8_t 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 OnHubLidarInfo(uint8_t status, uint8_t handle, HubQueryLidarInformationResponse *response, void *client_data) {
/** Callback function of get LiDAR units' information. */
void OnHubLidarInfo(livox_status status, uint8_t handle, HubQueryLidarInformationResponse *response, void *client_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
@@ -117,7 +139,44 @@ void OnHubLidarInfo(uint8_t status, uint8_t handle, HubQueryLidarInformationResp
}
}
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
void HubDeviceDisconnect() {
uint8_t handle = 0;
for (handle = 0; handle < kMaxLidarCount; ++handle) {
if (devices[handle].device_state == kDeviceStateConnect ) {
devices[handle].device_state = kDeviceStateDisconnect;
}
}
}
void HubDeviceConnect(const DeviceInfo *info) {
HubDeviceDisconnect();
uint8_t handle = info->handle;
DeviceInfo *_devices = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
livox_status status = GetConnectedDevices(_devices, &count);
if (status == kStatusSuccess) {
int i = 0;
for (i = 0; i < count; ++i) {
uint8_t handle = _devices[i].handle;
if (handle < kMaxLidarCount) {
devices[handle].handle = handle;
devices[handle].info = _devices[i];
devices[handle].device_state = kDeviceStateConnect;
}
}
}
if (_devices) {
free(_devices);
}
HubQueryLidarInformation(OnHubLidarInfo, NULL);
}
void HubDeviceStateChange(const DeviceInfo *info) {
devices[info->handle].info = *info;
}
/** Callback function of changing of device state. */
void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
@@ -126,55 +185,37 @@ void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
DeviceInfo *_devices = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
uint8_t status = GetConnectedDevices(_devices, &count);
if (status == kStatusSuccess) {
int i = 0;
for (i = 0; i < count; ++i) {
uint8_t handle = _devices[i].handle;
if (handle < kMaxLidarCount) {
devices[handle].handle = handle;
devices[handle].info = _devices[i];
devices[handle].device_state = kDeviceStateConnect;
}
}
}
if (_devices) {
free(_devices);
}
if (devices[handle].info.type == kDeviceTypeHub) {
HubQueryLidarInformation(OnHubLidarInfo, NULL);
}
if (devices[handle].device_state == kDeviceStateDisconnect) {
devices[handle].device_state = kDeviceStateConnect;
devices[handle].info = *info;
}
if (type == kEventHubConnectionChange) {
HubDeviceConnect(info);
printf("[WARNING] Hub sn: [%s] Connection Change!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
devices[handle].device_state = kDeviceStateDisconnect;
HubDeviceDisconnect();
printf("[WARNING] Hub sn [%s] Disconnect!!!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
devices[handle].info = *info;
HubDeviceStateChange(info);
printf("[WARNING] Hub sn [%s] StateChange!!!\n", info->broadcast_code);
}
if (devices[handle].device_state == kDeviceStateConnect) {
printf("Device State error_code %d\n", devices[handle].info.status.status_code);
printf("Device State working state %d\n", devices[handle].info.state);
printf("Device feature %d\n", devices[handle].info.feature);
SetErrorMessageCallback(handle, OnHubErrorStatusCallback);
printf("Hub Working State %d\n", devices[handle].info.state);
if (devices[handle].info.state == kLidarStateInit) {
printf("Hub State Change Progress %u\n", devices[handle].info.status.progress);
} else {
printf("Hub State Error Code 0X%08x\n", devices[handle].info.status.status_code.error_code);
}
printf("Hub feature %d\n", devices[handle].info.feature);
if (devices[handle].info.state == kLidarStateNormal) {
if (devices[handle].info.type == kDeviceTypeHub) {
HubStartSampling(OnSampleCallback, NULL);
} else {
LidarStartSampling(handle, OnSampleCallback, NULL);
}
HubStartSampling(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) {
if (info == NULL || info->dev_type != kDeviceTypeHub) {
return;
}
@@ -198,7 +239,7 @@ void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
uint8_t handle = 0;
result = AddHubToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, GetLidarData, NULL);
SetDataCallback(handle, GetHubData, NULL);
devices[handle].handle = handle;
devices[handle].device_state = kDeviceStateDisconnect;
}
@@ -212,9 +253,9 @@ int SetProgramOption(int argc, const char *argv[]) {
apr_pool_t *mp = NULL;
static const apr_getopt_option_t opt_option[] = {
/** Long-option, short-option, has-arg flag, description */
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "help", 'h', 0, "Show help" },
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "help", 'h', 0, "Show help" },
{ NULL, 0, 0, NULL },
};
apr_getopt_t *opt = NULL;
@@ -241,6 +282,7 @@ int SetProgramOption(int argc, const char *argv[]) {
switch (optch) {
case 'c':
printf("Register broadcast code: %s\n", optarg);
is_connect_all_broadcast_device = false;
strncpy(broadcast_code_list[0], optarg, kBroadcastCodeSize);
break;
case 'l':
@@ -286,11 +328,18 @@ int main(int argc, const char *argv[]) {
printf("Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
memset(devices, 0, sizeof(devices));
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
/** 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()) {
printf("Start discovering device.\n");
printf("Start discovering device.\n");
#ifdef WIN32
Sleep(20000);
@@ -298,11 +347,11 @@ int main(int argc, const char *argv[]) {
sleep(20);
#endif
HubStopSampling(OnStopSampleCallback, NULL);
printf("stop sample\n");
printf("Stop sample\n");
#ifdef WIN32
Sleep(10000);
Sleep(2000);
#else
sleep(10);
sleep(2);
#endif
}
sample/hub_lvx_file/lvx_file.cpp
+55 -51
View File
@@ -28,10 +28,13 @@
#define WRITE_BUFFER_LEN 1024 * 1024
#define MAGIC_CODE (0xac0ea767)
#define PACK_POINT_NUM 100
#define RAW_POINT_NUM 100
#define SINGLE_POINT_NUM 96
#define DUAL_POINT_NUM 48
#define IMU_POINT_NUM 1
#define M_PI 3.14159265358979323846
LvxFileHandle::LvxFileHandle() : cur_frame_index_(0), cur_offset_(0) {
LvxFileHandle::LvxFileHandle() : cur_frame_index_(0), cur_offset_(0), frame_duration_(kDefaultFrameDurationTime) {
}
bool LvxFileHandle::InitLvxFile() {
@@ -39,13 +42,7 @@ bool LvxFileHandle::InitLvxFile() {
char filename[30] = { 0 };
tm* local_time = localtime(&curtime);
sprintf(filename, "%d-%02d-%02d_%02d-%02d-%02d.lvx", local_time->tm_year + 1900,
local_time->tm_mon + 1,
local_time->tm_mday,
local_time->tm_hour,
local_time->tm_min,
local_time->tm_sec);
strftime(filename, sizeof(filename), "%Y-%m-%d_%H-%M-%S.lvx", local_time);
lvx_file_.open(filename, std::ios::out | std::ios::binary);
if (!lvx_file_.is_open()) {
@@ -55,25 +52,29 @@ bool LvxFileHandle::InitLvxFile() {
}
void LvxFileHandle::InitLvxFileHeader() {
LvxFileHeader lvx_file_header = { 0 };
LvxFilePublicHeader lvx_file_public_header = { 0 };
std::unique_ptr<char[]> write_buffer(new char[WRITE_BUFFER_LEN]);
std::string signature = "livox_tech";
memcpy(lvx_file_header.signature, signature.c_str(), signature.size());
lvx_file_header.version[0] = 1;
lvx_file_header.version[1] = 0;
lvx_file_header.version[2] = 0;
lvx_file_header.version[3] = 0;
memcpy(lvx_file_public_header.signature, signature.c_str(), signature.size());
lvx_file_header.magic_code = MAGIC_CODE;
lvx_file_public_header.version[0] = 1;
lvx_file_public_header.version[1] = 1;
lvx_file_public_header.version[2] = 0;
lvx_file_public_header.version[3] = 0;
memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_header, sizeof(LvxFileHeader));
cur_offset_ += sizeof(LvxFileHeader);
lvx_file_public_header.magic_code = MAGIC_CODE;
memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_public_header, sizeof(LvxFilePublicHeader));
cur_offset_ += sizeof(LvxFilePublicHeader);
uint8_t device_count = static_cast<uint8_t>(device_info_list_.size());
memcpy(write_buffer.get() + cur_offset_, (void *)&device_count, sizeof(uint8_t));
cur_offset_ += sizeof(uint8_t);
LvxFilePrivateHeader lvx_file_private_header = { 0 };
lvx_file_private_header.frame_duration = frame_duration_;
lvx_file_private_header.device_count = device_count;
memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_private_header, sizeof(LvxFilePrivateHeader));
cur_offset_ += sizeof(LvxFilePrivateHeader);
for (int i = 0; i < device_count; i++) {
memcpy(write_buffer.get() + cur_offset_, (void *)&device_info_list_[i], sizeof(LvxDeviceInfo));
cur_offset_ += sizeof(LvxDeviceInfo);
@@ -87,10 +88,13 @@ void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxBasePackDetail> &point_packe
FrameHeader frame_header = { 0 };
std::unique_ptr<char[]> write_buffer(new char[WRITE_BUFFER_LEN]);
int pack_num = point_packet_list_temp.size();
frame_header.current_offset = cur_offset_;
frame_header.next_offset = cur_offset_ + (int64_t)pack_num * sizeof(LvxBasePackDetail) + sizeof(FrameHeader);
frame_header.package_count = pack_num;
frame_header.next_offset = cur_offset_ + sizeof(FrameHeader);
auto iterator = point_packet_list_temp.begin();
for (; iterator != point_packet_list_temp.end(); iterator++) {
frame_header.next_offset += iterator->pack_size;
}
frame_header.frame_index = cur_frame_index_;
memcpy(write_buffer.get() + cur_pos, (void*)&frame_header, sizeof(FrameHeader));
@@ -98,15 +102,15 @@ void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxBasePackDetail> &point_packe
auto iter = point_packet_list_temp.begin();
for (; iter != point_packet_list_temp.end(); iter++) {
if (cur_pos + sizeof(LvxBasePackDetail) >= WRITE_BUFFER_LEN) {
if (cur_pos + iter->pack_size >= WRITE_BUFFER_LEN) {
lvx_file_.write((char*)write_buffer.get(), cur_pos);
cur_pos = 0;
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), sizeof(LvxBasePackDetail));
cur_pos += sizeof(LvxBasePackDetail);
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), iter->pack_size);
cur_pos += iter->pack_size;
}
else {
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), sizeof(LvxBasePackDetail));
cur_pos += sizeof(LvxBasePackDetail);
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), iter->pack_size);
cur_pos += iter->pack_size;
}
}
lvx_file_.write((char*)write_buffer.get(), cur_pos);
@@ -129,27 +133,27 @@ void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data, LvxBasePackDetail &pa
packet.timestamp_type = data->timestamp_type;
packet.data_type = data->data_type;
memcpy(packet.timestamp, data->timestamp, 8 * sizeof(uint8_t));
if (packet.data_type == 0) {
LivoxRawPoint tmp[PACK_POINT_NUM];
memcpy(tmp, (void *)data->data, PACK_POINT_NUM * sizeof(LivoxRawPoint));
for (int i = 0; i < PACK_POINT_NUM; i++) {
packet.point[i].x = static_cast<float>(tmp[i].x / 1000.0);
packet.point[i].y = static_cast<float>(tmp[i].y / 1000.0);
packet.point[i].z = static_cast<float>(tmp[i].z / 1000.0);
packet.point[i].reflectivity = tmp[i].reflectivity;
}
}
else if (packet.data_type == 1) {
LivoxSpherPoint tmp[PACK_POINT_NUM];
memcpy(tmp, (void *)data->data, PACK_POINT_NUM * sizeof(LivoxSpherPoint));
for (int i = 0; i < PACK_POINT_NUM; i++) {
packet.point[i].x = static_cast<float>(tmp[i].depth / 1000.0);
packet.point[i].y = static_cast<float>(tmp[i].theta);
packet.point[i].z = static_cast<float>(tmp[i].phi);
packet.point[i].reflectivity = tmp[i].reflectivity;
LivoxPoint temp = { packet.point[i].x * std::sin(packet.point[i].y) * std::cos(packet.point[i].z), packet.point[i].x * std::sin(packet.point[i].y) * std::sin(packet.point[i].z), packet.point[i].x * std::cos(packet.point[i].y) };
packet.point[i] = temp;
}
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point) - \
sizeof(packet.pack_size) + RAW_POINT_NUM*sizeof(LivoxRawPoint);
memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxRawPoint));
} else if(packet.data_type == 1){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +RAW_POINT_NUM*sizeof(LivoxSpherPoint);
memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxSpherPoint));
} else if(packet.data_type == 2){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint);
memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint));
} else if(packet.data_type == 3){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint);
memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint));
} else if(packet.data_type == 4){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint);
memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint));
} else if(packet.data_type == 5){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint);
memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint));
} else if(packet.data_type == 6){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +IMU_POINT_NUM*sizeof(LivoxImuPoint);
memcpy(packet.raw_point,(void *)data->data,IMU_POINT_NUM*sizeof(LivoxImuPoint));
}
}
sample/hub_lvx_file/lvx_file.h
+17 -5
View File
@@ -33,6 +33,10 @@
#include <mutex>
#include "livox_sdk.h"
#define kMaxPointSize 1500
#define kDefaultFrameDurationTime 50
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
@@ -51,13 +55,20 @@ typedef struct {
uint8_t signature[16];
uint8_t version[4];
uint32_t magic_code;
}LvxFileHeader;
} LvxFilePublicHeader;
typedef struct {
uint32_t frame_duration;
uint8_t device_count;
} LvxFilePrivateHeader;
typedef struct {
uint8_t lidar_broadcast_code[16];
uint8_t hub_broadcast_code[16];
uint8_t device_index;
uint8_t device_type;
uint8_t extrinsic_enable;
float roll;
float pitch;
float yaw;
@@ -76,15 +87,15 @@ typedef struct {
uint8_t timestamp_type;
uint8_t data_type;
uint8_t timestamp[8];
LivoxPoint point[100];
}LvxBasePackDetail;
uint8_t raw_point[kMaxPointSize];
uint32_t pack_size;
} LvxBasePackDetail;
typedef struct {
uint64_t current_offset;
uint64_t next_offset;
uint64_t frame_index;
uint64_t package_count;
}FrameHeader;
} FrameHeader;
#pragma pack()
@@ -107,4 +118,5 @@ private:
std::vector<LvxDeviceInfo> device_info_list_;
uint32_t cur_frame_index_;
uint64_t cur_offset_;
uint32_t frame_duration_;
};
sample/hub_lvx_file/main.cpp
+104 -39
View File
@@ -44,30 +44,44 @@ bool is_finish_extrinsic_parameter = false;
#define FRAME_RATE 20
using namespace std::chrono;
/** Connect the first broadcast hub in default and connect specific device when use program options or broadcast_code_list is not empty. */
std::vector<std::string> broadcast_code_list = {
//"000000000000001"
};
/** Receiving error message from Livox Hub. */
void OnHubErrorStatusCallback(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("sync_status : %u\n", message->hub_error_code.sync_status);
printf("temp_status : %u\n", message->hub_error_code.temp_status);
printf("lidar_status :%u\n", message->hub_error_code.lidar_status);
printf("lidar_link_status : %u\n", message->hub_error_code.lidar_link_status);
printf("firmware_err : %u\n", message->hub_error_code.firmware_err);
printf("system_status : %u\n", message->hub_error_code.system_status);
}
}
}
/** Receiving point cloud data from Livox Hub. */
void GetHubData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {
if (data) {
if (is_finish_extrinsic_parameter) {
std::lock_guard<std::mutex> lock(mtx);
std::unique_lock<std::mutex> lock(mtx);
LvxBasePackDetail packet;
packet.device_index = lidar_units_index[HubGetLidarHandle(data->slot, data->id)];
lvx_file_handler.BasePointsHandle(data, packet);
point_packet_list.push_back(packet);
if (point_packet_list.size() % (50 * broadcast_code_list.size()) == 0) {
condition_variable.notify_one();
}
}
}
}
/** Callback function of starting sampling. */
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
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) {
@@ -79,15 +93,15 @@ void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *da
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
}
/** Callback function of get LiDAR units' extrinsic parameter. */
void OnGetLidarUnitsExtrinsicParameter(uint8_t status, uint8_t handle, HubGetExtrinsicParameterResponse *response, void *data) {
void OnGetLidarUnitsExtrinsicParameter(livox_status status, uint8_t handle, HubGetExtrinsicParameterResponse *response, void *data) {
if (status == kStatusSuccess) {
if (response != 0) {
printf("OnGetLidarUnitsExtrinsicParameter statue %d handle %d response %d \n", status, handle, response->ret_code);
std::lock_guard<std::mutex> lock(mtx);
std::unique_lock<std::mutex> lock(mtx);
LvxDeviceInfo lidar_info;
for (int i = 0; i < response->count; i++) {
ExtrinsicParameterResponseItem temp;
@@ -107,12 +121,20 @@ void OnGetLidarUnitsExtrinsicParameter(uint8_t status, uint8_t handle, HubGetExt
}
lidar_info.device_type = devices[handle].info.type;
lidar_info.extrinsic_enable = false;
lidar_info.pitch = temp.pitch;
lidar_info.roll = temp.roll;
lidar_info.yaw = temp.yaw;
lidar_info.x = static_cast<float>(temp.x / 1000.0);
lidar_info.y = static_cast<float>(temp.y / 1000.0);
lidar_info.z = static_cast<float>(temp.z / 1000.0);
printf("index: %d\n",lidar_info.device_index);
printf("pitch: %f\n", lidar_info.pitch);
printf("roll: %f\n", lidar_info.roll);
printf("yaw: %f\n", lidar_info.yaw);
printf("x: %f\n", lidar_info.x);
printf("y: %f\n", lidar_info.y);
printf("z: %f\n", lidar_info.z);
lvx_file_handler.AddDeviceInfo(lidar_info);
}
is_finish_extrinsic_parameter = true;
@@ -124,7 +146,8 @@ void OnGetLidarUnitsExtrinsicParameter(uint8_t status, uint8_t handle, HubGetExt
}
}
void OnHubLidarInfo(uint8_t status, uint8_t handle, HubQueryLidarInformationResponse *response, void *client_data) {
/** Callback function of get LiDAR units' information. */
void OnHubLidarInfo(livox_status status, uint8_t handle, HubQueryLidarInformationResponse *response, void *client_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
@@ -139,8 +162,44 @@ void OnHubLidarInfo(uint8_t status, uint8_t handle, HubQueryLidarInformationResp
}
}
void HubDeviceDisconnect() {
uint8_t handle = 0;
for (handle = 0; handle < kMaxLidarCount; ++handle) {
if (devices[handle].device_state == kDeviceStateConnect ) {
devices[handle].device_state = kDeviceStateDisconnect;
}
}
}
void HubDeviceConnectionChange() {
HubDeviceDisconnect();
DeviceInfo *_devices = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
livox_status status = GetConnectedDevices(_devices, &count);
if (status == kStatusSuccess) {
int i = 0;
for (i = 0; i < count; ++i) {
uint8_t handle = _devices[i].handle;
if (handle < kMaxLidarCount) {
devices[handle].handle = handle;
devices[handle].info = _devices[i];
devices[handle].device_state = kDeviceStateConnect;
}
}
}
if (_devices) {
free(_devices);
}
HubQueryLidarInformation(OnHubLidarInfo, NULL);
}
void HubDeviceStateChange(const DeviceInfo *info) {
devices[info->handle].info = *info;
}
/** Callback function of changing of device state. */
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type) {
if (info == nullptr) {
return;
}
@@ -149,28 +208,29 @@ void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
HubQueryLidarInformation(OnHubLidarInfo, nullptr);
if (devices[handle].device_state == kDeviceStateDisconnect) {
devices[handle].device_state = kDeviceStateConnect;
devices[handle].info = *info;
}
if (type == kEventHubConnectionChange) {
HubDeviceConnectionChange();
printf("[WARNING] Hub sn: [%s] Connection Change!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
devices[handle].device_state = kDeviceStateDisconnect;
HubDeviceDisconnect();
printf("[WARNING] Hub[%s] Disconnect!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
devices[handle].info = *info;
HubDeviceStateChange(info);
printf("[WARNING] Hub sn [%s] StateChange!!!\n", info->broadcast_code);
}
if (devices[handle].device_state == kDeviceStateConnect) {
printf("Device State error_code %d\n", devices[handle].info.status.status_code);
printf("Device State working state %d\n", devices[handle].info.state);
printf("Device feature %d\n", devices[handle].info.feature);
printf("Hub Working State %d\n", devices[handle].info.state);
if (devices[handle].info.state == kLidarStateInit) {
printf("Hub State Change Progress %u\n", devices[handle].info.status.progress);
} else {
printf("Hub State Error Code 0X%08x\n", devices[handle].info.status.status_code.error_code);
}
printf("Hub feature %d\n", devices[handle].info.feature);
SetErrorMessageCallback(handle, OnHubErrorStatusCallback);
if (devices[handle].info.state == kLidarStateNormal) {
if (devices[handle].info.type == kDeviceTypeHub) {
HubGetExtrinsicParameter(OnGetLidarUnitsExtrinsicParameter, nullptr);
HubStartSampling(OnSampleCallback, nullptr);
devices[handle].device_state = kDeviceStateSampling;
}
HubGetExtrinsicParameter(OnGetLidarUnitsExtrinsicParameter, nullptr);
HubStartSampling(OnSampleCallback, nullptr);
devices[handle].device_state = kDeviceStateSampling;
}
}
}
@@ -179,7 +239,7 @@ void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
* 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 == nullptr) {
if (info == nullptr || info->dev_type!= kDeviceTypeHub) {
return;
}
@@ -197,8 +257,7 @@ void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (!found) {
return;
}
}
else {
} else {
broadcast_code_list.push_back(info->broadcast_code);
return;
}
@@ -221,10 +280,10 @@ int SetProgramOption(int argc, const char *argv[]) {
apr_pool_t *mp = nullptr;
static const apr_getopt_option_t opt_option[] = {
/** Long-option, short-option, has-arg flag, description */
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "time", 't', 1, "Time to save point cloud to the lvx file" },
{ "help", 'h', 0, "Show help" },
{ "help", 'h', 0, "Show help" },
{ nullptr, 0, 0, nullptr },
};
apr_getopt_t *opt = nullptr;
@@ -311,7 +370,7 @@ int main(int argc, const char *argv[]) {
/** Set the callback function called when device state change,
* which means connection/disconnection and changing of LiDAR state.
*/
SetDeviceStateUpdateCallback(OnDeviceChange);
SetDeviceStateUpdateCallback(OnDeviceInfoChange);
/** Start the device discovering routine. */
if (!Start()) {
@@ -330,17 +389,23 @@ int main(int argc, const char *argv[]) {
Uninit();
return -1;
}
lvx_file_handler.InitLvxFileHeader();
int i = 0;
steady_clock::time_point last_time = steady_clock::now();
for (i = 0; i < lvx_file_save_time * FRAME_RATE; ++i) {
std::list<LvxBasePackDetail> point_packet_list_temp;
{
std::unique_lock<std::mutex> lock(mtx);
condition_variable.wait(lock);
condition_variable.wait_for(lock, milliseconds(kDefaultFrameDurationTime) - (steady_clock::now() - last_time));
last_time = steady_clock::now();
point_packet_list_temp.swap(point_packet_list);
}
if(point_packet_list_temp.empty()) {
printf("Point cloud packet is empty.\n");
break;
}
printf("Finish save %d frame to lvx file.\n", i);
lvx_file_handler.SaveFrameToLvxFile(point_packet_list_temp);
}
@@ -348,7 +413,7 @@ int main(int argc, const char *argv[]) {
lvx_file_handler.CloseLvxFile();
HubStopSampling(OnStopSampleCallback, NULL);
printf("stop sample\n");
printf("Stop sample\n");
Uninit();
}
sample/lidar/main.c
+83 -29
View File
@@ -62,22 +62,57 @@ char broadcast_code_list[kMaxLidarCount][kBroadcastCodeSize] = {
"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] += data_num;
if (data_recveive_count[handle] % 10000 == 0) {
printf("receive packet count %d %d\n", handle, data_recveive_count[handle]);
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));
LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
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;
}
printf("data_type %d packet num %d\n", data->data_type, data_recveive_count[handle]);
}
}
}
/** Callback function of starting sampling. */
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
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) {
@@ -89,11 +124,11 @@ void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *da
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox LiDAR. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
void OnDeviceInformation(livox_status status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
@@ -106,38 +141,57 @@ void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationRespon
}
}
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 OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
printf("OnDeviceChange broadcast code %s update type %d\n", info->broadcast_code, type);
uint8_t handle = info->handle;
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
QueryDeviceInformation(handle, OnDeviceInformation, NULL);
if (devices[handle].device_state == kDeviceStateDisconnect) {
devices[handle].device_state = kDeviceStateConnect;
devices[handle].info = *info;
}
LidarConnect(info);
printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
devices[handle].device_state = kDeviceStateDisconnect;
LidarDisConnect(info);
printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
devices[handle].info = *info;
LidarStateChange(info);
printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
}
if (devices[handle].device_state == kDeviceStateConnect) {
printf("Device State error_code %d\n", devices[handle].info.status.status_code);
printf("Device State working state %d\n", devices[handle].info.state);
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) {
if (devices[handle].info.type == kDeviceTypeHub) {
HubStartSampling(OnSampleCallback, NULL);
} else {
LidarStartSampling(handle, OnSampleCallback, NULL);
}
LidarStartSampling(handle, OnSampleCallback, NULL);
devices[handle].device_state = kDeviceStateSampling;
}
}
@@ -147,7 +201,7 @@ void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
* 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) {
if (info == NULL || info->dev_type == kDeviceTypeHub) {
return;
}
@@ -186,9 +240,9 @@ int SetProgramOption(int argc, const char *argv[]) {
apr_pool_t *mp = NULL;
static const apr_getopt_option_t opt_option[] = {
/** Long-option, short-option, has-arg flag, description */
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "help", 'h', 0, "Show help" },
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "help", 'h', 0, "Show help" },
{ NULL, 0, 0, NULL },
};
apr_getopt_t *opt = NULL;
@@ -280,7 +334,7 @@ int main(int argc, const char *argv[]) {
/** Set the callback function called when device state change,
* which means connection/disconnection and changing of LiDAR state.
*/
SetDeviceStateUpdateCallback(OnDeviceChange);
SetDeviceStateUpdateCallback(OnDeviceInfoChange);
/** Start the device discovering routine. */
if (!Start()) {
sample/lidar_lvx_file/lvx_file.cpp
+54 -77
View File
@@ -30,10 +30,13 @@
#define WRITE_BUFFER_LEN 1024 * 1024
#define MAGIC_CODE (0xac0ea767)
#define PACK_POINT_NUM 100
#define RAW_POINT_NUM 100
#define SINGLE_POINT_NUM 96
#define DUAL_POINT_NUM 48
#define IMU_POINT_NUM 1
#define M_PI 3.14159265358979323846
LvxFileHandle::LvxFileHandle() : cur_frame_index_(0), cur_offset_(0) {
LvxFileHandle::LvxFileHandle() : cur_frame_index_(0), cur_offset_(0), frame_duration_(kDefaultFrameDurationTime) {
}
bool LvxFileHandle::InitLvxFile() {
@@ -41,13 +44,7 @@ bool LvxFileHandle::InitLvxFile() {
char filename[30] = { 0 };
tm* local_time = localtime(&curtime);
sprintf(filename, "%d-%02d-%02d_%02d-%02d-%02d.lvx", local_time->tm_year + 1900,
local_time->tm_mon + 1,
local_time->tm_mday,
local_time->tm_hour,
local_time->tm_min,
local_time->tm_sec);
strftime(filename, sizeof(filename), "%Y-%m-%d_%H-%M-%S.lvx", local_time);
lvx_file_.open(filename, std::ios::out | std::ios::binary);
if (!lvx_file_.is_open()) {
@@ -57,25 +54,29 @@ bool LvxFileHandle::InitLvxFile() {
}
void LvxFileHandle::InitLvxFileHeader() {
LvxFileHeader lvx_file_header = { 0 };
LvxFilePublicHeader lvx_file_public_header = { 0 };
std::unique_ptr<char[]> write_buffer(new char[WRITE_BUFFER_LEN]);
std::string signature = "livox_tech";
memcpy(lvx_file_header.signature, signature.c_str(), signature.size());
lvx_file_header.version[0] = 1;
lvx_file_header.version[1] = 0;
lvx_file_header.version[2] = 0;
lvx_file_header.version[3] = 0;
memcpy(lvx_file_public_header.signature, signature.c_str(), signature.size());
lvx_file_header.magic_code = MAGIC_CODE;
lvx_file_public_header.version[0] = 1;
lvx_file_public_header.version[1] = 1;
lvx_file_public_header.version[2] = 0;
lvx_file_public_header.version[3] = 0;
memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_header, sizeof(LvxFileHeader));
cur_offset_ += sizeof(LvxFileHeader);
lvx_file_public_header.magic_code = MAGIC_CODE;
memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_public_header, sizeof(LvxFilePublicHeader));
cur_offset_ += sizeof(LvxFilePublicHeader);
uint8_t device_count = static_cast<uint8_t>(device_info_list_.size());
memcpy(write_buffer.get() + cur_offset_, (void *)&device_count, sizeof(uint8_t));
cur_offset_ += sizeof(uint8_t);
LvxFilePrivateHeader lvx_file_private_header = { 0 };
lvx_file_private_header.frame_duration = frame_duration_;
lvx_file_private_header.device_count = device_count;
memcpy(write_buffer.get() + cur_offset_, (void *)&lvx_file_private_header, sizeof(LvxFilePrivateHeader));
cur_offset_ += sizeof(LvxFilePrivateHeader);
for (int i = 0; i < device_count; i++) {
memcpy(write_buffer.get() + cur_offset_, (void *)&device_info_list_[i], sizeof(LvxDeviceInfo));
cur_offset_ += sizeof(LvxDeviceInfo);
@@ -89,10 +90,13 @@ void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxBasePackDetail> &point_packe
FrameHeader frame_header = { 0 };
std::unique_ptr<char[]> write_buffer(new char[WRITE_BUFFER_LEN]);
int pack_num = point_packet_list_temp.size();
frame_header.current_offset = cur_offset_;
frame_header.next_offset = cur_offset_ + (int64_t)pack_num * sizeof(LvxBasePackDetail) + sizeof(FrameHeader);
frame_header.package_count = pack_num;
frame_header.next_offset = cur_offset_ + sizeof(FrameHeader);
auto iterator = point_packet_list_temp.begin();
for (; iterator != point_packet_list_temp.end(); iterator++) {
frame_header.next_offset += iterator->pack_size;
}
frame_header.frame_index = cur_frame_index_;
memcpy(write_buffer.get() + cur_pos, (void*)&frame_header, sizeof(FrameHeader));
@@ -100,15 +104,15 @@ void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxBasePackDetail> &point_packe
auto iter = point_packet_list_temp.begin();
for (; iter != point_packet_list_temp.end(); iter++) {
if (cur_pos + sizeof(LvxBasePackDetail) >= WRITE_BUFFER_LEN) {
if (cur_pos + iter->pack_size >= WRITE_BUFFER_LEN) {
lvx_file_.write((char*)write_buffer.get(), cur_pos);
cur_pos = 0;
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), sizeof(LvxBasePackDetail));
cur_pos += sizeof(LvxBasePackDetail);
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), iter->pack_size);
cur_pos += iter->pack_size;
}
else {
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), sizeof(LvxBasePackDetail));
cur_pos += sizeof(LvxBasePackDetail);
memcpy(write_buffer.get() + cur_pos, (void*)&(*iter), iter->pack_size);
cur_pos += iter->pack_size;
}
}
lvx_file_.write((char*)write_buffer.get(), cur_pos);
@@ -131,54 +135,27 @@ void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data, LvxBasePackDetail &pa
packet.timestamp_type = data->timestamp_type;
packet.data_type = data->data_type;
memcpy(packet.timestamp, data->timestamp, 8 * sizeof(uint8_t));
if (packet.data_type == 0) {
LivoxRawPoint tmp[PACK_POINT_NUM];
memcpy(tmp, (void *)data->data, PACK_POINT_NUM * sizeof(LivoxRawPoint));
for (int i = 0; i < PACK_POINT_NUM; i++) {
packet.point[i].x = static_cast<float>(tmp[i].x / 1000.0);
packet.point[i].y = static_cast<float>(tmp[i].y / 1000.0);
packet.point[i].z = static_cast<float>(tmp[i].z / 1000.0);
packet.point[i].reflectivity = tmp[i].reflectivity;
}
}
else if (packet.data_type == 1) {
LivoxSpherPoint tmp[PACK_POINT_NUM];
memcpy(tmp, (void *)data->data, PACK_POINT_NUM * sizeof(LivoxSpherPoint));
for (int i = 0; i < PACK_POINT_NUM; i++) {
packet.point[i].x = static_cast<float>(tmp[i].depth / 1000.0);
packet.point[i].y = static_cast<float>(tmp[i].theta);
packet.point[i].z = static_cast<float>(tmp[i].phi);
packet.point[i].reflectivity = tmp[i].reflectivity;
}
}
}
void LvxFileHandle::CalcExtrinsicPoints(LvxBasePackDetail &packet) {
LvxDeviceInfo info = device_info_list_[packet.device_index];
info.roll = static_cast<float>(info.roll * M_PI / 180.0);
info.pitch = static_cast<float>(info.pitch * M_PI / 180.0);
info.yaw = static_cast<float>(info.yaw * M_PI / 180.0);
float rotate[3][3] = { { std::cos(info.pitch) * std::cos(info.yaw), std::sin(info.roll) * std::sin(info.pitch) * std::cos(info.yaw) - std::cos(info.roll) * std::sin(info.yaw), std::cos(info.roll) * std::sin(info.pitch) * std::cos(info.yaw) + std::sin(info.roll) * std::sin(info.yaw) },
{ std::cos(info.pitch) * std::sin(info.yaw), std::sin(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) + std::cos(info.roll) * std::cos(info.yaw), std::cos(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) - std::sin(info.roll) * std::cos(info.yaw) },
{ -std::sin(info.pitch), std::sin(info.roll) * std::cos(info.pitch), std::cos(info.roll) * std::cos(info.pitch) } };
float trans[3] = { static_cast<float>(info.x), static_cast<float>(info.y), static_cast<float>(info.z) };
if (packet.data_type == 0) {
for (int i = 0; i < PACK_POINT_NUM; i++) {
LivoxPoint temp = packet.point[i];
packet.point[i].x = temp.x * rotate[0][0] + temp.y * rotate[0][1] + temp.z * rotate[0][2] + trans[0];
packet.point[i].y = temp.x * rotate[1][0] + temp.y * rotate[1][1] + temp.z * rotate[1][2] + trans[1];
packet.point[i].z = temp.x * rotate[2][0] + temp.y * rotate[2][1] + temp.z * rotate[2][2] + trans[2];
}
}
else {
for (int i = 0; i < PACK_POINT_NUM; i++) {
LivoxPoint temp = { packet.point[i].x * std::sin(packet.point[i].y) * std::cos(packet.point[i].z), packet.point[i].x * std::sin(packet.point[i].y) * std::sin(packet.point[i].z), packet.point[i].x * std::cos(packet.point[i].y) };
packet.point[i].x = temp.x * rotate[0][0] + temp.y * rotate[0][1] + temp.z * rotate[0][2] + trans[0];
packet.point[i].y = temp.x * rotate[1][0] + temp.y * rotate[1][1] + temp.z * rotate[1][2] + trans[1];
packet.point[i].z = temp.x * rotate[2][0] + temp.y * rotate[2][1] + temp.z * rotate[2][2] + trans[2];
}
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) + RAW_POINT_NUM*sizeof(LivoxRawPoint);
memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxRawPoint));
} else if(packet.data_type == 1){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +RAW_POINT_NUM*sizeof(LivoxSpherPoint);
memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxSpherPoint));
} else if(packet.data_type == 2){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint);
memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint));
} else if(packet.data_type == 3){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint);
memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint));
} else if(packet.data_type == 4){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint);
memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint));
} else if(packet.data_type == 5){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint);
memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint));
} else if(packet.data_type == 6){
packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +IMU_POINT_NUM*sizeof(LivoxImuPoint);
memcpy(packet.raw_point,(void *)data->data,IMU_POINT_NUM*sizeof(LivoxImuPoint));
}
}
sample/lidar_lvx_file/lvx_file.h
+15 -6
View File
@@ -33,6 +33,9 @@
#include <mutex>
#include "livox_sdk.h"
#define kMaxPointSize 1500
#define kDefaultFrameDurationTime 50
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
@@ -51,13 +54,19 @@ typedef struct {
uint8_t signature[16];
uint8_t version[4];
uint32_t magic_code;
}LvxFileHeader;
} LvxFilePublicHeader;
typedef struct {
uint32_t frame_duration;
uint8_t device_count;
} LvxFilePrivateHeader;
typedef struct {
uint8_t lidar_broadcast_code[16];
uint8_t hub_broadcast_code[16];
uint8_t device_index;
uint8_t device_type;
uint8_t extrinsic_enable;
float roll;
float pitch;
float yaw;
@@ -76,15 +85,15 @@ typedef struct {
uint8_t timestamp_type;
uint8_t data_type;
uint8_t timestamp[8];
LivoxPoint point[100];
}LvxBasePackDetail;
uint8_t raw_point[kMaxPointSize];
uint32_t pack_size;
} LvxBasePackDetail;
typedef struct {
uint64_t current_offset;
uint64_t next_offset;
uint64_t frame_index;
uint64_t package_count;
}FrameHeader;
} FrameHeader;
#pragma pack()
@@ -101,13 +110,13 @@ public:
int GetDeviceInfoListSize() { return device_info_list_.size(); }
void BasePointsHandle(LivoxEthPacket *data, LvxBasePackDetail &packet);
void CalcExtrinsicPoints(LvxBasePackDetail &packet);
private:
std::ofstream lvx_file_;
std::vector<LvxDeviceInfo> device_info_list_;
uint32_t cur_frame_index_;
uint64_t cur_offset_;
uint32_t frame_duration_;
};
void ParseExtrinsicXml(DeviceItem &item, LvxDeviceInfo &info);
sample/lidar_lvx_file/main.cpp
+151 -86
View File
@@ -22,11 +22,6 @@
// SOFTWARE.
//
#ifdef WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif
#include <apr_general.h>
#include <apr_getopt.h>
#include <algorithm>
@@ -37,42 +32,66 @@ DeviceItem devices[kMaxLidarCount];
LvxFileHandle lvx_file_handler;
std::list<LvxBasePackDetail> point_packet_list;
std::vector<std::string> broadcast_code_rev;
std::condition_variable condition_variable;
std::condition_variable lidar_arrive_condition;
std::condition_variable extrinsic_condition;
std::condition_variable point_pack_condition;
std::mutex mtx;
int lvx_file_save_time = 10;
bool is_finish_extrinsic_parameter = false;
bool is_read_extrinsic_from_xml = false;
uint8_t connected_lidar_count = 0;
#define FRAME_RATE 20
using namespace std::chrono;
/** Connect all the broadcast device in default and connect specific device when use program options or broadcast_code_list is not empty. */
std::vector<std::string> broadcast_code_list = {
//"000000000000001"
//"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) {
if (handle < broadcast_code_list.size() && is_finish_extrinsic_parameter) {
std::lock_guard<std::mutex> lock(mtx);
if (handle < connected_lidar_count && is_finish_extrinsic_parameter) {
std::unique_lock<std::mutex> lock(mtx);
LvxBasePackDetail packet;
packet.device_index = handle;
lvx_file_handler.BasePointsHandle(data, packet);
lvx_file_handler.CalcExtrinsicPoints(packet);
point_packet_list.push_back(packet);
if (point_packet_list.size() % (50 * broadcast_code_list.size()) == 0) {
condition_variable.notify_one();
}
}
}
}
/** Callback function of starting sampling. */
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
printf("OnSampleCallback statue %d handle %d response %d \n", status, handle, response);
void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
printf("OnSampleCallback statues %d handle %d response %d \n", status, handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
devices[handle].device_state = kDeviceStateConnect;
@@ -83,20 +102,21 @@ void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *da
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
}
/** Callback function of get LiDARs' extrinsic parameter. */
void OnGetLidarExtrinsicParameter(uint8_t status, uint8_t handle, LidarGetExtrinsicParameterResponse *response, void *data) {
void OnGetLidarExtrinsicParameter(livox_status status, uint8_t handle, LidarGetExtrinsicParameterResponse *response, void *data) {
if (status == kStatusSuccess) {
if (response != 0) {
printf("OnGetLidarExtrinsicParameter statue %d handle %d response %d \n", status, handle, response->ret_code);
std::lock_guard<std::mutex> lock(mtx);
std::unique_lock<std::mutex> lock(mtx);
LvxDeviceInfo lidar_info;
strncpy((char *)lidar_info.lidar_broadcast_code, devices[handle].info.broadcast_code, kBroadcastCodeSize);
memset(lidar_info.hub_broadcast_code, 0, kBroadcastCodeSize);
lidar_info.device_index = handle;
lidar_info.device_type = devices[handle].info.type;
lidar_info.extrinsic_enable = true;
lidar_info.pitch = response->pitch;
lidar_info.roll = response->roll;
lidar_info.yaw = response->yaw;
@@ -104,9 +124,9 @@ void OnGetLidarExtrinsicParameter(uint8_t status, uint8_t handle, LidarGetExtrin
lidar_info.y = static_cast<float>(response->y / 1000.0);
lidar_info.z = static_cast<float>(response->z / 1000.0);
lvx_file_handler.AddDeviceInfo(lidar_info);
if (lvx_file_handler.GetDeviceInfoListSize() == broadcast_code_list.size()) {
if (lvx_file_handler.GetDeviceInfoListSize() == connected_lidar_count) {
is_finish_extrinsic_parameter = true;
condition_variable.notify_one();
extrinsic_condition.notify_one();
}
}
}
@@ -122,12 +142,12 @@ void LidarGetExtrinsicFromXml(uint8_t handle) {
lvx_file_handler.AddDeviceInfo(lidar_info);
if (lvx_file_handler.GetDeviceInfoListSize() == broadcast_code_list.size()) {
is_finish_extrinsic_parameter = true;
condition_variable.notify_one();
extrinsic_condition.notify_one();
}
}
/** Query the firmware version of Livox LiDAR. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
void OnDeviceInformation(livox_status status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
@@ -140,8 +160,27 @@ void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationRespon
}
}
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 OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type) {
if (info == nullptr) {
return;
}
@@ -150,33 +189,35 @@ void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
QueryDeviceInformation(handle, OnDeviceInformation, nullptr);
if (devices[handle].device_state == kDeviceStateDisconnect) {
devices[handle].device_state = kDeviceStateConnect;
devices[handle].info = *info;
}
LidarConnect(info);
printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
devices[handle].device_state = kDeviceStateDisconnect;
LidarDisConnect(info);
printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
devices[handle].info = *info;
LidarStateChange(info);
printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
}
if (devices[handle].device_state == kDeviceStateConnect) {
printf("Device State error_code %d\n", devices[handle].info.status.status_code);
printf("Device State working state %d\n", devices[handle].info.state);
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) {
if (devices[handle].info.type != kDeviceTypeHub) {
if (!is_read_extrinsic_from_xml) {
LidarGetExtrinsicParameter(handle, OnGetLidarExtrinsicParameter, nullptr);
}
else {
LidarGetExtrinsicFromXml(handle);
}
LidarStartSampling(handle, OnSampleCallback, nullptr);
devices[handle].device_state = kDeviceStateSampling;
if (!is_read_extrinsic_from_xml) {
LidarGetExtrinsicParameter(handle, OnGetLidarExtrinsicParameter, nullptr);
} else {
LidarGetExtrinsicFromXml(handle);
}
LidarStartSampling(handle, OnSampleCallback, nullptr);
devices[handle].device_state = kDeviceStateSampling;
}
}
}
@@ -185,42 +226,61 @@ void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
* 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 == nullptr) {
if (info == nullptr || info->dev_type == kDeviceTypeHub) {
return;
}
printf("Receive Broadcast Code %s\n", info->broadcast_code);
if (broadcast_code_list.size() > 0) {
bool found = false;
uint8_t i = 0;
for (i = 0; i < broadcast_code_list.size(); ++i) {
if (strncmp(info->broadcast_code, broadcast_code_list[i].c_str(), kBroadcastCodeSize) == 0) {
found = true;
break;
}
}
if (!found) {
return;
}
}
else {
if ((broadcast_code_rev.size() == 0) || (std::find(broadcast_code_rev.begin(), broadcast_code_rev.end(), info->broadcast_code) == broadcast_code_rev.end()))
broadcast_code_rev.push_back(info->broadcast_code);
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, nullptr);
devices[handle].handle = handle;
devices[handle].device_state = kDeviceStateDisconnect;
if ((broadcast_code_rev.size() == 0) ||
(std::find(broadcast_code_rev.begin(), broadcast_code_rev.end(), info->broadcast_code) == broadcast_code_rev.end())) {
broadcast_code_rev.push_back(info->broadcast_code);
lidar_arrive_condition.notify_one();
}
}
/** Wait until no new device arriving in 2 second. */
void WaitForDevicesReady( ) {
bool device_ready = false;
seconds wait_time = seconds(2);
steady_clock::time_point last_time = steady_clock::now();
while (!device_ready) {
std::unique_lock<std::mutex> lock(mtx);
lidar_arrive_condition.wait_for(lock,wait_time);
if ((steady_clock::now() - last_time + milliseconds(50)) >= wait_time) {
device_ready = true;
} else {
last_time = steady_clock::now();
}
}
}
void WaitForExtrinsicParameter() {
std::unique_lock<std::mutex> lock(mtx);
extrinsic_condition.wait(lock);
}
void AddDevicesToConnect() {
if (broadcast_code_rev.size() == 0)
return;
for (int i = 0; i < broadcast_code_rev.size(); ++i) {
if ((broadcast_code_list.size() != 0) &&
(std::find(broadcast_code_list.begin(), broadcast_code_list.end(), broadcast_code_rev[i]) == broadcast_code_list.end())) {
continue;
}
uint8_t handle = 0;
bool result = AddLidarToConnect(broadcast_code_rev[i].c_str(), &handle);
if (result == kStatusSuccess) {
/** Set the point cloud data for a specific Livox LiDAR. */
SetDataCallback(handle, GetLidarData, nullptr);
devices[handle].handle = handle;
devices[handle].device_state = kDeviceStateDisconnect;
connected_lidar_count++;
}
}
}
/** Set the program options.
* You can input the registered device broadcast code and decide whether to save the log file.
*/
@@ -229,11 +289,11 @@ int SetProgramOption(int argc, const char *argv[]) {
apr_pool_t *mp = nullptr;
static const apr_getopt_option_t opt_option[] = {
/** Long-option, short-option, has-arg flag, description */
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "time", 't', 1, "Time to save point cloud to the lvx file" },
{ "param", 'p', 0, "Get the extrinsic parameter from extrinsic.xml file" },
{ "help", 'h', 0, "Show help" },
{ "help", 'h', 0, "Show help" },
{ nullptr, 0, 0, nullptr },
};
apr_getopt_t *opt = nullptr;
@@ -338,7 +398,7 @@ int main(int argc, const char *argv[]) {
/** Set the callback function called when device state change,
* which means connection/disconnection and changing of LiDAR state.
*/
SetDeviceStateUpdateCallback(OnDeviceChange);
SetDeviceStateUpdateCallback(OnDeviceInfoChange);
/** Start the device discovering routine. */
if (!Start()) {
@@ -347,35 +407,40 @@ int main(int argc, const char *argv[]) {
}
printf("Start discovering device.\n");
#ifdef WIN32
Sleep(2000);
#else
sleep(2);
#endif
WaitForDevicesReady();
if (broadcast_code_rev.size() != 0)
broadcast_code_list = broadcast_code_rev;
AddDevicesToConnect();
{
std::unique_lock<std::mutex> lock(mtx);
condition_variable.wait(lock);
if (connected_lidar_count == 0) {
printf("No device will be connected.\n");
Uninit();
return -1;
}
WaitForExtrinsicParameter();
printf("Start initialize lvx file.\n");
if (!lvx_file_handler.InitLvxFile()) {
Uninit();
return -1;
}
lvx_file_handler.InitLvxFileHeader();
int i = 0;
steady_clock::time_point last_time = steady_clock::now();
for (i = 0; i < lvx_file_save_time * FRAME_RATE; ++i) {
std::list<LvxBasePackDetail> point_packet_list_temp;
{
std::unique_lock<std::mutex> lock(mtx);
condition_variable.wait(lock);
point_pack_condition.wait_for(lock, milliseconds(kDefaultFrameDurationTime) - (steady_clock::now() - last_time));
last_time = steady_clock::now();
point_packet_list_temp.swap(point_packet_list);
}
if(point_packet_list_temp.empty()) {
printf("Point cloud packet is empty.\n");
break;
}
printf("Finish save %d frame to lvx file.\n", i);
lvx_file_handler.SaveFrameToLvxFile(point_packet_list_temp);
@@ -392,4 +457,4 @@ int main(int argc, const char *argv[]) {
/** Uninitialize Livox-SDK. */
Uninit();
}
}