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