test

libs/locate_system/adjust_ppk_by_locate/convert.cc

@@ -206,11 +206,28 @@ bool Convert::processScan(
         const pandar_msgs::PandarScan::ConstPtr &scanMsg, PPointCloud &pc)
 {
 //    LOG(INFO) << "scanMsg->packets.size(): " << scanMsg->packets.size();
-    if(0 == scanMsg->packets.size()){
+    if(scanMsg->packets.size() < 2){
         return false;
     }
     if(!inited){
         init(scanMsg->packets[0]);
+
+        uint16_t frontAzimuth = *(uint16_t*)(&(scanMsg->packets[0].data[0]) + m_iFirstAzimuthIndex);
+        uint16_t backAzimuth = *(uint16_t*)(&(scanMsg->packets[1].data[0]) + m_iFirstAzimuthIndex);
+        if(((frontAzimuth < backAzimuth) && ((backAzimuth - frontAzimuth) <  m_iAngleSize * 10))
+           ||
+          ((frontAzimuth > backAzimuth) && (frontAzimuth - backAzimuth) > m_iAngleSize * 10))
+        {
+          m_bClockwise = true; //Clockwise
+        }
+
+        if(((frontAzimuth > backAzimuth) && ((frontAzimuth - backAzimuth) <  m_iAngleSize * 10))
+           ||
+          ((frontAzimuth < backAzimuth) && (backAzimuth - frontAzimuth) > m_iAngleSize * 10))
+        {
+          m_bClockwise = false; //countClockwise
+        }
+        LOG(INFO) << "m_bClockwise: " << m_bClockwise;
         inited = true;
     }
     m_OutMsgArray[0]->clear();
@@ -814,6 +831,7 @@ void Convert::init(pandar_msgs::PandarPacket packet)
       printf("init mode: workermode: %x,return mode: %x,speed: %d\n",m_iWorkMode, m_iReturnMode, m_iMotorSpeed);
       changeAngleSize();
       changeReturnBlockSize();
+
       boost::shared_ptr<PPointCloud> outMag0(new PPointCloud(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize, 1));
       boost::shared_ptr<PPointCloud> outMag1(new PPointCloud(CIRCLE_ANGLE / m_iAngleSize * m_iLaserNum * m_iReturnBlockSize, 1));
       m_OutMsgArray[0] = outMag0;