diff --git a/livox_ros_driver/launch/livox_hub.launch b/livox_ros_driver/launch/livox_hub.launch
index 7923895..2231485 100644
--- a/livox_ros_driver/launch/livox_hub.launch
+++ b/livox_ros_driver/launch/livox_hub.launch
@@ -11,6 +11,8 @@
+
+
@@ -21,6 +23,8 @@
+
+
+
+
@@ -21,6 +23,8 @@
+
+
+
+
@@ -20,7 +22,9 @@
-
+
+
+
+
+
@@ -21,7 +23,9 @@
-
+
+
+
diff --git a/livox_ros_driver/launch/livox_lidar_msg.launch b/livox_ros_driver/launch/livox_lidar_msg.launch
index 6ca18f4..668c926 100644
--- a/livox_ros_driver/launch/livox_lidar_msg.launch
+++ b/livox_ros_driver/launch/livox_lidar_msg.launch
@@ -11,6 +11,8 @@
+
+
@@ -21,7 +23,9 @@
-
+
+
+
diff --git a/livox_ros_driver/launch/livox_lidar_rviz.launch b/livox_ros_driver/launch/livox_lidar_rviz.launch
index 067e2ad..d0a52e5 100644
--- a/livox_ros_driver/launch/livox_lidar_rviz.launch
+++ b/livox_ros_driver/launch/livox_lidar_rviz.launch
@@ -11,6 +11,8 @@
+
+
@@ -21,6 +23,8 @@
+
+
+
+
@@ -21,6 +23,8 @@
+
+
+
+
@@ -21,6 +23,8 @@
+
+
+
+
@@ -21,7 +23,9 @@
-
+
+
+
diff --git a/livox_ros_driver/livox_ros_driver/lddc.cpp b/livox_ros_driver/livox_ros_driver/lddc.cpp
index 047ec1a..3a21d85 100644
--- a/livox_ros_driver/livox_ros_driver/lddc.cpp
+++ b/livox_ros_driver/livox_ros_driver/lddc.cpp
@@ -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) {
diff --git a/livox_ros_driver/livox_ros_driver/lddc.h b/livox_ros_driver/livox_ros_driver/lddc.h
index 8936147..e09245a 100644
--- a/livox_ros_driver/livox_ros_driver/lddc.h
+++ b/livox_ros_driver/livox_ros_driver/lddc.h
@@ -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];
diff --git a/livox_ros_driver/livox_ros_driver/lds.cpp b/livox_ros_driver/livox_ros_driver/lds.cpp
index 9076bb6..07ed3dc 100644
--- a/livox_ros_driver/livox_ros_driver/lds.cpp
+++ b/livox_ros_driver/livox_ros_driver/lds.cpp
@@ -43,6 +43,35 @@ bool IsFilePathValid(const char *path_str) {
}
}
+uint64_t RawLdsStampToNs(LdsStamp ×tamp, 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(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));
}
}
diff --git a/livox_ros_driver/livox_ros_driver/lds.h b/livox_ros_driver/livox_ros_driver/lds.h
index 0bc0243..37a036f 100644
--- a/livox_ros_driver/livox_ros_driver/lds.h
+++ b/livox_ros_driver/livox_ros_driver/lds.h
@@ -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 ×tamp, 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);
diff --git a/livox_ros_driver/livox_ros_driver/lds_lvx.cpp b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp
index e484469..969e0ed 100644
--- a/livox_ros_driver/livox_ros_driver/lds_lvx.cpp
+++ b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp
@@ -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);
diff --git a/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp b/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
index f622825..2268f52 100644
--- a/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
+++ b/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
@@ -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;