fixed bug : buffer don't have enough space (HesaiTechnology/HesaiLida…

…r_General_SDK#23) Signed-off-by: Masayuki Murata <[email protected]>
CMU-cabot · Jun 11, 2024 · f1b940d · f1b940d
1 parent cdf1b3b
commit f1b940d
Show file tree

Hide file tree

Showing 2 changed files with 59 additions and 32 deletions.
diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.cc
@@ -769,12 +769,12 @@ void PandarGeneral_Internal::ProcessLiarPacket() {
   //   return;
 
   while (enable_lidar_process_thr_) {
-    if (!m_PacketsBuffer.hasEnoughPackets()) {
+    PandarPacket packet;
+    if (!m_PacketsBuffer.pop(packet))
+    {
       usleep(1000);
       continue;
     }
-    PandarPacket packet = *(m_PacketsBuffer.getIterCalc());
-    m_PacketsBuffer.moveIterCalc();
     // rawpacket.stamp = packet.stamp;
     // rawpacket.stamp.nanosec = (packet.stamp - floor(packet.stamp))*1000000000;
     rawpacket.stamp.sec = floor(packet.stamp);

diff --git a/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h b/src/HesaiLidar_General_SDK/src/PandarGeneralRaw/src/pandarGeneral_internal.h
@@ -24,8 +24,10 @@
 
 #include <list>
 #include <string>
+#include <mutex>
 
 #include <boost/function.hpp>
+#include <condition_variable>
 
 #include "pandarGeneral/point_types.h"
 #include "src/input.h"
@@ -155,6 +157,8 @@ HS_LIDAR_L64_7_BLOCK_PACKET_BODY_SIZE + HS_LIDAR_L64_PACKET_TAIL_WITHOUT_UDPSEQ_
 #define HS_LIDAR_QT_COORDINATE_CORRECTION_D0 (20)
 #define COORDINATE_CORRECTION_CHECK (false)
 
+#define PKT_ARRAY_SIZE 36000
+
 using std::placeholders::_1;
 using std::placeholders::_2;
 using std::placeholders::_3;
@@ -273,49 +277,72 @@ typedef struct PandarGPS_s PandarGPS;
 
 #define ROTATION_MAX_UNITS (36001)
 
-typedef std::array<PandarPacket, 36000> PktArray;
+typedef std::array<PandarPacket, PKT_ARRAY_SIZE> PktArray;
 
 typedef struct PacketsBuffer_s {
+private:
+  std::mutex mutex;
   PktArray m_buffers{};
   PktArray::iterator m_iterPush;
-  PktArray::iterator m_iterCalc;
-  bool m_startFlag;
+  PktArray::iterator m_iterPop;
+  bool m_full;
+  std::condition_variable buffer_not_full;
+  std::condition_variable buffer_not_empty;
+public:
   inline PacketsBuffer_s() {
     m_iterPush = m_buffers.begin();
-    m_iterCalc = m_buffers.begin();
-    m_startFlag = false;
+    m_iterPop = m_buffers.begin();
+    m_full = false;
   }
-  inline int push_back(PandarPacket pkt) {
-    if (!m_startFlag) {
-      *m_iterPush = pkt;
-      m_startFlag = true;
-      return 1;
-    } 
-    m_iterPush++;
-
-    if (m_iterPush == m_iterCalc) {
-      printf("buffer don't have space!,%d\n", m_iterPush - m_buffers.begin());
-      return 0;
+  inline int push_back(const PandarPacket& pkt) {
+    std::unique_lock<std::mutex> lock(mutex);
+
+    //    if (m_full) {
+    //      printf("Pandar: buffer doesn't have space!\n");
+    //    return 0;
+    //    }
+
+    while(m_full)
+    {
+      buffer_not_full.wait(lock);
     }
 
-    if (m_buffers.end() == m_iterPush) {
+    *m_iterPush = pkt;
+    ++m_iterPush;
+    if (m_iterPush == m_buffers.end()) {
       m_iterPush = m_buffers.begin();
-      *m_iterPush = pkt;
     }
-    *m_iterPush = pkt;
+
+    if (m_iterPush == m_iterPop) {
+      m_full = true;
+    }
+    buffer_not_empty.notify_all();
+
     return 1;
-
   }
-  inline bool hasEnoughPackets() {
-    return ((m_iterPush - m_iterCalc > 0 ) ||
-            ((m_iterPush - m_iterCalc + 36000 > 0 ) && (m_buffers.end() - m_iterCalc < 1000) && (m_iterPush - m_buffers.begin() < 1000)));
-  }
-  inline PktArray::iterator getIterCalc() { return m_iterCalc;}
-  inline void moveIterCalc() {
-    m_iterCalc++;
-    if (m_buffers.end() == m_iterCalc) {
-      m_iterCalc = m_buffers.begin();
+
+  inline bool pop(PandarPacket& packet) {
+    std::unique_lock<std::mutex> lock(mutex);
+
+    //    if (!m_full && m_iterPush == m_iterPop) {
+    //      return false;
+    //    }
+
+    while(!m_full && m_iterPush == m_iterPop)
+    {
+      buffer_not_empty.wait(lock);
     }
+
+    packet = *m_iterPop;
+
+    m_iterPop++;
+    if (m_buffers.end() == m_iterPop) {
+      m_iterPop = m_buffers.begin();
+    }
+    m_full = false;
+    buffer_not_full.notify_all();
+
+    return true;
   }
 } PacketsBuffer;