[update]版本号v3.0.2，完成SDK对外的接口Start( )内部增加对数据处理类状态标志变量进行复位操作

ldlidar_driver/include/core/ldlidar_driver.h

@@ -41,7 +41,7 @@ typedef enum CommunicationMode {
 }CommunicationModeTypeDef;
 
 class LDLidarDriver {
-public:
+ public:
   LDLidarDriver();
   ~LDLidarDriver();
 
@@ -89,10 +89,10 @@ class LDLidarDriver {
    * @brief Whether the connection of the communication channel is normal after the lidar is powered on
    * @param[in]
    * *@param timeout:  Wait timeout, in milliseconds
-   * @retval if times >= 1000, return false, communication connection is fail;
-   *   if "times < 1000", return ture, communication connection is successful.
+   * @retval if times >= timeout, return false, communication connection is fail;
+   *   if "times < timeout", return ture, communication connection is successful.
   */
-  bool WaitLidarCommConnect(int64_t timeout = 1000);
+  bool WaitLidarCommConnect(int64_t timeout);
 
   /**
    * @brief get lidar laser scan point cloud data
@@ -146,7 +146,7 @@ class LDLidarDriver {
 
   static void SetIsOkStatus(bool status) { is_ok_ = status;}
 
-private:
+ private:
   std::string sdk_version_number_;
   static bool is_ok_;
   bool is_start_flag_;

ldlidar_driver/include/dataprocess/lipkg.h

@@ -76,8 +76,6 @@ class LiPkg {
 
   bool GetLaserScanData(Points2D& out);
 
-  Points2D GetLaserScanData(void);
-
   void RegisterTimestampGetFunctional(std::function<uint64_t(void)> timestamp_handle);
 
   bool GetLidarPowerOnCommStatus(void);
@@ -86,11 +84,15 @@ class LiPkg {
 
   LidarStatus GetLidarStatus(void);
 
-  // Get lidar data frame ready flag  
-  bool IsFrameReady(void);  
-  // Lidar data frame readiness flag reset
-  void ResetFrameReady(void);
-
+  void ClearDataProcessStatus(void) {
+    is_frame_ready_ = false;
+    is_poweron_comm_normal_ = false;
+    lidarstatus_ = LidarStatus::NORMAL;
+    last_pkg_timestamp_ = 0;
+    first_frame_ = true;
+  }
+
+
  private:
   LDType product_type_;
   uint16_t timestamp_;
@@ -101,6 +103,8 @@ class LiPkg {
   LidarStatus lidarstatus_;
   int measure_point_frequence_;
   std::function<uint64_t(void)> get_timestamp_;
+  uint64_t last_pkg_timestamp_;
+  bool first_frame_;
 
   LiDARFrameTypeDef pkg_;
   Points2D frame_tmp_;
@@ -116,6 +120,11 @@ class LiPkg {
   bool AssemblePacket();  
   void SetFrameReady(void);
   void SetLaserScanData(Points2D& src);
+
+  // Get lidar data frame ready flag  
+  bool IsFrameReady(void);  
+  // Lidar data frame readiness flag reset
+  void ResetFrameReady(void);
 };
 
 } // namespace ldlidar

ldlidar_driver/include/filter/tofbf.h

@@ -41,6 +41,11 @@ enum class FilterType{
 };
 
 class Tofbf {
+ public:
+  Tofbf(int speed, LDType type);
+  ~Tofbf();
+  std::vector<PointData> Filter(const std::vector<PointData> &tmp) const;
+
  private:
   FilterType filter_type_;
   // Low intensity threshold
@@ -55,12 +60,6 @@ class Tofbf {
   Tofbf &operator=(const Tofbf &) = delete;
   std::vector<PointData> NearFilter(const std::vector<PointData> &tmp) const;
   std::vector<PointData> NoiseFilter(const std::vector<PointData> &tmp) const;
-
- public:
-  Tofbf(int speed, LDType type);
-  std::vector<PointData> Filter(const std::vector<PointData> &tmp) const;
-
-  ~Tofbf();
 };
 
 } // namespace ldlidar

ldlidar_driver/src/core/ldlidar_driver.cpp

@@ -24,14 +24,15 @@ namespace ldlidar {
 
 bool LDLidarDriver::is_ok_ = false;
 
-LDLidarDriver::LDLidarDriver() : sdk_version_number_("v3.0.1"),
-  is_start_flag_(false),
-  register_get_timestamp_handle_(nullptr),
-  comm_pkg_(new LiPkg()),
-  comm_serial_(new SerialInterfaceLinux()),
-  comm_tcp_network_(new TCPSocketInterfaceLinux()),
-  comm_udp_network_(new UDPSocketInterfaceLinux()) {
-  last_pubdata_times_ = std::chrono::steady_clock::now();
+LDLidarDriver::LDLidarDriver() 
+  : sdk_version_number_("v3.0.2"),
+    is_start_flag_(false),
+    register_get_timestamp_handle_(nullptr),
+    comm_pkg_(new LiPkg()),
+    comm_serial_(new SerialInterfaceLinux()),
+    comm_tcp_network_(new TCPSocketInterfaceLinux()),
+    comm_udp_network_(new UDPSocketInterfaceLinux()) {
+    last_pubdata_times_ = std::chrono::steady_clock::now();
 }
 
 LDLidarDriver::~LDLidarDriver() {
@@ -78,6 +79,8 @@ bool LDLidarDriver::Start(LDType product_name,
     LD_LOG_ERROR("get timestamp fuctional is not register.","");
     return false;
   }
+
+  comm_pkg_->ClearDataProcessStatus();
   comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_);
   comm_pkg_->SetProductType(product_name);
 
@@ -128,6 +131,7 @@ bool LDLidarDriver::Start(LDType product_name,
     return false;
   }
 
+  comm_pkg_->ClearDataProcessStatus();
   comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_);
   comm_pkg_->SetProductType(product_name);
 
@@ -217,13 +221,13 @@ bool LDLidarDriver::WaitLidarCommConnect(int64_t timeout) {
   auto last_time = std::chrono::steady_clock::now();
 
   bool is_recvflag = false;
-  while (!is_recvflag && \
-  (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - last_time).count() < timeout)) {
+  do {
     if (comm_pkg_->GetLidarPowerOnCommStatus()) {
       is_recvflag = true;
     }
     usleep(1000);
-  }
+  } while (!is_recvflag && (std::chrono::duration_cast<std::chrono::milliseconds>(
+    std::chrono::steady_clock::now() - last_time).count() < timeout));
 
   if (is_recvflag) {
     last_pubdata_times_ = std::chrono::steady_clock::now();

ldlidar_driver/src/dataprocess/lipkg.cpp

@@ -56,15 +56,18 @@ uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) {
   return crc;
 }
 
-LiPkg::LiPkg() : product_type_(LDType::NO_VERSION),
-      timestamp_(0),
-      speed_(0),
-      is_frame_ready_(false),
-      is_poweron_comm_normal_(false),
-      is_filter_(true),
-      lidarstatus_(LidarStatus::NORMAL),
-      measure_point_frequence_(4500),
-      get_timestamp_(nullptr){
+LiPkg::LiPkg() 
+  : product_type_(LDType::NO_VERSION),
+    timestamp_(0),
+    speed_(0),
+    is_frame_ready_(false),
+    is_poweron_comm_normal_(false),
+    is_filter_(true),
+    lidarstatus_(LidarStatus::NORMAL),
+    measure_point_frequence_(4500),
+    get_timestamp_(nullptr),
+    last_pkg_timestamp_(0),
+    first_frame_(true) {
 
 }
 
@@ -143,20 +146,19 @@ bool LiPkg::AnalysisOne(uint8_t byte) {
 bool LiPkg::Parse(const uint8_t *data, long len) {
   for (int i = 0; i < len; i++) {
     if (AnalysisOne(data[i])) {
-      static uint64_t last_pkg_timestamp = 0;
       is_poweron_comm_normal_ = true;
       // parse a package is success
       double diff = (pkg_.end_angle / 100 - pkg_.start_angle / 100 + 360) % 360;
       if (diff <= ((double)pkg_.speed * POINT_PER_PACK / measure_point_frequence_ * 1.5)) {
 
-        if (0 == last_pkg_timestamp) {
-          last_pkg_timestamp = get_timestamp_();
+        if (0 == last_pkg_timestamp_) {
+          last_pkg_timestamp_ = get_timestamp_();
           continue;
         }
         uint64_t current_pack_stamp = get_timestamp_();
         int pkg_point_number = POINT_PER_PACK;
         double pack_stamp_point_step =  
-          static_cast<double>(current_pack_stamp - last_pkg_timestamp) / static_cast<double>(pkg_point_number - 1);
+          static_cast<double>(current_pack_stamp - last_pkg_timestamp_) / static_cast<double>(pkg_point_number - 1);
 
         speed_ = pkg_.speed; // Degrees per second
         timestamp_ = pkg_.timestamp; // In milliseconds
@@ -171,11 +173,11 @@ bool LiPkg::Parse(const uint8_t *data, long len) {
             data.angle -= 360.0;
           }
           data.intensity = pkg_.point[i].intensity;
-          data.stamp = static_cast<uint64_t>(last_pkg_timestamp + (pack_stamp_point_step * i));
+          data.stamp = static_cast<uint64_t>(last_pkg_timestamp_ + (pack_stamp_point_step * i));
           frame_tmp_.push_back(PointData(data.angle, data.distance, data.intensity, data.stamp));
         }
 
-        last_pkg_timestamp = current_pack_stamp; //// update last pkg timestamp
+        last_pkg_timestamp_ = current_pack_stamp; //// update last pkg timestamp
       }
     }
   }
@@ -213,9 +215,8 @@ bool LiPkg::AssemblePacket() {
       std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) { return a.stamp < b.stamp; });
 
       if (tmp.size() > 0) {
-        static bool first_frame = true;
-        if (first_frame) {
-          first_frame = false;
+        if (first_frame_) {
+          first_frame_ = false;
         } else {
           SetLaserScanData(tmp);
           SetFrameReady();
@@ -290,11 +291,6 @@ bool LiPkg::GetLaserScanData(Points2D& out) {
   }
 }
 
-Points2D LiPkg::GetLaserScanData(void) {
-  std::lock_guard<std::mutex> lg(mutex_lock2_);
-  return laser_scan_data_;
-}
-
 void LiPkg::SetLaserScanData(Points2D& src) {
   std::lock_guard<std::mutex> lg(mutex_lock2_);
   laser_scan_data_ = src;
@@ -305,7 +301,12 @@ void LiPkg::RegisterTimestampGetFunctional(std::function<uint64_t(void)> timesta
 }
 
 bool LiPkg::GetLidarPowerOnCommStatus(void) {
-  return is_poweron_comm_normal_;
+  if (is_poweron_comm_normal_) {
+    is_poweron_comm_normal_ = false;
+    return true;
+  } else {
+    return false;
+  }
 }
 
 void LiPkg::EnableFilter(bool is_enable) {

src/publish_node/main.cpp

@@ -81,7 +81,7 @@ int main(int argc, char **argv) {
     exit(EXIT_FAILURE);
   }
 
-  if (ldlidarnode->WaitLidarCommConnect(500)) {
+  if (ldlidarnode->WaitLidarCommConnect(3000)) {
     ROS_INFO("ldlidar communication is normal.");
   } else {
     ROS_ERROR("ldlidar communication is abnormal.");
@@ -95,16 +95,12 @@ int main(int argc, char **argv) {
   ros::Rate r(10); //10hz
   ldlidar::Points2D laser_scan_points;
   double lidar_spin_freq;
-  bool is_get = false;
+  ROS_INFO("Publish topic message:ldlidar scan data .");
 
   while (ros::ok()) {
 
-    switch (ldlidarnode->GetLaserScanData(laser_scan_points, 1000)){
+    switch (ldlidarnode->GetLaserScanData(laser_scan_points, 1500)){
       case ldlidar::LidarStatus::NORMAL: 
-        if (!is_get) {
-          is_get = true;
-          ROS_INFO("get ldlidar normal data and publish topic message.");
-        }
         ldlidarnode->GetLidarSpinFreq(lidar_spin_freq);
         ToLaserscanMessagePublish(laser_scan_points, lidar_spin_freq, setting, lidar_pub);
         break;