[update]版本号v2.3.0,优化代码工程层级，实现数据处理应用层与ROS接口应用层分离

.gitignore

@@ -1,3 +1,3 @@
-.vscode
+*.json
 *.zip
 core

.vscode/c_cpp_properties.json

@@ -4,9 +4,11 @@
             "name": "Linux",
             "includePath": [
                 "${workspaceFolder}/**",
+                "${workspaceFolder}/include/**",
+                "${workspaceFolder}/include/ldlidar_driver/**",
                 "/usr/include/**",
-                "/opt/ros/kinetic/include/**",
-                "/opt/ros/noetic/include/**"
+                "/opt/ros/kinetic/include/**"/*,
+                "/opt/ros/noetic/include/**" */
             ],
             "defines": [],
             "compilerPath": "/usr/bin/g++",

CMakeLists.txt

@@ -120,7 +120,8 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
+${CMAKE_CURRENT_SOURCE_DIR}/include
+${CMAKE_CURRENT_SOURCE_DIR}/include/ldlidar_driver/include
 ${catkin_INCLUDE_DIRS}
 )
 
@@ -136,12 +137,14 @@ ${catkin_INCLUDE_DIRS}
 #  message("-- #define USE_SLBI")
 #endif()
 
-file(GLOB  MAIN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
-add_executable(${PROJECT_NAME}_node  ${MAIN_SRC})
+file(GLOB  LDLIDAR_DRIVER_SRC ${CMAKE_CURRENT_SOURCE_DIR}/include/ldlidar_driver/*.cpp)
+file(GLOB  MAIN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/publish_node/*.cpp)
+file(GLOB  LISTEN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/listen_node/*.cpp)
+
+add_executable(${PROJECT_NAME}_node  ${MAIN_SRC} ${LDLIDAR_DRIVER_SRC})
 target_link_libraries(${PROJECT_NAME}_node pthread ${catkin_LIBRARIES})
 
-file(GLOB  SRC ${CMAKE_CURRENT_SOURCE_DIR}/listen/*.cpp)
-add_executable(${PROJECT_NAME}_listen_node  ${SRC})
+add_executable(${PROJECT_NAME}_listen_node  ${LISTEN_SRC})
 target_link_libraries(${PROJECT_NAME}_listen_node  ${catkin_LIBRARIES})
 
 ## Declare a C++ library

src/lipkg.h → include/ldlidar_driver/include/lipkg.h

@@ -22,7 +22,7 @@
 #ifndef __LIPKG_H
 #define __LIPKG_H
 
-#include "ros_api.h"
+// #include "ros_api.h"
 #include <stdint.h>
 
 #include <array>
@@ -57,10 +57,13 @@ typedef struct __attribute__((packed)) {
 
 class LiPkg {
  public:
-  LiPkg(std::string frame_id, bool laser_scan_dir, bool enable_angle_crop_func,
-  double angle_crop_min, double angle_crop_max);
+  const int kPointFrequence = 4500;
+
+  LiPkg(std::string product_name);
   // get Lidar spin speed (Hz)
   double GetSpeed(void); 
+  // get lidar spind speed (degree per second) origin
+  uint16_t GetSpeedOrigin(void);
   // get time stamp of the packet
   uint16_t GetTimestamp(void);  
   // Get lidar data frame ready flag  
@@ -71,32 +74,26 @@ class LiPkg {
   // the number of errors in parser process of lidar data frame
   long GetErrorTimes(void);  
   void CommReadCallback(const char *byte, size_t len);
-  // Get sensor_msg/LaserScan type data
-  sensor_msgs::LaserScan GetLaserScan() { return output_; }
-
+  Points2D GetLaserScanData(void);
+
  private:
-  const int kPointFrequence = 4500;
-  std::string frame_id_;
+  std::string product_name_;
   uint16_t timestamp_;
   double speed_;
   long error_times_;
   bool is_frame_ready_;
-  bool laser_scan_dir_;
-  bool enable_angle_crop_func_;
-  double angle_crop_min_;
-  double angle_crop_max_;
-  LiDARFrameTypeDef pkg;
+  LiDARFrameTypeDef pkg_;
   std::vector<uint8_t> data_tmp_;
-  std::vector<PointData> frame_tmp_;
-  std::mutex  mutex_lock;
-  sensor_msgs::LaserScan output_;
+  Points2D frame_tmp_;
+  Points2D laser_scan_data_;
+  std::mutex  mutex_lock_;
 
    // parse single packet
   bool AnalysisOne(uint8_t byte);
   bool Parse(const uint8_t* data, long len);  
   // combine stantard data into data frames and calibrate
   bool AssemblePacket();  
-  void ToLaserscan(std::vector<PointData>& src);
+  void FillLaserScanData(Points2D& src);
 };
 
 #endif  //__LIPKG_H

src/lipkg.cpp → include/ldlidar_driver/lipkg.cpp

@@ -61,17 +61,12 @@ uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) {
   return crc;
 }
 
-LiPkg::LiPkg(std::string frame_id, bool laser_scan_dir, bool enable_angle_crop_func,
-  double angle_crop_min, double angle_crop_max)
-    : timestamp_(0),
+LiPkg::LiPkg(std::string product_name)
+    : product_name_(product_name),
+      timestamp_(0),
       speed_(0),
       error_times_(0),
-      is_frame_ready_(false),
-      frame_id_(frame_id),
-      laser_scan_dir_(laser_scan_dir),
-      enable_angle_crop_func_(enable_angle_crop_func),
-      angle_crop_min_(angle_crop_min),
-      angle_crop_max_(angle_crop_max) {
+      is_frame_ready_(false){
 
 }
 
@@ -105,11 +100,11 @@ bool LiPkg::AnalysisOne(uint8_t byte) {
     case DATA:
       tmp[count++] = byte;
       if (count >= pkg_count) {
-        memcpy((uint8_t *)&pkg, tmp, pkg_count);
-        uint8_t crc = CalCRC8((uint8_t *)&pkg, pkg_count - 1);
+        memcpy((uint8_t *)&pkg_, tmp, pkg_count);
+        uint8_t crc = CalCRC8((uint8_t *)&pkg_, pkg_count - 1);
         state = HEADER;
         count = 0;
-        if (crc == pkg.crc8) {
+        if (crc == pkg_.crc8) {
           return true;
         } else {
           error_times_++;
@@ -128,24 +123,24 @@ bool LiPkg::Parse(const uint8_t *data, long len) {
   for (int i = 0; i < len; i++) {
     if (AnalysisOne(data[i])) {
       // 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 / kPointFrequence * 3 / 2) {
+      double diff = (pkg_.end_angle / 100 - pkg_.start_angle / 100 + 360) % 360;
+      if (diff > (double)pkg_.speed * POINT_PER_PACK / kPointFrequence * 3 / 2) {
         error_times_++;
       } else {
-        speed_ = pkg.speed; // Degrees per second
-        timestamp_ = pkg.timestamp; // In milliseconds
-        uint32_t diff = ((uint32_t)pkg.end_angle + 36000 - (uint32_t)pkg.start_angle) % 36000;
+        speed_ = pkg_.speed; // Degrees per second
+        timestamp_ = pkg_.timestamp; // In milliseconds
+        uint32_t diff = ((uint32_t)pkg_.end_angle + 36000 - (uint32_t)pkg_.start_angle) % 36000;
         float step = diff / (POINT_PER_PACK - 1) / 100.0;
-        float start = (double)pkg.start_angle / 100.0;
-        float end = (double)(pkg.end_angle % 36000) / 100.0;
+        float start = (double)pkg_.start_angle / 100.0;
+        float end = (double)(pkg_.end_angle % 36000) / 100.0;
         PointData data;
         for (int i = 0; i < POINT_PER_PACK; i++) {
-          data.distance = pkg.point[i].distance;
+          data.distance = pkg_.point[i].distance;
           data.angle = start + i * step;
           if (data.angle >= 360.0) {
             data.angle -= 360.0;
           }
-          data.intensity = pkg.point[i].intensity;
+          data.intensity = pkg_.point[i].intensity;
           frame_tmp_.push_back(PointData(data.angle, data.distance, data.intensity));
         }
       }
@@ -176,7 +171,7 @@ bool LiPkg::AssemblePacket() {
       // ROS_INFO_STREAM("[ldrobot] filter back poit size: " << tmp.size());
       if (tmp.size() > 0) {
         if (tmp.front().angle < 1.0) {
-          ToLaserscan(tmp);
+          FillLaserScanData(tmp);
           SetFrameReady();
         }
         frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count);
@@ -194,6 +189,10 @@ double LiPkg::GetSpeed(void) {
   return (speed_ / 360.0);  // unit is hz
 }
 
+uint16_t LiPkg::GetSpeedOrigin(void) {
+  return speed_;
+}
+
 uint16_t LiPkg::GetTimestamp(void) {
   return timestamp_; 
 }
@@ -204,14 +203,14 @@ bool LiPkg::IsFrameReady(void) {
 
 void LiPkg::ResetFrameReady(void) {
   {
-    std::lock_guard<std::mutex> lock(mutex_lock);
+    std::lock_guard<std::mutex> lock(mutex_lock_);
     is_frame_ready_ = false;
   }
 }
 
 void LiPkg::SetFrameReady(void) {
   {
-    std::lock_guard<std::mutex> lock(mutex_lock);
+    std::lock_guard<std::mutex> lock(mutex_lock_);
     is_frame_ready_ = true;
   }
 }
@@ -226,93 +225,12 @@ void LiPkg::CommReadCallback(const char *byte, size_t len) {
   }
 }
 
-void LiPkg::ToLaserscan(std::vector<PointData>& src) {
-  float angle_min, angle_max, range_min, range_max, angle_increment;
-  // Adjust the parameters according to the demand
-  angle_min = ANGLE_TO_RADIAN(src.front().angle);
-  angle_max = ANGLE_TO_RADIAN(src.back().angle);
-  range_min = 0.02;
-  range_max = 12;
-  angle_increment = ANGLE_TO_RADIAN(speed_ / kPointFrequence);
-  // ROS_INFO_STREAM("[ldrobot] angle min: " << src.front().angle);
-  // ROS_INFO_STREAM("[ldrobot] angle max: " << src.back().angle);
-  // ROS_INFO_STREAM("[ldrobot] speed(hz): " << GetSpeed());
-  static uint16_t last_times_stamp = 0;
-  float dealt_times_stamp = 0;
-  uint16_t tmp_times_stamp = GetTimestamp();
-  if (tmp_times_stamp - last_times_stamp < 0) {
-    dealt_times_stamp = (tmp_times_stamp - last_times_stamp + 30000) / 1000.f;
-  } else {
-    dealt_times_stamp = (tmp_times_stamp - last_times_stamp) / 1000.f;
-  }
-  last_times_stamp = tmp_times_stamp;
-  // Calculate the number of scanning points
-  if (speed_ > 0) {
-    unsigned int beam_size = static_cast<unsigned int>(ceil((angle_max - angle_min) / angle_increment));
-    // ROS_INFO_STREAM("[ldrobot] beam_size: " << beam_size);
-    output_.header.stamp = ros::Time::now();
-    output_.header.frame_id = frame_id_;
-    output_.angle_min = angle_min;
-    output_.angle_max = angle_max;
-    output_.range_min = range_min;
-    output_.range_max = range_max;
-    output_.angle_increment = angle_increment;
-    if (beam_size <= 1) {
-      output_.time_increment = 0;
-    } else {
-      output_.time_increment = dealt_times_stamp/(beam_size-1);
-    }
-    output_.scan_time = dealt_times_stamp;
-    // First fill all the data with Nan
-    output_.ranges.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
-    output_.intensities.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
-
-    unsigned int last_index = 0;
-    for (auto point : src) {
-      float range = point.distance / 1000.f;  // distance unit transform to meters
-      float intensity = point.intensity;      // laser receive intensity 
-      float dir_angle;
-
-      if ((point.distance == 0) && (point.intensity == 0)) { // filter is handled to  0, Nan will be assigned variable.
-        range = std::numeric_limits<float>::quiet_NaN(); 
-        intensity = std::numeric_limits<float>::quiet_NaN();
-      }
-
-      if (laser_scan_dir_) {
-        dir_angle = static_cast<float>(360.f - point.angle); // Lidar rotation data flow changed from clockwise to counterclockwise
-      } else {
-        dir_angle = point.angle;
-      }
-
-      if (enable_angle_crop_func_) { // Angle crop setting, Mask data within the set angle range
-        if ((dir_angle >= angle_crop_min_) && (dir_angle <= angle_crop_max_)) {
-          range = std::numeric_limits<float>::quiet_NaN();
-          intensity = std::numeric_limits<float>::quiet_NaN();
-        }
-      }
+Points2D LiPkg::GetLaserScanData(void) {
+  return laser_scan_data_;
+}
 
-      float angle = ANGLE_TO_RADIAN(dir_angle); // Lidar angle unit form degree transform to radian
-      unsigned int index = (unsigned int)((angle - output_.angle_min) / output_.angle_increment);
-      if (index < beam_size) {
-        // If the current content is Nan, it is assigned directly
-        if (std::isnan(output_.ranges[index])) {
-          output_.ranges[index] = range;
-          unsigned int err = index - last_index;
-          if (err == 2){
-            output_.ranges[index - 1] = range;
-            output_.intensities[index - 1] = intensity;
-          }
-        } else { // Otherwise, only when the distance is less than the current
-                //   value, it can be re assigned
-          if (range < output_.ranges[index]) {
-            output_.ranges[index] = range;
-          }
-        }
-        output_.intensities[index] = intensity;
-        last_index = index;
-      }
-    }
-  }
+void LiPkg::FillLaserScanData(Points2D& src) {
+  laser_scan_data_ = src;
 }
 
 /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF

src/ros_api.h → include/ros_api.h

@@ -23,6 +23,16 @@
 
 #include <ros/ros.h>
 #include <sensor_msgs/LaserScan.h>
+#include <string>
+
+struct LaserScanSetting
+{
+  std::string frame_id;
+  bool laser_scan_dir;
+  bool enable_angle_crop_func;
+  double angle_crop_min;
+  double angle_crop_max;
+};
 
 #endif //__ROS_API_H__
 

launch/ld06.launch

@@ -20,9 +20,9 @@
   <param name="angle_crop_max" type="double" value="225.0"/>
  </node>
 <!-- ldlidar message subscriber node -->
- <node name="ListenLD06" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
+ <!-- node name="ListenLD06" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
   <param name="topic_name" value="scan"/>
- </node>
+ </node -->
  <!-- publisher tf transform, parents frame is base_link, child frame is base_laser -->
  <!-- args="x y z yaw pitch roll parents_frame_id child_frame_id period_in_ms"-->
  <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50"/>

launch/ld19.launch

@@ -20,9 +20,9 @@
   <param name="angle_crop_max" type="double" value="225.0"/>
  </node>
 <!-- ldlidar message subscriber node -->
- <node name="ListenLD19" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
+ <!-- node name="ListenLD19" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
   <param name="topic_name" value="scan"/>
- </node>
+ </node -->
  <!-- publisher tf transform, parents frame is base_link, child frame is base_laser -->
  <!-- args="x y z yaw pitch roll parents_frame_id child_frame_id period_in_ms"-->
  <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50"/>