feat:add file boundary judgment when read lvx file

livox_ros_driver/launch/lvx_to_rosbag.launch

@@ -5,7 +5,7 @@
 	<arg name="xfer_format" default="0"/>
 	<arg name="multi_topic" default="0"/>
 	<arg name="data_src" default="2"/>
-	<arg name="publish_freq" default="2000.0"/>
+	<arg name="publish_freq" default="20.0"/>
 	<arg name="output_type" default="1"/>
 	<arg name="rviz_enable" default="false"/>
 	<arg name="rosbag_enable" default="false"/>

livox_ros_driver/livox_ros_driver/lddc.cpp

@@ -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)) {
+    if ((packet_gap > packet_loss_threshold_lower) && published_packet && \
-      ROS_INFO("Lidar[%d] packet loss", handle);
+        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 && \
+    if (((timestamp - last_timestamp) > kDeviceDisconnectThreshold) && \
-        ((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,7 +404,9 @@ uint32_t Lddc::PublishImuData(LidarDataQueue* queue, uint32_t packet_num,
   QueueProPop(queue, &storage_packet);
   LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
   timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
+  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);

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) {

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;

livox_ros_driver/livox_ros_driver/lds_lvx.cpp

@@ -187,8 +187,15 @@ void LdsLvx::ReadLvxFile() {
     }
   }
 
-  while(IsAllQueueEmpty()) {
+  int32_t wait_cnt = 10;
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  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; i<lidar_count_; i++) {
+    LidarDevice* p_lidar = &lidars_[i];
+    if (remain_size[i] != QueueIsEmpty(&p_lidar->data)) {
+      remain_size[i] = QueueIsEmpty(&p_lidar->data);
+      return false;
+    }
+  }
+
+  return true;
+}
+
 }

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

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();

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;
@@ -308,6 +309,13 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
     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;
   uint64_t read_length;
@@ -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;