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_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();
}