mirrors/livox_ros_driver
1
0
Fork 0
mirror of https://github.com/Livox-SDK/livox_ros_driver.git synced 2023-04-06 15:49:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat:add lidar bag or imu bag parameter

Browse Source 
1. fix corner case bug when covert lvx file to rosbag;
2. add lidar bag and imu bag parameter for the convertion of lvx file to rosbag;
This commit is contained in:
Livox-SDK
2020-11-25 14:59:41 +08:00
parent 12c709c0c9
commit 286827827d
15 changed files with 131 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
livox_ros_driver/launch/livox_hub.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,6 +23,8 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

4
livox_ros_driver/launch/livox_hub_msg.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,6 +23,8 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

6
livox_ros_driver/launch/livox_hub_rviz.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -20,7 +22,9 @@
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

6
livox_ros_driver/launch/livox_lidar.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,7 +23,9 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

6
livox_ros_driver/launch/livox_lidar_msg.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,7 +23,9 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

4
livox_ros_driver/launch/livox_lidar_rviz.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,6 +23,8 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

4
livox_ros_driver/launch/livox_template.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,6 +23,8 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

4
livox_ros_driver/launch/lvx_to_rosbag.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="false"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,6 +23,8 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

6
livox_ros_driver/launch/lvx_to_rosbag_rviz.launch
View File

@@ -11,6 +11,8 @@
<arg name="rosbag_enable" default="true"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -21,7 +23,9 @@
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

26
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

