ZhenpengChenCode commented 1 year ago

您好，请问，什么时候支持将lvx2的点云数据转成ros包？

Livox-Infra commented 1 year ago

后续会支持，可能会以一个单独小工具的形式提供转换功能，目前未完全确定方案

sukhrajklair commented 1 year ago

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.

arvrschool commented 1 year ago

I have the problem too.

cartooh commented 1 year ago

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

include "lds_lvx.h"

include "lvx_file.h"

include "comm/pub_handler.h"

include

include <sys/param.h>

namespace livox_ros {

// std::conditionvariable LdsLvx::cv; // std::mutex LdsLvx::mtx_; std::atomic_bool LdsLvx::is_fileend(false);

LdsLvx::LdsLvx(double publish_freq) : Lds(publish_freq, kSourceLvxFile), isinitialized(false) { }

LdsLvx::~LdsLvx() { }

int LdsLvx::Init(const char *lvx_path) { if (isinitialized) { 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); inflvx2.open(lvx_path, std::ios_base::binary); if (!inflvx2) { char path[MAXPATHLEN]; char *p = getcwd (path, sizeof(path)); printf("Open %s @ %s file fail!\n", lvx_path, p); return false; }

inflvx2.seekg( 0, std::ios_base::end ); totalframe = inflvx2.tellg(); inflvx2.seekg( 0, std::ios_base::beg );

ResetLds(kSourceLvxFile);

PubHeader pubheader; inflvx2.read((char*)(&pubheader), sizeof(pubheader));

PriHeader priheader; inflvx2.read((char*)(&priheader), sizeof(priheader));

uint32_t valid_lidarcount = (int)priheader.device_count; if (!valid_lidarcount || (valid_lidarcount >= kMaxSourceLidar)) { inflvx2.close(); printf("Lidar count error in %s : %d\n", lvx_path, valid_lidarcount); return false; } printf("LvxFile[%s] have %d lidars\n", lvx_path, valid_lidarcount);

for (uint32_t i = 0; i < valid_lidarcount; i++) { DeviceInfo devinfo; inflvx2.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_readlvx = std::make_shared(std::bind(&LdsLvx::ReadLvxFile, this));

isinitialized = 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_fileend.store(true); } }

void LdsLvx::ReadLvxFile() { while (!start_readlvx); 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 (!inflvx2.eof()) { FrameHeader fraheader; inflvx2.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"); inflvx2.close(); }

} // namespace livox_ros

- lds_lvx.h
```cpp: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

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

livoxnode.future = livox_node.exitsignal.get_future();

/* Lidar data distribute control and lidar data source set / livox_node.lddcptr = std::make_unique(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id, lidar_bag, imu_bag); livox_node.lddcptr->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.lddcptr->PrepareExit(); livox_node.exitsignal.set_value(); }

livox_node.pointclouddata_pollthread = std::make_shared(&DriverNode::PointCloudDataPollThread, &livox_node); livox_node.imudata_pollthread = std::make_shared(&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_ = exitsignal.get_future();

/* Lidar data distribute control and lidar data source set / lddcptr = std::make_unique(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id); lddcptr->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_pollthread = std::make_shared(&DriverNode::PointCloudDataPollThread, this); imudata_pollthread = std::make_shared(&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 { lddcptr->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 { lddcptr->DistributeImuData(); status = future_.wait_for(std::chrono::seconds(0)); } while (status == std::future_status::timeout); }

- lvx_file.h
```cpp: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)

NHMMing commented 1 year ago

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

sssolitude commented 7 months ago

一年过去了，有办法了嘛

Lixc23tsinghua commented 5 months ago

笑死了，我也遇到这个问题了。。。。mid360不配用目标检测

Livox-SDK / livox_ros_driver2

convert lvx2 to rosbag #9

include

include

include

include

include

include

include

include

include "lds_lvx.h"

include "lvx_file.h"

include "comm/pub_handler.h"

include

include <sys/param.h>

ifdef BUILDING_ROS2

endif

include

include

include

include

include

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"

ifdef BUILDING_ROS1

elif defined BUILDING_ROS2

include <rclcpp_components/register_node_macro.hpp>

endif // defined BUILDING_ROS2