optimize:publish fixed points every pointcloud2 message

livox_ros_driver/livox_ros_driver/lddc.cpp

@@ -130,17 +130,23 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
     QueueProPop(queue, &storage_packet);
     LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
     
+    uint32_t packet_interval = GetPointInterval(raw_packet->data_type);
+    uint64_t packet_loss_threshold = packet_interval + packet_interval / 2;
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
     if (published_packet && \
-       ((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
+       ((timestamp - last_timestamp) > packet_loss_threshold)) {
        if (kSourceLvxFile != data_source) {
-        ROS_INFO("Lidar[%d] packet loss", handle);
+        ROS_INFO("Lidar[%d] packet loss");
+        point_base = FillZeroPointXyzrtl(point_base, storage_packet.point_num);
+        cloud.width += storage_packet.point_num; 
+        last_timestamp = last_timestamp + packet_interval;
+        ++published_packet;
         break;
       }
     }
 
-    if (!published_packet) {
+    if (!published_packet) { // use the first packet timestamp as pointcloud2 msg timestamp
-      cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
+      cloud.header.stamp = ros::Time(timestamp/1000000000.0);
     }
     cloud.width += storage_packet.point_num;
 
@@ -410,27 +416,17 @@ void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar) {
 
   while (!QueueIsEmpty(p_queue)) {
     uint32_t used_size = QueueUsedSize(p_queue);
-    uint32_t publish_packet_upper_limit = GetPacketNumPerSec(lidar->info.type);
+    uint32_t onetime_publish_packets = GetPacketNumPerSec(lidar->info.type) / publish_frq_;
-    uint32_t publish_packet_lower_limit = publish_packet_upper_limit / 2 / ((uint32_t)publish_frq_);
+    if (used_size < onetime_publish_packets) {
-    /** increase margin */
-    publish_packet_upper_limit = publish_packet_upper_limit + publish_packet_upper_limit / 10;
-    if (used_size < publish_packet_lower_limit) {
       break;
     }
 
-    if (used_size > publish_packet_upper_limit) {
-      used_size = publish_packet_upper_limit;
-    }
-
     if (kPointCloud2Msg == transfer_format_) {
-      if (used_size == PublishPointcloud2(p_queue, used_size, handle)) {
+      PublishPointcloud2(p_queue, onetime_publish_packets, handle);
-      }
     } else if (kLivoxCustomMsg == transfer_format_) {
-      if (used_size == PublishCustomPointcloud(p_queue, used_size, handle)) {
+      PublishCustomPointcloud(p_queue, onetime_publish_packets, handle);
-      }
     } else if (kPclPxyziMsg == transfer_format_) {
-      if (used_size == PublishPointcloudData(p_queue, used_size, handle)) {
+      PublishPointcloudData(p_queue, onetime_publish_packets, handle);
-      }
     }
   }
 }

livox_ros_driver/livox_ros_driver/lds.cpp

@@ -375,7 +375,23 @@ PointConvertHandler GetConvertHandler(uint8_t data_type) {
     return nullptr;
 }
 
+uint8_t* FillZeroPointXyzrtl(uint8_t* point_buf,  uint32_t num) {
+  LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
+  uint32_t points_per_packet  = num;
 
+  while(points_per_packet) {
+    dst_point->x = 0;
+    dst_point->y = 0;
+    dst_point->z = 0;
+    dst_point->reflectivity = 0;
+    dst_point->tag  = 0;
+    dst_point->line = 0;
+    ++dst_point;
+    --points_per_packet;
+  }
+
+  return (uint8_t *)dst_point;
+}
 
 #if 0
 

livox_ros_driver/livox_ros_driver/lds.h

@@ -179,8 +179,9 @@ typedef struct {
 
 typedef struct {
   uint32_t points_per_packet;
-  uint32_t points_per_second;  /**< unit:ns */
+  uint32_t points_per_second;
-  uint32_t point_interval;
+  uint32_t point_interval;     /**< unit:ns */
+  uint32_t packet_interval;    /**< unit:ns */
   uint32_t packet_length;
 } PacketInfoPair;
 
@@ -215,13 +216,13 @@ typedef uint8_t* (* PointConvertHandler)(uint8_t* point_buf, LivoxEthPacket* eth
                                          ExtrinsicParameter& extrinsic);
 
 const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = {
-  {100, 100000, 10000   , 1318},
+  {100, 100000, 10000   ,1000000   ,1318},
-  {100, 100000, 10000   , 918},
+  {100, 100000, 10000   ,1000000   ,918},
-  {96,  240000, 4167    , 1362},
+  {96,  240000, 4167    ,400000    ,1362},
-  {96,  240000, 4167    , 978},
+  {96,  240000, 4167    ,400000    ,978},
-  {96,  480000, 4167    , 1362},
+  {96,  480000, 4167    ,400000    ,1362},
-  {48,  480000, 4167    , 978},
+  {48,  480000, 4167    ,400000    ,978},
-  {1,   100,    10000000, 42}
+  {1,   100,    10000000,10000000  ,42}
 };
 
 /**
@@ -235,6 +236,7 @@ void ParseCommandlineInputBdCode(const char* cammandline_str,
 PointConvertHandler GetConvertHandler(uint8_t data_type);
 uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
                              ExtrinsicParameter& extrinsic);
+uint8_t* FillZeroPointXyzrtl(uint8_t* point_buf,  uint32_t num);
 uint8_t* LivoxImuDataProcess(uint8_t* point_buf,  LivoxEthPacket* eth_packet);
 void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix);
 void PointExtrisincCompensation(PointXyz* dst_point, ExtrinsicParameter& extrinsic);
@@ -252,6 +254,10 @@ inline uint32_t GetPointsPerPacket(uint32_t data_type) {
   return packet_info_pair_table[data_type].points_per_packet;
 }
 
+inline uint32_t GetPacketInterval(uint32_t data_type) {
+  return packet_info_pair_table[data_type].packet_interval;
+}
+
 inline uint32_t GetEthPacketLen(uint32_t data_type) {
   return packet_info_pair_table[data_type].packet_length;
 }

livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp

@@ -22,17 +22,18 @@
 // SOFTWARE.
 //
 
+#include "include/livox_ros_driver.h"
+
 #include <vector>
 #include <chrono>
 
-#include "livox_sdk.h"
 #include <ros/ros.h>
-
+#include "livox_sdk.h"
 #include "lds_lvx.h"
 #include "lds_lidar.h"
 #include "lds_hub.h"
 #include "lddc.h"
-#include "include/livox_ros_driver.h"
+
 
 using namespace livox_ros;