@@ -43,13 +43,15 @@ namespace livox_ros {
/** Lidar Data Distribute Control--------------------------------------------*/
Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
double frq, std::string &frame_id)
double frq, std::string &frame_id, bool lidar_bag, bool imu_bag)
: transfer_format_(format),
use_multi_topic_(multi_topic),
data_src_(data_src),
output_type_(output_type),
publish_frq_(frq),
frame_id_(frame_id) {
frame_id_(frame_id),
enable_lidar_bag_(lidar_bag),
enable_imu_bag_(imu_bag) {
publish_period_ns_ = kNsPerSecond / publish_frq_;
lds_ = nullptr;
memset(private_pub_, 0, sizeof(private_pub_));
@@ -239,9 +241,9 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
if (kOutputToRos == output_type_) {
p_publisher->publish(cloud);
} else {
if (bag_) {
if (bag_ && enable_lidar_bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
cloud);
cloud);
}
}
if (!lidar->data_is_pubulished) {
@@ -340,9 +342,9 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
if (kOutputToRos == output_type_) {
p_publisher->publish(cloud);
} else {
if (bag_) {
if (bag_ && enable_lidar_bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
cloud);
cloud);
}
}
if (!lidar->data_is_pubulished) {
@@ -464,7 +466,7 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
if (kOutputToRos == output_type_) {
p_publisher->publish(livox_msg);
} else {
if (bag_) {
if (bag_ && enable_lidar_bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
livox_msg);
}
@@ -513,9 +515,9 @@ uint32_t Lddc::PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
if (kOutputToRos == output_type_) {
p_publisher->publish(imu_data);
} else {
if (bag_) {
if (bag_ && enable_imu_bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
imu_data);
imu_data);
}
}
return published_packet;
@@ -592,9 +594,10 @@ ros::Publisher *Lddc::GetCurrentPublisher(uint8_t handle) {
if (use_multi_topic_) {
pub = &private_pub_[handle];
queue_size = queue_size / 8; // queue size is 4 for only one lidar
} else {
pub = &global_pub_;
queue_size = queue_size * 8;
queue_size = queue_size * 8; // shared queue size is 256, for all lidars
}
if (*pub == nullptr) {
@@ -640,9 +643,10 @@ ros::Publisher *Lddc::GetCurrentImuPublisher(uint8_t handle) {
if (use_multi_topic_) {
pub = &private_imu_pub_[handle];
queue_size = queue_size / 8; // queue size is 4 for only one lidar
} else {
pub = &global_imu_pub_;
queue_size = queue_size * 4;
queue_size = queue_size * 8; // shared queue size is 256, for all lidars
}
if (*pub == nullptr) {

4
livox_ros_driver/livox_ros_driver/lddc.h
View File

@@ -47,7 +47,7 @@ typedef enum {
class Lddc {
public:
Lddc(int format, int multi_topic, int data_src, int output_type, double frq,
std::string &frame_id);
std::string &frame_id, bool lidar_bag, bool imu_bag);
~Lddc();
int RegisterLds(Lds *lds);
@@ -94,6 +94,8 @@ class Lddc {
double publish_frq_;
uint32_t publish_period_ns_;
std::string frame_id_;
bool enable_lidar_bag_;
bool enable_imu_bag_;
ros::Publisher *private_pub_[kMaxSourceLidar];
ros::Publisher *global_pub_;
ros::Publisher *private_imu_pub_[kMaxSourceLidar];

60
livox_ros_driver/livox_ros_driver/lds.cpp
View File

@@ -43,6 +43,35 @@ bool IsFilePathValid(const char *path_str) {
}
}
uint64_t RawLdsStampToNs(LdsStamp &timestamp, uint8_t timestamp_type) {
if (timestamp_type == kTimestampTypePps) {
return timestamp.stamp;
} else if (timestamp_type == kTimestampTypeNoSync) {
return timestamp.stamp;
} else if (timestamp_type == kTimestampTypePtp) {
return timestamp.stamp;
} else if (timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = timestamp.stamp_bytes[0] + 100; // map 2000 to 1990
time_utc.tm_mon = timestamp.stamp_bytes[1] - 1; // map 1~12 to 0~11
time_utc.tm_mday = timestamp.stamp_bytes[2];
time_utc.tm_hour = timestamp.stamp_bytes[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
// uint64_t time_epoch = mktime(&time_utc);
uint64_t time_epoch = timegm(&time_utc); // no timezone
time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high; // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
printf("Timestamp type[%d] invalid.\n", timestamp_type);
return 0;
}
}
uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src) {
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
@@ -86,6 +115,7 @@ uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint8_t product_type,
uint32_t queue_size =
(interval_ms * GetPacketNumPerSec(product_type, data_type)) / 1000;
queue_size = queue_size * 2;
if (queue_size < kMinEthPacketQueueSize) {
queue_size = kMinEthPacketQueueSize;
} else if (queue_size > kMaxEthPacketQueueSize) {
@@ -598,9 +628,9 @@ void Lds::UpdateLidarInfoByEthPacket(LidarDevice *p_lidar, \
p_lidar->onetime_publish_packets = \
GetPacketNumPerSec(p_lidar->info.type, \
p_lidar->raw_data_type) * buffer_time_ms_ / 1000;
printf("DataType[%d] PacketInterval[%d] PublishPackets[%d]\n", \
p_lidar->raw_data_type, p_lidar->packet_interval, \
p_lidar->onetime_publish_packets);
printf("Lidar[%d][%s] DataType[%d] PacketInterval[%d] PublishPackets[%d]\n",
p_lidar->handle, p_lidar->info.broadcast_code, p_lidar->raw_data_type,
p_lidar->packet_interval, p_lidar->onetime_publish_packets);
}
}
@@ -608,8 +638,15 @@ void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
LidarDevice *p_lidar = &lidars_[handle];
LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
LdsStamp cur_timestamp;
uint64_t timestamp;
memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
sizeof(cur_timestamp));
timestamp = RawLdsStampToNs(cur_timestamp, eth_packet->timestamp_type);
if (timestamp >= kRosTimeMax) {
printf("Raw EthPacket time out of range Lidar[%d]\n", handle);
return;
}
if (kImu != eth_packet->data_type) {
UpdateLidarInfoByEthPacket(p_lidar, eth_packet);
@@ -629,15 +666,14 @@ void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(
buffer_time_ms_, p_lidar->info.type, eth_packet->data_type);
queue_size = queue_size * 8; /* 8 multiple the min size */
InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle,
printf("Lidar[%d][%s] storage queue size : %d %d\n", p_lidar->handle,
p_lidar->info.broadcast_code, queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet, \
GetEthPacketLen(eth_packet->data_type), \
packet_statistic->timebase, \
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type),
packet_statistic->timebase,
GetPointsPerPacket(eth_packet->data_type));
if (QueueUsedSize(p_queue) > p_lidar->onetime_publish_packets) {
if (semaphore_.GetCount() <= 0) {
@@ -662,13 +698,13 @@ void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = 256; /* fixed imu data queue size */
InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,
printf("Lidar[%d][%s] imu storage queue size : %d %d\n", p_lidar->handle,
p_lidar->info.broadcast_code, queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet, \
GetEthPacketLen(eth_packet->data_type),\
packet_statistic->imu_timebase, \
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type),
packet_statistic->imu_timebase,
GetPointsPerPacket(eth_packet->data_type));
}
}

4
livox_ros_driver/livox_ros_driver/lds.h
View File

@@ -48,7 +48,7 @@ const uint32_t kMaxSourceLidar = 32;
/** Eth packet relative info parama */
const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */
const uint32_t kMaxEthPacketQueueSize = 8192; /**< must be 2^n */
const uint32_t kMaxEthPacketQueueSize = 131072; /**< must be 2^n */
const uint32_t kImuEthPacketQueueSize = 256;
const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */
@@ -56,6 +56,7 @@ const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */
const uint32_t KCartesianPointSize = 13;
const uint32_t KSphericalPointSzie = 9;
const uint64_t kRosTimeMax = 4294967296000000000; /**< 2^32 * 1000000000ns */
const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */
/**< the threshold of packet continuous */
const int64_t kMaxPacketTimeGap = 1700000;
@@ -262,6 +263,7 @@ const ProductTypePointInfoPair product_type_info_pair_table[kMaxProductType] = {
* Global function for general use.
*/
bool IsFilePathValid(const char *path_str);
uint64_t RawLdsStampToNs(LdsStamp &timestamp, uint8_t timestamp_type);
uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src);
uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint8_t product_type,
uint8_t data_type);

18
livox_ros_driver/livox_ros_driver/lds_lvx.cpp
View File

@@ -120,11 +120,6 @@ int LdsLvx::InitLdsLvx(const char *lvx_path) {
p_extrinsic->trans[2] = lvx_dev_info.z;
EulerAnglesToRotationMatrix(p_extrinsic->euler, p_extrinsic->rotation);
p_extrinsic->enable = lvx_dev_info.extrinsic_enable;
uint32_t queue_size = kMaxEthPacketQueueSize * 16;
InitQueue(&lidars_[handle].data, queue_size);
queue_size = kMaxEthPacketQueueSize;
InitQueue(&lidars_[handle].imu_data, queue_size);
}
t_read_lvx_ =
@@ -166,6 +161,19 @@ void LdsLvx::ReadLvxFile() {
}
data_type = eth_packet->data_type;
if (handle >= lvx_file_->GetDeviceCount()) {
printf("Raw data handle error, error handle is %d\n", handle);
break;
}
if (data_type >= kMaxPointDataType) {
printf("Raw data type error, error data_type is %d\n", data_type);
break;
}
if (eth_packet->version != 5) {
printf("EthPacket version[%d] not supported\n", eth_packet->version);
break;
}
/** Packet length + device index */
data_offset += (GetEthPacketLen(data_type) + 1);
StorageRawPacket(handle, eth_packet);

14
livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
View File

@@ -69,9 +69,11 @@ int main(int argc, char **argv) {
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 10.0; /* Hz */
int output_type = kOutputToRos;
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.getParam("xfer_format", xfer_format);
livox_node.getParam("multi_topic", multi_topic);
@@ -79,17 +81,19 @@ int main(int argc, char **argv) {
livox_node.getParam("publish_freq", publish_freq);
livox_node.getParam("output_data_type", output_type);
livox_node.getParam("frame_id", frame_id);
livox_node.getParam("enable_lidar_bag", lidar_bag);
livox_node.getParam("enable_imu_bag", imu_bag);
if (publish_freq > 100.0) {
publish_freq = 100.0;
} else if (publish_freq < 1.0) {
publish_freq = 1.0;
} else if (publish_freq < 0.1) {
publish_freq = 0.1;
} else {
publish_freq = publish_freq;
}
/** Lidar data distribute control and lidar data source set */
Lddc *lddc = new Lddc(xfer_format, multi_topic, data_src, output_type,
publish_freq, frame_id);
publish_freq, frame_id, lidar_bag, imu_bag);
lddc->SetRosNode(&livox_node);
int ret = 0;