diff --git a/livox_ros_driver/launch/lvx_to_rosbag.launch b/livox_ros_driver/launch/lvx_to_rosbag.launch
index 34c0ae6..6e2c01f 100644
--- a/livox_ros_driver/launch/lvx_to_rosbag.launch
+++ b/livox_ros_driver/launch/lvx_to_rosbag.launch
@@ -5,7 +5,7 @@
-
+
diff --git a/livox_ros_driver/livox_ros_driver/lddc.cpp b/livox_ros_driver/livox_ros_driver/lddc.cpp
index 351f214..ce02ae2 100644
--- a/livox_ros_driver/livox_ros_driver/lddc.cpp
+++ b/livox_ros_driver/livox_ros_driver/lddc.cpp
@@ -122,7 +122,7 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.fields[5].name = "line";
cloud.fields[5].count = 1;
cloud.fields[5].datatype = sensor_msgs::PointField::UINT8;
-
+
cloud.data.resize(packet_num * kMaxPointPerEthPacket * sizeof(LivoxPointXyzrtl));
cloud.point_step = sizeof(LivoxPointXyzrtl);
uint8_t *point_base = cloud.data.data();
@@ -136,9 +136,11 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
int64_t packet_gap = timestamp - last_timestamp;
- if (published_packet && (packet_gap > packet_loss_threshold_lower)) {
+ if (published_packet && (packet_gap > packet_loss_threshold_lower) && \
+ lds_->lidars_[handle].data_is_pubulished) {
+ ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
if (kSourceLvxFile != data_source) {
- ROS_INFO("Lidar[%d] packet loss %ld", handle, packet_loss_threshold_lower);
+ //ROS_INFO("Lidar[%d] packet loss %ld %d %d", handle, packet_loss_threshold_lower, packet_interval, raw_packet->data_type);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap > packet_loss_threshold_upper) { // skip when gap is too large
break;
@@ -190,6 +192,10 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
}
}
+ if (!lds_->lidars_[handle].data_is_pubulished) {
+ lds_->lidars_[handle].data_is_pubulished = true;
+ }
+
return published_packet;
}
@@ -216,8 +222,9 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
int64_t packet_gap = timestamp - last_timestamp;
- if (published_packet && (packet_gap > packet_loss_threshold_lower)) {
- ROS_INFO("Lidar[%d] packet loss", handle);
+ if ((packet_gap > packet_loss_threshold_lower) && published_packet && \
+ lds_->lidars_[handle].data_is_pubulished) {
+ ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap > packet_loss_threshold_upper) { // skip when gap is too large
break;
@@ -276,6 +283,10 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
}
}
+ if (!lds_->lidars_[handle].data_is_pubulished) {
+ lds_->lidars_[handle].data_is_pubulished = true;
+ }
+
return published_packet;
}
@@ -310,8 +321,8 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
}
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
- if (published_packet && \
- ((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) {
+ if (((timestamp - last_timestamp) > kDeviceDisconnectThreshold) && \
+ published_packet && lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss", handle);
break;
}
@@ -373,6 +384,10 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
}
}
+ if (!lds_->lidars_[handle].data_is_pubulished) {
+ lds_->lidars_[handle].data_is_pubulished = true;
+ }
+
return published_packet;
}
@@ -389,8 +404,10 @@ uint32_t Lddc::PublishImuData(LidarDataQueue* queue, uint32_t packet_num,
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast(storage_packet.raw_data);
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
- imu_data.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
-
+ if (timestamp >= 0) {
+ imu_data.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
+ }
+
uint8_t point_buf[2048];
LivoxImuDataProcess(point_buf, raw_packet);
diff --git a/livox_ros_driver/livox_ros_driver/lds.cpp b/livox_ros_driver/livox_ros_driver/lds.cpp
index 17b7ff6..21d3055 100644
--- a/livox_ros_driver/livox_ros_driver/lds.cpp
+++ b/livox_ros_driver/livox_ros_driver/lds.cpp
@@ -426,8 +426,9 @@ void Lds::ResetLidar(LidarDevice* lidar, uint8_t data_src) {
/** unallocated state */
lidar->handle = kMaxSourceLidar;
- lidar->connect_state = kConnectStateOff;
lidar->data_src = data_src;
+ lidar->data_is_pubulished = false;
+ lidar->connect_state = kConnectStateOff;
}
void Lds::SetLidarDataSrc(LidarDevice* lidar, uint8_t data_src) {
diff --git a/livox_ros_driver/livox_ros_driver/lds.h b/livox_ros_driver/livox_ros_driver/lds.h
index 65c9790..2a64a3c 100644
--- a/livox_ros_driver/livox_ros_driver/lds.h
+++ b/livox_ros_driver/livox_ros_driver/lds.h
@@ -167,6 +167,7 @@ typedef struct {
typedef struct {
uint8_t handle; /**< Lidar access handle. */
uint8_t data_src; /**< From raw lidar or livox file. */
+ bool data_is_pubulished; /**< Indicate the data of lidar whether is pubulished. */
volatile LidarConnectState connect_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
diff --git a/livox_ros_driver/livox_ros_driver/lds_lvx.cpp b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp
index 3fdb7d8..926f596 100644
--- a/livox_ros_driver/livox_ros_driver/lds_lvx.cpp
+++ b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp
@@ -187,8 +187,15 @@ void LdsLvx::ReadLvxFile() {
}
}
- while(IsAllQueueEmpty()) {
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ int32_t wait_cnt = 10;
+ while(!IsAllQueueEmpty()) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ if (IsAllQueueReadStop()) {
+ --wait_cnt;
+ if (wait_cnt <= 0) {
+ break;
+ }
+ }
}
RequestExit();
}
@@ -204,4 +211,17 @@ bool LdsLvx::IsAllQueueEmpty() {
return true;
}
+bool LdsLvx::IsAllQueueReadStop() {
+ static uint32_t remain_size[kMaxSourceLidar];
+ for (int i=0; idata)) {
+ remain_size[i] = QueueIsEmpty(&p_lidar->data);
+ return false;
+ }
+ }
+
+ return true;
+}
+
}
diff --git a/livox_ros_driver/livox_ros_driver/lds_lvx.h b/livox_ros_driver/livox_ros_driver/lds_lvx.h
index e130eb1..3998d79 100644
--- a/livox_ros_driver/livox_ros_driver/lds_lvx.h
+++ b/livox_ros_driver/livox_ros_driver/lds_lvx.h
@@ -61,6 +61,7 @@ class LdsLvx : public Lds {
void ReadLvxFile();
bool IsAllQueueEmpty();
+ bool IsAllQueueReadStop();
volatile bool is_initialized_;
OutPacketBuffer packets_of_frame_;
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 3a8e0ac..6e37d72 100644
--- a/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
+++ b/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
@@ -63,7 +63,7 @@ int main(int argc, char **argv) {
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
- double publish_freq = 50.0; /* Hz */
+ double publish_freq = 20.0; /* Hz */
int output_type = kOutputToRos;
livox_node.getParam("xfer_format", xfer_format);
@@ -149,7 +149,11 @@ int main(int argc, char **argv) {
}
ros::Time::init();
- ros::Rate r(publish_freq);
+ double poll_freq = publish_freq;
+ if (data_src == kSourceLvxFile) {
+ poll_freq = 2000;
+ }
+ ros::Rate r(poll_freq);
while (ros::ok()) {
lddc->DistributeLidarData();
r.sleep();
diff --git a/livox_ros_driver/livox_ros_driver/lvx_file.cpp b/livox_ros_driver/livox_ros_driver/lvx_file.cpp
index 069b76b..0fe2159 100644
--- a/livox_ros_driver/livox_ros_driver/lvx_file.cpp
+++ b/livox_ros_driver/livox_ros_driver/lvx_file.cpp
@@ -159,6 +159,7 @@ int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
size_ = lvx_file_.tellg();
lvx_file_.seekg (0, std::ios::beg);
+ printf("Filesize %lu\n", size_);
if (size_ < MiniFileSize()) {
state_ = kLvxFileSizeFault;
@@ -307,6 +308,13 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
state_ = kLvxFileAtEnd;
return kLvxFileAtEnd;
}
+
+ uint64_t tmp_size = lvx_file_.tellg();
+ if (tmp_size >= size_) {
+ printf("At the file end %lu\n", tmp_size);
+ state_ = kLvxFileAtEnd;
+ return kLvxFileAtEnd;
+ }
FrameHeader frame_header;
FrameHeaderV0 frame_header_v0;
@@ -334,7 +342,6 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
packets_of_frame->data_size = DataSizeOfFrame(frame_header_v0);
read_length = packets_of_frame->data_size;
}
-
lvx_file_.read((char *)(packets_of_frame->packet), read_length);
if (lvx_file_) {
return kLvxFileOk;