增加适配R3雷达

Author: zhanyiaini

CMakeLists.txt

@@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX)
 # version
 set(YDLIDAR_SDK_VERSION_MAJOR 1)
 set(YDLIDAR_SDK_VERSION_MINOR 2)
-set(YDLIDAR_SDK_VERSION_PATCH 5)
+set(YDLIDAR_SDK_VERSION_PATCH 6)
 set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
 
 ##########################################################

core/common/DriverInterface.h

@@ -512,6 +512,7 @@ namespace ydlidar
           YDLIDAR_G5 = 20,            /**< G5 LiDAR Model. */
           YDLIDAR_G7 = 21,            /**< G7 LiDAR Model. */
           YDLIDAR_SCL = 22,           // SCL雷达
+          YDLIDAR_R3 = 23, //R3雷达
 
           YDLIDAR_GS2 = 51, // GS2雷达
           YDLIDAR_GS1 = 52, // GS1雷达

core/common/ydlidar_datatype.h

@@ -113,6 +113,7 @@ typedef struct {
   float sampleRate = .0;
   // Array of lidar points
   std::vector<LaserPoint> points;
+  int size = 0; //实际点数（固定分辨率时点数与实际点数不符）
   // Configuration of scan
   LaserConfig config;
   int moduleNum = 0;

core/common/ydlidar_help.h

@@ -135,6 +135,12 @@ inline std::string lidarModelToString(int model)
   case DriverInterface::YDLIDAR_G7:
     name = "G7";
     break;
+  case DriverInterface::YDLIDAR_SCL:
+    name = "SCL";
+    break;
+  case DriverInterface::YDLIDAR_R3:
+    name = "R3";
+    break;
   case DriverInterface::YDLIDAR_GS1:
     name = "GS1";
     break;
@@ -336,12 +342,21 @@ inline bool hasSampleRate(int model)
 
   return ret;
 }
+
+inline bool isR3Lidar(int model)
+{
+  if (model == DriverInterface::YDLIDAR_R3)
+  {
+      return true;
+  }
+  return false;
+}
+
 /*!
  * @brief Is there a zero offset angle
  * @param model   lidar model
  * @return true if there are zero offset angle, otherwise false.
  */
-
 inline bool hasZeroAngle(int model) {
   bool ret = false;
 

examples/tri_test.cpp

@@ -270,9 +270,9 @@ int main(int argc, char *argv[])
   laser.enableSunNoise(false);
 
   //设置是否获取底板设备信息（默认仅尝试获取模组设备信息）
-  laser.setBottomPriority(false);
+  laser.setBottomPriority(true);
   //启用调试
-  laser.setEnableDebug(true);
+  laser.setEnableDebug(false);
 
   uint32_t t = getms(); //时间
   int c = 0; //计数
@@ -334,9 +334,9 @@ int main(int argc, char *argv[])
       // for (size_t i = 0; i < scan.points.size(); ++i)
       // {
       //   const LaserPoint &p = scan.points.at(i);
-      //   printf("%d d %f a %f\n", i, p.range, p.angle * 180.0 / M_PI);
+      //   printf("%d a:%.02f d:%.01f\n", i, p.angle * 180.0 / M_PI, p.range * 1000.0);
       // }
-      fflush(stdout);
+      // fflush(stdout);
     }
     else
     {

src/CYdLidar.cpp

@@ -679,8 +679,8 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
     {
       const node_info& node = global_nodes[i];
 
-      // printf("%lu a %.01f r %u\n", 
-      //   i, float(node.angle) / 64.0f, node.dist);
+      // printf("%lu a:%.01f d:%u\n", 
+      //   i, float(node.angle) / 128.0f, node.dist);
 
       if (isNetTOFLidar(m_LidarType))
       {
@@ -700,6 +700,10 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
       {
         range = static_cast<float>(global_nodes[i].dist / 2000.f);
       }
+      else if (isR3Lidar(lidar_model))
+      {
+        range = static_cast<float>(global_nodes[i].dist / 40000.f);
+      }
       else
       {
         if (isTOFLidar(m_LidarType) || 
@@ -787,6 +791,8 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
       }
     } //end for (int i = 0; i < count; i++)
 
+    outscan.size = outscan.points.size(); //保留原点云数
+
     if (m_FixedResolution)
     {
       outscan.points.resize(all_node_count);
@@ -1408,10 +1414,10 @@ bool CYdLidar::getDeviceInfo()
 
   frequencyOffset = 0.4;
   lidar_model = di.model;
+  printf("Current Lidar Model Code %d\n", lidar_model);
   bool intensity = hasIntensity(di.model);
   defalutSampleRate = getDefaultSampleRate(di.model);
   // printf("getDefaultSampleRate %d\n", defalutSampleRate.size());
-
   intensity = m_Intensity;
   std::string serial_number;
   lidarPtr->setIntensities(intensity);

src/YDlidarDriver.cpp

@@ -749,7 +749,9 @@ result_t YDlidarDriver::waitDevicePackage(uint32_t timeout)
 
     device_info di;
     getData(reinterpret_cast<uint8_t *>(&di), DEVICEINFOSIZE);
-    model = di.model;
+    //没有启用底板时才使用模组的雷达型号码
+    if (!m_Bottom)
+      model = di.model;
     m_HasDeviceInfo |= EPT_Module;
     m_ModuleDevInfo = di;
     printfDeviceInfo(di, EPT_Module);
@@ -1270,7 +1272,7 @@ void YDlidarDriver::parseNodeFromeBuffer(node_info *node)
                         package.nodes[nodeIndex].dist & 0xfffc;
                 (*node).is = package.nodes[nodeIndex].dist & 0x0003;
 
-                // printf("%d i %u\n", nodeIndex, (*node).qual);
+                // printf("%d d:%u\n", nodeIndex, (*node).dist);
                 // fflush(stdout);
             }
             else
@@ -1309,7 +1311,8 @@ void YDlidarDriver::parseNodeFromeBuffer(node_info *node)
                 // printf("SCL correct angle [%d]\n", correctAngle);
             }
             else if (isTriangleLidar(m_LidarType) &&
-                !isTminiLidar(model)) //去掉Tmini雷达的角度二级解析
+                !isTminiLidar(model) && //去掉Tmini雷达的角度二级解析
+                !isR3Lidar(model)) //去掉R3雷达的角度二级解析
             {
                 correctAngle = (int32_t)(((atan(((21.8 * (155.3 - ((*node).dist / 4.0))) / 155.3) / ((*node).dist / 4.0))) * 180.0 / 3.1415) * 64.0);
             }
@@ -1666,7 +1669,7 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout)
     //获取启动时抛出的设备信息或每帧数据中的设备信息
     if (m_HasDeviceInfo & EPT_Module)
     {
-      info = m_BaseDevInfo;
+      info = m_ModuleDevInfo;
       return RESULT_OK;
     }
     else