solved a performance problem (1.1.3)

CHANGELOG.md

@@ -2,40 +2,35 @@
 
 All notable changes to this project will be documented in this file.
 
-## [1.1.2] - 2023-02-15
+## [1.1.3] - 2023-03-13
+### Fixed
+- Improve performance when running in ROS2 Humble;
 
+--- 
+## [1.1.2] - 2023-02-15
 ### Changed
 - Change publish frequency range to [0.5Hz, 10 Hz];
-
 ### Fixed
-- Fix a high CPU-Usage problem;
-
+- Fix a high CPU-usage problem;
 
 --- 
 ## [1.1.1] - 2023-01-09
 ### Added
 - Offer valid line-number info in the point cloud data of MID-360 Lidar.
 - Enable IMU by default.
-
 ### Changed
 - Update the README slightly.
 
 --- 
 ## [1.0.0] - 2022-12-12
-
 ### Added
-
 - Support Mid-360 Lidar.
 - Support for Ubuntu 22.04 ROS2 humble.
 - Support multi-topic fuction, the suffix of the topic name corresponds to the ip address of each Lidar. 
-
 ### Changed
-
 - Remove the embedded SDK.
 - Constraint: Livox ROS Driver 2 for ROS2 does not support message passing with PCL native data types.
-
 ### Fixed
-
 - Fix IMU packet loss.
 - Fix some conflicts with livox ros driver.
 - Fixed HAP Lidar publishing PointCloud2 and CustomMsg format point clouds with no line number.

CMakeLists.txt

@@ -123,7 +123,6 @@ if(ROS_EDITION STREQUAL "ROS1")
     PRIVATE
     src/driver_node.cpp
     src/lds.cpp
-    src/lds_lvx.cpp
     src/lds_lidar.cpp
     src/lddc.cpp
     src/livox_ros_driver2.cpp
@@ -265,7 +264,6 @@ else(ROS_EDITION STREQUAL "ROS2")
     src/driver_node.cpp
     src/lds.cpp
     src/lds_lidar.cpp
-    src/lds_lvx.cpp
 
     src/comm/comm.cpp
     src/comm/ldq.cpp

LICENSE.txt

@@ -63,8 +63,6 @@ livox_ros_driver2
     ├── lds.h
     ├── lds_lidar.cpp
     ├── lds_lidar.h
-    ├── lds_lvx.cpp
-    ├── lds_lvx.h
     ├── livox_ros_driver2.cpp
     └── parse_cfg_file
         ├── parse_cfg_file.cpp

src/lddc.cpp

@@ -35,7 +35,6 @@
 
 #include "driver_node.h"
 #include "lds_lidar.h"
-//#include "lds_lvx.h"
 
 namespace livox_ros {
 
@@ -125,7 +124,7 @@ void Lddc::DistributePointCloudData(void) {
     return;
   }
 
-  lds_->semaphore_.Wait();
+  lds_->pcd_semaphore_.Wait();
   for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
     uint32_t lidar_id = i;
     LidarDevice *lidar = &lds_->lidars_[lidar_id];
@@ -145,7 +144,9 @@ void Lddc::DistributeImuData(void) {
   if (lds_->IsRequestExit()) {
     std::cout << "DistributeImuData is RequestExit" << std::endl;
     return;
-  }   
+  }
+
+  lds_->imu_semaphore_.Wait();
   for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
     uint32_t lidar_id = i;
     LidarDevice *lidar = &lds_->lidars_[lidar_id];

src/lds.cpp

@@ -39,7 +39,8 @@ CacheIndex Lds::cache_index_;
 /* Member function --------------------------------------------------------- */
 Lds::Lds(const double publish_freq, const uint8_t data_src)
     : lidar_count_(kMaxSourceLidar),
-      semaphore_(0),
+      pcd_semaphore_(0),
+      imu_semaphore_(0),
       publish_freq_(publish_freq),
       data_src_(data_src),
       request_exit_(false) {
@@ -114,6 +115,11 @@ void Lds::StorageImuData(ImuData* imu_data) {
   LidarDevice *p_lidar = &lidars_[index];
   LidarImuDataQueue* imu_queue = &p_lidar->imu_data;
   imu_queue->Push(imu_data);
+  if (!imu_queue->Empty()) {
+    if (imu_semaphore_.GetCount() <= 0) {
+      imu_semaphore_.Signal();
+    }
+  }
 }
 
 void Lds::StorageLvxPointData(PointFrame* frame) {
@@ -178,13 +184,13 @@ void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint
   if (!QueueIsFull(queue)) {
     QueuePushAny(queue, (uint8_t *)lidar_data, base_time);
     if (!QueueIsEmpty(queue)) {
-      if (semaphore_.GetCount() <= 0) {
-        semaphore_.Signal();
+      if (pcd_semaphore_.GetCount() <= 0) {
+        pcd_semaphore_.Signal();
       }
     }
   } else {
-    if (semaphore_.GetCount() <= 0) {
-        semaphore_.Signal();
+    if (pcd_semaphore_.GetCount() <= 0) {
+        pcd_semaphore_.Signal();
     }
   }
 }

src/lds.h

@@ -68,7 +68,8 @@ class Lds {
  public:
   uint8_t lidar_count_;                 /**< Lidar access handle. */
   LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */
-  Semaphore semaphore_;
+  Semaphore pcd_semaphore_;
+  Semaphore imu_semaphore_;
   static CacheIndex cache_index_;
  protected:
   double publish_freq_;

src/livox_ros_driver2.cpp

@@ -33,7 +33,6 @@
 #include "driver_node.h"
 #include "lddc.h"
 #include "lds_lidar.h"
-#include "lds_lvx.h"
 
 using namespace livox_ros;
 
@@ -200,7 +199,7 @@ void DriverNode::PointCloudDataPollThread()
   std::this_thread::sleep_for(std::chrono::seconds(3));
   do {
     lddc_ptr_->DistributePointCloudData();
-    status = future_.wait_for(std::chrono::seconds(0));
+    status = future_.wait_for(std::chrono::microseconds(0));
   } while (status == std::future_status::timeout);
 }
 
@@ -210,7 +209,7 @@ void DriverNode::ImuDataPollThread()
   std::this_thread::sleep_for(std::chrono::seconds(3));
   do {
     lddc_ptr_->DistributeImuData();
-    status = future_.wait_for(std::chrono::seconds(0));
+    status = future_.wait_for(std::chrono::microseconds(0));
   } while (status == std::future_status::timeout);
 }