test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -69,7 +69,6 @@ void AdjustPPK::ReadBag(const string &bagPath)
         if(convert_->processScan(m.instantiate<pandar_msgs::PandarScan>(), cloud)){
             OnReceivedCloud(cloud);
         }
-        exit(0);
     }
 }
 
@@ -104,13 +103,13 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
               << " cloud.size(): " << cloud.size()
               << " frontPTime: " << cloud.front().timestamp
               << " backPTime: " << cloud.back().timestamp;
-//    if(cloudPacket.timestamp < 1683806268.0){
-//        return;
-//    }
-//    if(cloudPacket.timestamp > 1683806270.0){
-//        exit(0);
-//    }
-    pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(cloudPacket.timestamp) + ".pcd", cloud);
+    if(cloudPacket.timestamp < 1683806268.0){
+        return;
+    }
+    if(cloudPacket.timestamp > 1683806270.0){
+        exit(0);
+    }
+//    pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(cloudPacket.timestamp) + ".pcd", cloud);
     for(const auto &p : cloud){
         PointInter point;
         point.x = p.x;

libs/locate_system/adjust_ppk_by_locate/convert.cc

@@ -125,7 +125,7 @@ Convert::Convert(const std::string &firetimeFile, const std::string &correctionF
   m_iFirstAzimuthIndex = 0;
   m_iLastAzimuthIndex = 0;
   m_iTotalPointsNum = 0;
-  m_bClockwise = true;
+  m_bClockwise = false;
 
   hasGps = 0;
   // subscribe to PandarScan packets
@@ -212,22 +212,23 @@ bool Convert::processScan(
     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
-        }
+//        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;
 
-        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();
@@ -1201,7 +1202,8 @@ void Convert::calcPointXYZIT(const pandar_msgs::PandarPacket &packet, int cursor
     index += PANDAR128_HEAD_SIZE;
     for (int blockid = 0; blockid < header->u8BlockNum; blockid++) {
       uint16_t u16Azimuth = *(uint16_t*)(&packet.data[0] + index);
-      LOG(INFO) << "u16Azimuth: " << u16Azimuth;
+//      LOG(INFO) << "u16Azimuth: " << u16Azimuth;
+
       // ROS_WARN("#####block.fAzimuth[%u]",u16Azimuth);
       index += PANDAR128_AZIMUTH_SIZE;
 
@@ -1252,8 +1254,8 @@ void Convert::calcPointXYZIT(const pandar_msgs::PandarPacket &packet, int cursor
         } else if (azimuthIdx < 0) {
           azimuthIdx += CIRCLE;
         }
-        LOG_EVERY_N(INFO, 256) << "azimuthIdx: " << azimuthIdx
-                               << " m_fSinAllAngle[azimuthIdx]" << m_fSinAllAngle[azimuthIdx];
+//        LOG_EVERY_N(INFO, 256) << "azimuthIdx: " << azimuthIdx
+//                               << " m_fSinAllAngle[azimuthIdx]" << m_fSinAllAngle[azimuthIdx];
 
         point.x = xyDistance * m_fSinAllAngle[azimuthIdx];
         point.y = xyDistance * m_fCosAllAngle[azimuthIdx];