config log

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -112,7 +112,7 @@ Vector3d AdjustPPK::ReadCenter(const string& taskName)
         boost::split(line_vec, line, boost::is_any_of(":"), boost::token_compress_on);
         center.z() = stof(line_vec.at(1));
     }
-//    LOG(INFO) << "center_: " << center.transpose();
+    LOG(INFO) << taskName << " center_: " << center.transpose();
     return center;
 }
 
@@ -225,6 +225,7 @@ void AdjustPPK::LoadMapInfo()
         directory_iterator task_end_iter;
         for(; task_iter != task_end_iter; task_iter++) {
             string taskName = task_iter->path().filename().string();
+            auto taskCenter = ReadCenter(taskName);
             string pcdDir = task_iter->path().string() + "/pcd_cart";
             if(is_directory(pcdDir)){
                 directory_iterator pcd_iter(pcdDir);
@@ -239,7 +240,7 @@ void AdjustPPK::LoadMapInfo()
                     pcdPathInfo.pcdPath = pcd_iter->path().string();
                     if(baseTaskName_ != taskName){
                         pcdPathInfo.taskName = taskName;
-                        pcdPathInfo.taskCenter = ReadCenter(taskName);
+                        pcdPathInfo.taskCenter = taskCenter;
                     }
                     if(meshVec_.find(meshId) == meshVec_.end()){
                         MashMap meshMap;

libs/locate_system/adjust_ppk_by_locate/convert.cc

@@ -80,9 +80,16 @@ static const float azimuth_offset[] = {
     -3.436f, 1.152f, -3.44f,  1.154f, -3.443f, 1.157f,  -3.446f, -3.449f,
 };
 
+static const std::vector<uint16_t> active_laser = {
+    1, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 30, 34, 38, 42, 46, 50,
+    54, 58, 62, 66, 70, 74, 78, 82, 86, 90, 92, 94, 96, 98, 100, 102, 104, 106,
+    108, 110, 112, 114, 116, 118, 120, 122, 124, 126, 128,
+};
+
 /** @brief Constructor. */
-Convert::Convert(const std::string &firetimeFile, const std::string &correctionFile)
-    :m_sLidarFiretimeFile(firetimeFile), m_sLidarCorrectionFile(correctionFile){
+Convert::Convert(const std::string &firetimeFile, const std::string &correctionFile, bool use_specified_laser)
+    :m_sLidarFiretimeFile(firetimeFile), m_sLidarCorrectionFile(correctionFile),
+      use_specified_laser_(use_specified_laser){
   
   m_sRosVersion = "PandarSwiftROS_1.0.37";
   ROS_WARN("--------PandarSwift ROS version: %s--------\n\n",m_sRosVersion.c_str());
@@ -200,6 +207,12 @@ Convert::Convert(const std::string &firetimeFile, const std::string &correctionF
 //  boost::thread publishPointsThr(
 //      boost::bind(&Convert::publishPointsThread, this));
 
+  if(use_specified_laser_){
+      active_laser_mask_.resize(PANDAR128_LASER_NUM, false);
+      for(auto laser_id : active_laser){
+          active_laser_mask_.at(laser_id - 1) = true;
+      }
+  }
 }
 
 bool Convert::processScan(
@@ -1108,6 +1121,11 @@ void Convert::calcPointXYZIT(const pandar_msgs::PandarPacket &packet, int cursor
 			if(1 == blockid)
 				state = (pkt.tail.nShutdownFlag & 0x30) >> 4;
 			for (int i = 0; i < pkt.head.u8LaserNum; i++) {
+                if(use_specified_laser_){
+                    if(!active_laser_mask_.at(i)){
+                        continue;
+                    }
+                }
 				/* for all the units in a block */
 				Pandar128Unit &unit = block.units[i];
 				PPoint point;

libs/locate_system/adjust_ppk_by_locate/convert.h

@@ -383,7 +383,9 @@ typedef struct RedundantPoint_s {
 namespace pandar_pointcloud {
 class Convert {
  public:
-  Convert(const std::string& firetimeFile, const std::string &correctionFile);
+  Convert(const std::string& firetimeFile,
+          const std::string &correctionFile,
+          bool use_specified_laser = false);
   ~Convert() {}
 
   bool processScan(const pandar_msgs::PandarScan::ConstPtr &scanMsg,
@@ -485,6 +487,9 @@ class Convert {
 
   bool inited = false;
 
+  bool use_specified_laser_ = false;
+
+  std::vector<bool> active_laser_mask_;
 };
 
 }  // namespace pandar_pointcloud