livox_ros_driver2
livox_ros_driver2 copied to clipboard
Published
20 hours ago •
Livox-SDK
Livox-SDK
convert lvx2 to rosbag
I see there is sample data on Livox Mid360 product page but its in lvx2 format. Is it possible for you to upload it in rosbag or pcd(s) format? I tried the converter tool in LivoxViewer2 but it creates one pcd file for all of the scans.
It's not working, what's wrong?
- lds_lvx.cpp
//
// The MIT License (MIT)
//
// Copyright (c) 2022 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 <unistd.h>
#include <string.h>
#include <functional>
#include <memory>
#include <thread>
#include <condition_variable>
#include <mutex>
#include "lds_lvx.h"
#include "lvx_file.h"
#include "comm/pub_handler.h"
#include <unistd.h>
#include <sys/param.h>
namespace livox_ros {
// std::condition_variable LdsLvx::cv_;
// std::mutex LdsLvx::mtx_;
std::atomic_bool LdsLvx::is_file_end_(false);
LdsLvx::LdsLvx(double publish_freq) : Lds(publish_freq, kSourceLvxFile), is_initialized_(false) {
}
LdsLvx::~LdsLvx() {
}
int LdsLvx::Init(const char *lvx_path) {
if (is_initialized_) {
printf("Livox file data source is already inited!\n");
return false;
}
#ifdef BUILDING_ROS2
DisableLivoxSdkConsoleLogger();
#endif
printf("Lds lvx init lvx_path:%s.\n", lvx_path);
inf_lvx2_.open(lvx_path, std::ios_base::binary);
if (!inf_lvx2_)
{
char path[MAXPATHLEN];
char *p = getcwd (path, sizeof(path));
printf("Open %s @ %s file fail!\n", lvx_path, p);
return false;
}
inf_lvx2_.seekg( 0, std::ios_base::end );
total_frame_ = inf_lvx2_.tellg();
inf_lvx2_.seekg( 0, std::ios_base::beg );
ResetLds(kSourceLvxFile);
PubHeader pubheader;
inf_lvx2_.read((char*)(&pubheader), sizeof(pubheader));
PriHeader priheader;
inf_lvx2_.read((char*)(&priheader), sizeof(priheader));
uint32_t valid_lidar_count_ = (int)priheader.device_count;
if (!valid_lidar_count_ || (valid_lidar_count_ >= kMaxSourceLidar)) {
inf_lvx2_.close();
printf("Lidar count error in %s : %d\n", lvx_path, valid_lidar_count_);
return false;
}
printf("LvxFile[%s] have %d lidars\n", lvx_path, valid_lidar_count_);
for (uint32_t i = 0; i < valid_lidar_count_; i++) {
DeviceInfo devinfo;
inf_lvx2_.read((char*)(&devinfo), sizeof(devinfo));
uint32_t handle = devinfo.lidar_id;
uint8_t index = 0;
int8_t ret = cache_index_.GetFreeIndex(kLivoxLidarType, handle, index);
if (ret != 0) {
std::cout << "failed to get free index, lidar ip: " << IpNumToString(handle) << std::endl;
continue;
}
LidarDevice& lidar = lidars_[index];
lidar.lidar_type = kLivoxLidarType;
lidar.connect_state = kConnectStateSampling;
lidar.handle = handle;
printf("lidar.lidar_type: %d; devinfo.lidar_type: %d, device_type:%d\n", lidar.lidar_type, devinfo.lidar_type, devinfo.device_type);
LidarExtParameter lidar_param;
lidar_param.handle = handle;
lidar_param.lidar_type = kLivoxLidarType;
lidar_param.param.roll = devinfo.roll;
lidar_param.param.pitch = devinfo.pitch;
lidar_param.param.yaw = devinfo.yaw;
lidar_param.param.x = devinfo.x;
lidar_param.param.y = devinfo.y;
lidar_param.param.z = devinfo.z;
pub_handler().AddLidarsExtParam(lidar_param);
}
t_read_lvx_ =
std::make_shared<std::thread>(std::bind(&LdsLvx::ReadLvxFile, this));
is_initialized_ = true;
StartRead();
return true;
}
void LdsLvx::OnPointCloudsFrameCallback(uint32_t frame_index, uint32_t total_frame, PointFrame *point_cloud_frame, void *client_data) {
if (!point_cloud_frame) {
printf("Point clouds frame call back failed, point cloud frame is nullptr.\n");
return;
}
LdsLvx* lds = static_cast<LdsLvx*>(client_data);
if (lds == nullptr) {
printf("Point clouds frame call back failed, client data is nullptr.\n");
return;
}
lds->StorageLvxPointData(point_cloud_frame);
if (frame_index == total_frame) {
is_file_end_.store(true);
}
}
void LdsLvx::ReadLvxFile() {
while (!start_read_lvx_);
printf("Start to read lvx file.\n");
uint8_t line_num = kLineNumberHAP;
auto start_time = std::chrono::high_resolution_clock::now();
int frame_cnt = 0;
while (!inf_lvx2_.eof())
{
FrameHeader fraheader;
inf_lvx2_.read((char*)(&fraheader), sizeof(fraheader));
// std::cout << "Frm curr offset:" << fraheader.curr_offset << std::endl;
// std::cout << "Next offset:" << fraheader.next_offset << std::endl;
// std::cout << "Frm idx:" << fraheader.frame_idx << std::endl;
int bindx = 0;
while (bindx < (int)(fraheader.next_offset - fraheader.curr_offset - sizeof(fraheader)))
{
BasePackHeader pheader;
inf_lvx2_.read((char*)(&pheader), sizeof(pheader));
bindx += sizeof(pheader);
PointFrame point_cloud_frame;
std::vector<PointXyzlt> points_clouds;
point_cloud_frame.lidar_num = 1;
PointPacket &pkt = point_cloud_frame.lidar_point[0];
pkt.handle = pheader.lidar_id;
pkt.lidar_type = LidarProtoType::kLivoxLidarType;
// std::cout << "Data type:" << (int)pheader.data_type << std::endl;
// std::cout << "Length:" << pheader.length << std::endl;
// std::cout << "Frm counter:" << (int)pheader.frame_counter << std::endl;
//long long int ts = *reinterpret_cast<long long int*>(&pheader.timestamp[0]);
//point_cloud_frame.base_time = ts;
point_cloud_frame.base_time = std::chrono::high_resolution_clock::now().time_since_epoch().count();
// printf("timestampe: %lld\n", ts);
if (pheader.data_type == 1)
{
int pcount = pheader.length / 14;
points_clouds.resize(pcount);
pkt.points_num = pcount;
pkt.points = points_clouds.data();
//printf("pcount: %d\n", pcount);
for (int i = 0; i < pcount; i++)
{
ExtendRowPoint pdetail;
inf_lvx2_.read((char*)(&pdetail), sizeof(pdetail));
PointXyzlt &p = points_clouds[i];
p.x = pdetail.x * 0.001f;
p.y = pdetail.y * 0.001f;
p.z = pdetail.z * 0.001f;
p.intensity = pdetail.reflectivity;
p.line = i % line_num;
p.tag = pdetail.tag;
//p.offset_time = ts + i;
p.offset_time = std::chrono::high_resolution_clock::now().time_since_epoch().count();
/*
fdata.push_back(pdetail.x * 0.001f);
fdata.push_back(pdetail.y * 0.001f);
fdata.push_back(pdetail.z * 0.001f);
fdata.push_back(pdetail.reflectivity);
fdata.push_back(pdetail.tag);
*/
}
} else if (pheader.data_type == 2) {
int pcount = pheader.length / 8;
points_clouds.resize(pcount);
//printf("pcount: %d\n", pcount);
for (int i = 0; i < pcount; i++)
{
ExtendHalfRowPoint pdetail;
inf_lvx2_.read((char*)(&pdetail), sizeof(pdetail));
/*
fdata.push_back(pdetail.x * 0.01f);
fdata.push_back(pdetail.y * 0.01f);
fdata.push_back(pdetail.z * 0.01f);
fdata.push_back(pdetail.reflectivity);
fdata.push_back(pdetail.tag);
*/
}
} else {
std::cout << "Error:Can not surport this data type---" << (int)pheader.data_type << std::endl;
break;
}
StorageLvxPointData(&point_cloud_frame);
bindx += pheader.length;
}
++frame_cnt;
auto diff = std::chrono::milliseconds(50) * frame_cnt + start_time - std::chrono::high_resolution_clock::now();
if(diff > std::chrono::milliseconds(0))
{
std::this_thread::sleep_for(diff);
}
}
printf("done\n");
inf_lvx2_.close();
}
} // namespace livox_ros
- lds_lvx.h
//
// The MIT License (MIT)
//
// Copyright (c) 2022 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.
//
// livox lidar lvx data source
#ifndef LIVOX_ROS_DRIVER_LDS_LVX_H_
#define LIVOX_ROS_DRIVER_LDS_LVX_H_
#include <memory>
#include <atomic>
#include <fstream>
#include "lds.h"
#include "comm/comm.h"
#include "livox_lidar_api.h"
#include "livox_lidar_def.h"
namespace livox_ros {
/**
* Lidar data source abstract.
*/
class LdsLvx final : public Lds {
public:
static LdsLvx *GetInstance(double publish_freq) {
static LdsLvx lds_lvx(publish_freq);
return &lds_lvx;
}
int Init(const char *lvx_path);
void ReadLvxFile();
private:
LdsLvx(double publish_freq);
LdsLvx(const LdsLvx &) = delete;
~LdsLvx();
LdsLvx &operator=(const LdsLvx &) = delete;
void StartRead() { start_read_lvx_ = true; }
void StopRead() { start_read_lvx_ = false; }
bool IsStarted() { return start_read_lvx_; }
static void OnPointCloudsFrameCallback(uint32_t frame_index, uint32_t total_frame, PointFrame *point_cloud_frame, void *client_data);
private:
volatile bool is_initialized_;
std::ifstream inf_lvx2_;
uint32_t total_frame_;
std::shared_ptr<std::thread> t_read_lvx_;
volatile bool start_read_lvx_;
static std::atomic_bool is_file_end_;
};
} // namespace livox_ros
#endif
- livox_ros_driver2.cpp
//
// The MIT License (MIT)
//
// Copyright (c) 2022 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 <iostream>
#include <chrono>
#include <vector>
#include <csignal>
#include <thread>
#include "include/livox_ros_driver2.h"
#include "include/ros_headers.h"
#include "driver_node.h"
#include "lddc.h"
#include "lds_lidar.h"
#include "lds_lvx.h"
using namespace livox_ros;
#ifdef BUILDING_ROS1
int main(int argc, char **argv) {
/** Ros related */
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {
ros::console::notifyLoggerLevelsChanged();
}
ros::init(argc, argv, "livox_lidar_publisher");
// ros::NodeHandle livox_node;
livox_ros::DriverNode livox_node;
DRIVER_INFO(livox_node, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING);
/** Init default system parameter */
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 10.0; /* Hz */
int output_type = kOutputToRos;
std::string frame_id = "livox_frame";
bool lidar_bag = true;
bool imu_bag = false;
livox_node.GetNode().getParam("xfer_format", xfer_format);
livox_node.GetNode().getParam("multi_topic", multi_topic);
livox_node.GetNode().getParam("data_src", data_src);
livox_node.GetNode().getParam("publish_freq", publish_freq);
livox_node.GetNode().getParam("output_data_type", output_type);
livox_node.GetNode().getParam("frame_id", frame_id);
livox_node.GetNode().getParam("enable_lidar_bag", lidar_bag);
livox_node.GetNode().getParam("enable_imu_bag", imu_bag);
printf("data source:%u.\n", data_src);
if (publish_freq > 100.0) {
publish_freq = 100.0;
} else if (publish_freq < 0.5) {
publish_freq = 0.5;
} else {
publish_freq = publish_freq;
}
livox_node.future_ = livox_node.exit_signal_.get_future();
/** Lidar data distribute control and lidar data source set */
livox_node.lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src, output_type,
publish_freq, frame_id, lidar_bag, imu_bag);
livox_node.lddc_ptr_->SetRosNode(&livox_node);
if (data_src == kSourceRawLidar) {
DRIVER_INFO(livox_node, "Data Source is raw lidar.");
std::string user_config_path;
livox_node.getParam("user_config_path", user_config_path);
DRIVER_INFO(livox_node, "Config file : %s", user_config_path.c_str());
LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq);
livox_node.lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar));
if ((read_lidar->InitLdsLidar(user_config_path))) {
DRIVER_INFO(livox_node, "Init lds lidar successfully!");
} else {
DRIVER_ERROR(livox_node, "Init lds lidar failed!");
}
} else if (data_src == kSourceLvxFile) {
DRIVER_INFO(livox_node, "Data Source is lvx2 file.");
std::string cmdline_file_path;
livox_node.getParam("cmdline_file_path", cmdline_file_path);
DRIVER_INFO(livox_node, "lvx2 file : %s", cmdline_file_path.c_str());
do {
if (!IsFilePathValid(cmdline_file_path.c_str())) {
ROS_ERROR("File path invalid : %s !", cmdline_file_path.c_str());
break;
}
std::string rosbag_file_path;
int path_end_pos = cmdline_file_path.find_last_of('.');
rosbag_file_path = cmdline_file_path.substr(0, path_end_pos);
rosbag_file_path += ".bag";
LdsLvx *read_lvx = LdsLvx::GetInstance(publish_freq);
livox_node.lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lvx));
livox_node.lddc_ptr_->CreateBagFile(rosbag_file_path);
if ((read_lvx->Init(cmdline_file_path.c_str()))) {
DRIVER_INFO(livox_node, "Init lds lvx successfully!");
} else {
DRIVER_ERROR(livox_node, "Init lds lvx failed!");
}
} while (0);
} else {
DRIVER_ERROR(livox_node, "Invalid data src (%d), please check the launch file", data_src);
livox_node.lddc_ptr_->PrepareExit();
livox_node.exit_signal_.set_value();
}
livox_node.pointclouddata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::PointCloudDataPollThread, &livox_node);
livox_node.imudata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, &livox_node);
while (ros::ok()) { usleep(10000); }
return 0;
}
#elif defined BUILDING_ROS2
namespace livox_ros
{
DriverNode::DriverNode(const rclcpp::NodeOptions & node_options)
: Node("livox_driver_node", node_options)
{
DRIVER_INFO(*this, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING);
/** Init default system parameter */
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 10.0; /* Hz */
int output_type = kOutputToRos;
std::string frame_id;
this->declare_parameter("xfer_format", xfer_format);
this->declare_parameter("multi_topic", 0);
this->declare_parameter("data_src", data_src);
this->declare_parameter("publish_freq", 10.0);
this->declare_parameter("output_data_type", output_type);
this->declare_parameter("frame_id", "frame_default");
this->declare_parameter("user_config_path", "path_default");
this->declare_parameter("cmdline_input_bd_code", "000000000000001");
this->declare_parameter("lvx_file_path", "/home/livox/livox_test.lvx");
this->get_parameter("xfer_format", xfer_format);
this->get_parameter("multi_topic", multi_topic);
this->get_parameter("data_src", data_src);
this->get_parameter("publish_freq", publish_freq);
this->get_parameter("output_data_type", output_type);
this->get_parameter("frame_id", frame_id);
if (publish_freq > 100.0) {
publish_freq = 100.0;
} else if (publish_freq < 0.5) {
publish_freq = 0.5;
} else {
publish_freq = publish_freq;
}
future_ = exit_signal_.get_future();
/** Lidar data distribute control and lidar data source set */
lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id);
lddc_ptr_->SetRosNode(this);
if (data_src == kSourceRawLidar) {
DRIVER_INFO(*this, "Data Source is raw lidar.");
std::string user_config_path;
this->get_parameter("user_config_path", user_config_path);
DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str());
std::string cmdline_bd_code;
this->get_parameter("cmdline_input_bd_code", cmdline_bd_code);
LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq);
lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar));
if ((read_lidar->InitLdsLidar(user_config_path))) {
DRIVER_INFO(*this, "Init lds lidar success!");
} else {
DRIVER_ERROR(*this, "Init lds lidar fail!");
}
} else {
DRIVER_ERROR(*this, "Invalid data src (%d), please check the launch file", data_src);
}
pointclouddata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::PointCloudDataPollThread, this);
imudata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, this);
}
} // namespace livox_ros
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(livox_ros::DriverNode)
#endif // defined BUILDING_ROS2
void DriverNode::PointCloudDataPollThread()
{
std::future_status status;
std::this_thread::sleep_for(std::chrono::seconds(3));
do {
lddc_ptr_->DistributePointCloudData();
status = future_.wait_for(std::chrono::seconds(0));
} while (status == std::future_status::timeout);
}
void DriverNode::ImuDataPollThread()
{
std::future_status status;
std::this_thread::sleep_for(std::chrono::seconds(3));
do {
lddc_ptr_->DistributeImuData();
status = future_.wait_for(std::chrono::seconds(0));
} while (status == std::future_status::timeout);
}
- lvx_file.h
#include <string>
#include <iostream>
#include <sstream>
#include <cstdint>
#include <vector>
#include <fstream>
#define FRM_POINTS_COUNT 45216
#pragma pack(push, 1)
class PubHeader
{
public:
PubHeader()
{
}
~PubHeader()
{
}
char singnate[16];
char ver[4];
std::uint32_t magic_doce;
};
class PriHeader
{
public:
PriHeader()
{
}
~PriHeader()
{
}
std::uint32_t frame_duration;
std::uint8_t device_count;
};
class DeviceInfo
{
public:
DeviceInfo()
{
}
~DeviceInfo()
{
}
std::uint8_t lidar_broadcast_code[16];
std::uint8_t hub_brocast_code[16];
std::uint32_t lidar_id;
std::uint8_t lidar_type;
std::uint8_t device_type;
std::uint8_t extrinsic_enable;
float roll;
float pitch;
float yaw;
float x;
float y;
float z;
};
class FrameHeader
{
public:
FrameHeader()
{
}
~FrameHeader()
{
}
std::uint64_t curr_offset;
std::uint64_t next_offset;
std::uint64_t frame_idx;
};
class BasePackHeader
{
public:
BasePackHeader()
{
}
~BasePackHeader()
{
}
std::uint8_t version;
std::uint32_t lidar_id;
std::uint8_t lidar_type;
std::uint8_t timestamp_type;
std::uint8_t timestamp[8];
std::uint16_t udp_counter;
std::uint8_t data_type;
std::uint32_t length;
std::uint8_t frame_counter;
std::uint8_t reserve[4];
};
class ExtendRowPoint
{
public:
ExtendRowPoint()
{
}
~ExtendRowPoint()
{
}
std::int32_t x;
std::int32_t y;
std::int32_t z;
std::uint8_t reflectivity;
std::uint8_t tag;
};
class ExtendHalfRowPoint
{
public:
ExtendHalfRowPoint()
{
}
~ExtendHalfRowPoint()
{
}
std::int16_t x;
std::int16_t y;
std::int16_t z;
std::uint8_t reflectivity;
std::uint8_t tag;
};
class BasePackDetail
{
public:
BasePackDetail()
{
}
~BasePackDetail()
{
}
BasePackHeader header;
std::vector<ExtendRowPoint> raw_point;
};
class BaseHalfPackDetail
{
public:
BaseHalfPackDetail()
{
}
~BaseHalfPackDetail()
{
}
BasePackHeader header;
std::vector<ExtendHalfRowPoint> raw_point;
};
#pragma pack(pop)
It's not working, what's wrong?
- lds_lvx.cpp
// // The MIT License (MIT) // // Copyright (c) 2022 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 <unistd.h> #include <string.h> #include <functional> #include <memory> #include <thread> #include <condition_variable> #include <mutex> #include "lds_lvx.h" #include "lvx_file.h" #include "comm/pub_handler.h" #include <unistd.h> #include <sys/param.h> namespace livox_ros { // std::condition_variable LdsLvx::cv_; // std::mutex LdsLvx::mtx_; std::atomic_bool LdsLvx::is_file_end_(false); LdsLvx::LdsLvx(double publish_freq) : Lds(publish_freq, kSourceLvxFile), is_initialized_(false) { } LdsLvx::~LdsLvx() { } int LdsLvx::Init(const char *lvx_path) { if (is_initialized_) { printf("Livox file data source is already inited!\n"); return false; } #ifdef BUILDING_ROS2 DisableLivoxSdkConsoleLogger(); #endif printf("Lds lvx init lvx_path:%s.\n", lvx_path); inf_lvx2_.open(lvx_path, std::ios_base::binary); if (!inf_lvx2_) { char path[MAXPATHLEN]; char *p = getcwd (path, sizeof(path)); printf("Open %s @ %s file fail!\n", lvx_path, p); return false; } inf_lvx2_.seekg( 0, std::ios_base::end ); total_frame_ = inf_lvx2_.tellg(); inf_lvx2_.seekg( 0, std::ios_base::beg ); ResetLds(kSourceLvxFile); PubHeader pubheader; inf_lvx2_.read((char*)(&pubheader), sizeof(pubheader)); PriHeader priheader; inf_lvx2_.read((char*)(&priheader), sizeof(priheader)); uint32_t valid_lidar_count_ = (int)priheader.device_count; if (!valid_lidar_count_ || (valid_lidar_count_ >= kMaxSourceLidar)) { inf_lvx2_.close(); printf("Lidar count error in %s : %d\n", lvx_path, valid_lidar_count_); return false; } printf("LvxFile[%s] have %d lidars\n", lvx_path, valid_lidar_count_); for (uint32_t i = 0; i < valid_lidar_count_; i++) { DeviceInfo devinfo; inf_lvx2_.read((char*)(&devinfo), sizeof(devinfo)); uint32_t handle = devinfo.lidar_id; uint8_t index = 0; int8_t ret = cache_index_.GetFreeIndex(kLivoxLidarType, handle, index); if (ret != 0) { std::cout << "failed to get free index, lidar ip: " << IpNumToString(handle) << std::endl; continue; } LidarDevice& lidar = lidars_[index]; lidar.lidar_type = kLivoxLidarType; lidar.connect_state = kConnectStateSampling; lidar.handle = handle; printf("lidar.lidar_type: %d; devinfo.lidar_type: %d, device_type:%d\n", lidar.lidar_type, devinfo.lidar_type, devinfo.device_type); LidarExtParameter lidar_param; lidar_param.handle = handle; lidar_param.lidar_type = kLivoxLidarType; lidar_param.param.roll = devinfo.roll; lidar_param.param.pitch = devinfo.pitch; lidar_param.param.yaw = devinfo.yaw; lidar_param.param.x = devinfo.x; lidar_param.param.y = devinfo.y; lidar_param.param.z = devinfo.z; pub_handler().AddLidarsExtParam(lidar_param); } t_read_lvx_ = std::make_shared<std::thread>(std::bind(&LdsLvx::ReadLvxFile, this)); is_initialized_ = true; StartRead(); return true; } void LdsLvx::OnPointCloudsFrameCallback(uint32_t frame_index, uint32_t total_frame, PointFrame *point_cloud_frame, void *client_data) { if (!point_cloud_frame) { printf("Point clouds frame call back failed, point cloud frame is nullptr.\n"); return; } LdsLvx* lds = static_cast<LdsLvx*>(client_data); if (lds == nullptr) { printf("Point clouds frame call back failed, client data is nullptr.\n"); return; } lds->StorageLvxPointData(point_cloud_frame); if (frame_index == total_frame) { is_file_end_.store(true); } } void LdsLvx::ReadLvxFile() { while (!start_read_lvx_); printf("Start to read lvx file.\n"); uint8_t line_num = kLineNumberHAP; auto start_time = std::chrono::high_resolution_clock::now(); int frame_cnt = 0; while (!inf_lvx2_.eof()) { FrameHeader fraheader; inf_lvx2_.read((char*)(&fraheader), sizeof(fraheader)); // std::cout << "Frm curr offset:" << fraheader.curr_offset << std::endl; // std::cout << "Next offset:" << fraheader.next_offset << std::endl; // std::cout << "Frm idx:" << fraheader.frame_idx << std::endl; int bindx = 0; while (bindx < (int)(fraheader.next_offset - fraheader.curr_offset - sizeof(fraheader))) { BasePackHeader pheader; inf_lvx2_.read((char*)(&pheader), sizeof(pheader)); bindx += sizeof(pheader); PointFrame point_cloud_frame; std::vector<PointXyzlt> points_clouds; point_cloud_frame.lidar_num = 1; PointPacket &pkt = point_cloud_frame.lidar_point[0]; pkt.handle = pheader.lidar_id; pkt.lidar_type = LidarProtoType::kLivoxLidarType; // std::cout << "Data type:" << (int)pheader.data_type << std::endl; // std::cout << "Length:" << pheader.length << std::endl; // std::cout << "Frm counter:" << (int)pheader.frame_counter << std::endl; //long long int ts = *reinterpret_cast<long long int*>(&pheader.timestamp[0]); //point_cloud_frame.base_time = ts; point_cloud_frame.base_time = std::chrono::high_resolution_clock::now().time_since_epoch().count(); // printf("timestampe: %lld\n", ts); if (pheader.data_type == 1) { int pcount = pheader.length / 14; points_clouds.resize(pcount); pkt.points_num = pcount; pkt.points = points_clouds.data(); //printf("pcount: %d\n", pcount); for (int i = 0; i < pcount; i++) { ExtendRowPoint pdetail; inf_lvx2_.read((char*)(&pdetail), sizeof(pdetail)); PointXyzlt &p = points_clouds[i]; p.x = pdetail.x * 0.001f; p.y = pdetail.y * 0.001f; p.z = pdetail.z * 0.001f; p.intensity = pdetail.reflectivity; p.line = i % line_num; p.tag = pdetail.tag; //p.offset_time = ts + i; p.offset_time = std::chrono::high_resolution_clock::now().time_since_epoch().count(); /* fdata.push_back(pdetail.x * 0.001f); fdata.push_back(pdetail.y * 0.001f); fdata.push_back(pdetail.z * 0.001f); fdata.push_back(pdetail.reflectivity); fdata.push_back(pdetail.tag); */ } } else if (pheader.data_type == 2) { int pcount = pheader.length / 8; points_clouds.resize(pcount); //printf("pcount: %d\n", pcount); for (int i = 0; i < pcount; i++) { ExtendHalfRowPoint pdetail; inf_lvx2_.read((char*)(&pdetail), sizeof(pdetail)); /* fdata.push_back(pdetail.x * 0.01f); fdata.push_back(pdetail.y * 0.01f); fdata.push_back(pdetail.z * 0.01f); fdata.push_back(pdetail.reflectivity); fdata.push_back(pdetail.tag); */ } } else { std::cout << "Error:Can not surport this data type---" << (int)pheader.data_type << std::endl; break; } StorageLvxPointData(&point_cloud_frame); bindx += pheader.length; } ++frame_cnt; auto diff = std::chrono::milliseconds(50) * frame_cnt + start_time - std::chrono::high_resolution_clock::now(); if(diff > std::chrono::milliseconds(0)) { std::this_thread::sleep_for(diff); } } printf("done\n"); inf_lvx2_.close(); } } // namespace livox_ros
- lds_lvx.h
// // The MIT License (MIT) // // Copyright (c) 2022 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. // // livox lidar lvx data source #ifndef LIVOX_ROS_DRIVER_LDS_LVX_H_ #define LIVOX_ROS_DRIVER_LDS_LVX_H_ #include <memory> #include <atomic> #include <fstream> #include "lds.h" #include "comm/comm.h" #include "livox_lidar_api.h" #include "livox_lidar_def.h" namespace livox_ros { /** * Lidar data source abstract. */ class LdsLvx final : public Lds { public: static LdsLvx *GetInstance(double publish_freq) { static LdsLvx lds_lvx(publish_freq); return &lds_lvx; } int Init(const char *lvx_path); void ReadLvxFile(); private: LdsLvx(double publish_freq); LdsLvx(const LdsLvx &) = delete; ~LdsLvx(); LdsLvx &operator=(const LdsLvx &) = delete; void StartRead() { start_read_lvx_ = true; } void StopRead() { start_read_lvx_ = false; } bool IsStarted() { return start_read_lvx_; } static void OnPointCloudsFrameCallback(uint32_t frame_index, uint32_t total_frame, PointFrame *point_cloud_frame, void *client_data); private: volatile bool is_initialized_; std::ifstream inf_lvx2_; uint32_t total_frame_; std::shared_ptr<std::thread> t_read_lvx_; volatile bool start_read_lvx_; static std::atomic_bool is_file_end_; }; } // namespace livox_ros #endif
- livox_ros_driver2.cpp
// // The MIT License (MIT) // // Copyright (c) 2022 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 <iostream> #include <chrono> #include <vector> #include <csignal> #include <thread> #include "include/livox_ros_driver2.h" #include "include/ros_headers.h" #include "driver_node.h" #include "lddc.h" #include "lds_lidar.h" #include "lds_lvx.h" using namespace livox_ros; #ifdef BUILDING_ROS1 int main(int argc, char **argv) { /** Ros related */ if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged(); } ros::init(argc, argv, "livox_lidar_publisher"); // ros::NodeHandle livox_node; livox_ros::DriverNode livox_node; DRIVER_INFO(livox_node, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING); /** Init default system parameter */ int xfer_format = kPointCloud2Msg; int multi_topic = 0; int data_src = kSourceRawLidar; double publish_freq = 10.0; /* Hz */ int output_type = kOutputToRos; std::string frame_id = "livox_frame"; bool lidar_bag = true; bool imu_bag = false; livox_node.GetNode().getParam("xfer_format", xfer_format); livox_node.GetNode().getParam("multi_topic", multi_topic); livox_node.GetNode().getParam("data_src", data_src); livox_node.GetNode().getParam("publish_freq", publish_freq); livox_node.GetNode().getParam("output_data_type", output_type); livox_node.GetNode().getParam("frame_id", frame_id); livox_node.GetNode().getParam("enable_lidar_bag", lidar_bag); livox_node.GetNode().getParam("enable_imu_bag", imu_bag); printf("data source:%u.\n", data_src); if (publish_freq > 100.0) { publish_freq = 100.0; } else if (publish_freq < 0.5) { publish_freq = 0.5; } else { publish_freq = publish_freq; } livox_node.future_ = livox_node.exit_signal_.get_future(); /** Lidar data distribute control and lidar data source set */ livox_node.lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id, lidar_bag, imu_bag); livox_node.lddc_ptr_->SetRosNode(&livox_node); if (data_src == kSourceRawLidar) { DRIVER_INFO(livox_node, "Data Source is raw lidar."); std::string user_config_path; livox_node.getParam("user_config_path", user_config_path); DRIVER_INFO(livox_node, "Config file : %s", user_config_path.c_str()); LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq); livox_node.lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar)); if ((read_lidar->InitLdsLidar(user_config_path))) { DRIVER_INFO(livox_node, "Init lds lidar successfully!"); } else { DRIVER_ERROR(livox_node, "Init lds lidar failed!"); } } else if (data_src == kSourceLvxFile) { DRIVER_INFO(livox_node, "Data Source is lvx2 file."); std::string cmdline_file_path; livox_node.getParam("cmdline_file_path", cmdline_file_path); DRIVER_INFO(livox_node, "lvx2 file : %s", cmdline_file_path.c_str()); do { if (!IsFilePathValid(cmdline_file_path.c_str())) { ROS_ERROR("File path invalid : %s !", cmdline_file_path.c_str()); break; } std::string rosbag_file_path; int path_end_pos = cmdline_file_path.find_last_of('.'); rosbag_file_path = cmdline_file_path.substr(0, path_end_pos); rosbag_file_path += ".bag"; LdsLvx *read_lvx = LdsLvx::GetInstance(publish_freq); livox_node.lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lvx)); livox_node.lddc_ptr_->CreateBagFile(rosbag_file_path); if ((read_lvx->Init(cmdline_file_path.c_str()))) { DRIVER_INFO(livox_node, "Init lds lvx successfully!"); } else { DRIVER_ERROR(livox_node, "Init lds lvx failed!"); } } while (0); } else { DRIVER_ERROR(livox_node, "Invalid data src (%d), please check the launch file", data_src); livox_node.lddc_ptr_->PrepareExit(); livox_node.exit_signal_.set_value(); } livox_node.pointclouddata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::PointCloudDataPollThread, &livox_node); livox_node.imudata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, &livox_node); while (ros::ok()) { usleep(10000); } return 0; } #elif defined BUILDING_ROS2 namespace livox_ros { DriverNode::DriverNode(const rclcpp::NodeOptions & node_options) : Node("livox_driver_node", node_options) { DRIVER_INFO(*this, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING); /** Init default system parameter */ int xfer_format = kPointCloud2Msg; int multi_topic = 0; int data_src = kSourceRawLidar; double publish_freq = 10.0; /* Hz */ int output_type = kOutputToRos; std::string frame_id; this->declare_parameter("xfer_format", xfer_format); this->declare_parameter("multi_topic", 0); this->declare_parameter("data_src", data_src); this->declare_parameter("publish_freq", 10.0); this->declare_parameter("output_data_type", output_type); this->declare_parameter("frame_id", "frame_default"); this->declare_parameter("user_config_path", "path_default"); this->declare_parameter("cmdline_input_bd_code", "000000000000001"); this->declare_parameter("lvx_file_path", "/home/livox/livox_test.lvx"); this->get_parameter("xfer_format", xfer_format); this->get_parameter("multi_topic", multi_topic); this->get_parameter("data_src", data_src); this->get_parameter("publish_freq", publish_freq); this->get_parameter("output_data_type", output_type); this->get_parameter("frame_id", frame_id); if (publish_freq > 100.0) { publish_freq = 100.0; } else if (publish_freq < 0.5) { publish_freq = 0.5; } else { publish_freq = publish_freq; } future_ = exit_signal_.get_future(); /** Lidar data distribute control and lidar data source set */ lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id); lddc_ptr_->SetRosNode(this); if (data_src == kSourceRawLidar) { DRIVER_INFO(*this, "Data Source is raw lidar."); std::string user_config_path; this->get_parameter("user_config_path", user_config_path); DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str()); std::string cmdline_bd_code; this->get_parameter("cmdline_input_bd_code", cmdline_bd_code); LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq); lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar)); if ((read_lidar->InitLdsLidar(user_config_path))) { DRIVER_INFO(*this, "Init lds lidar success!"); } else { DRIVER_ERROR(*this, "Init lds lidar fail!"); } } else { DRIVER_ERROR(*this, "Invalid data src (%d), please check the launch file", data_src); } pointclouddata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::PointCloudDataPollThread, this); imudata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, this); } } // namespace livox_ros #include <rclcpp_components/register_node_macro.hpp> RCLCPP_COMPONENTS_REGISTER_NODE(livox_ros::DriverNode) #endif // defined BUILDING_ROS2 void DriverNode::PointCloudDataPollThread() { std::future_status status; std::this_thread::sleep_for(std::chrono::seconds(3)); do { lddc_ptr_->DistributePointCloudData(); status = future_.wait_for(std::chrono::seconds(0)); } while (status == std::future_status::timeout); } void DriverNode::ImuDataPollThread() { std::future_status status; std::this_thread::sleep_for(std::chrono::seconds(3)); do { lddc_ptr_->DistributeImuData(); status = future_.wait_for(std::chrono::seconds(0)); } while (status == std::future_status::timeout); }
- lvx_file.h
#include <string> #include <iostream> #include <sstream> #include <cstdint> #include <vector> #include <fstream> #define FRM_POINTS_COUNT 45216 #pragma pack(push, 1) class PubHeader { public: PubHeader() { } ~PubHeader() { } char singnate[16]; char ver[4]; std::uint32_t magic_doce; }; class PriHeader { public: PriHeader() { } ~PriHeader() { } std::uint32_t frame_duration; std::uint8_t device_count; }; class DeviceInfo { public: DeviceInfo() { } ~DeviceInfo() { } std::uint8_t lidar_broadcast_code[16]; std::uint8_t hub_brocast_code[16]; std::uint32_t lidar_id; std::uint8_t lidar_type; std::uint8_t device_type; std::uint8_t extrinsic_enable; float roll; float pitch; float yaw; float x; float y; float z; }; class FrameHeader { public: FrameHeader() { } ~FrameHeader() { } std::uint64_t curr_offset; std::uint64_t next_offset; std::uint64_t frame_idx; }; class BasePackHeader { public: BasePackHeader() { } ~BasePackHeader() { } std::uint8_t version; std::uint32_t lidar_id; std::uint8_t lidar_type; std::uint8_t timestamp_type; std::uint8_t timestamp[8]; std::uint16_t udp_counter; std::uint8_t data_type; std::uint32_t length; std::uint8_t frame_counter; std::uint8_t reserve[4]; }; class ExtendRowPoint { public: ExtendRowPoint() { } ~ExtendRowPoint() { } std::int32_t x; std::int32_t y; std::int32_t z; std::uint8_t reflectivity; std::uint8_t tag; }; class ExtendHalfRowPoint { public: ExtendHalfRowPoint() { } ~ExtendHalfRowPoint() { } std::int16_t x; std::int16_t y; std::int16_t z; std::uint8_t reflectivity; std::uint8_t tag; }; class BasePackDetail { public: BasePackDetail() { } ~BasePackDetail() { } BasePackHeader header; std::vector<ExtendRowPoint> raw_point; }; class BaseHalfPackDetail { public: BaseHalfPackDetail() { } ~BaseHalfPackDetail() { } BasePackHeader header; std::vector<ExtendHalfRowPoint> raw_point; }; #pragma pack(pop)
it maybe has some difference in file header between lvx and lvx2
