فهرست منبع

2023-6-30 修改车辆宽度限制 <=1.95m,入口多线测量与单线测量警告每100次打印一次

yct 1 سال پیش
والد
کامیت
acda145bd8

+ 1 - 1
测量节点/velodyne_lidar/ground_region.cpp

@@ -654,7 +654,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         if(fabs(rough_width - accurate_width) > 0.15)
         {
             std::string disp_str = std::string("width difference too large, assume noise situation, ")+std::to_string(rough_width)+", "+std::to_string(accurate_width);
-            LOG(WARNING) << disp_str;
+            LOG_EVERY_N(WARNING,100) << disp_str;
             return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, MINOR_ERROR, disp_str.c_str());
         }
 

+ 2 - 2
测量节点/velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -903,9 +903,9 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
     /*printf("it : %d ,summary loss: %.5f \n",
            summary.iterations.size(),loss);*/
 
-    if (detect_result.width < 1.25 || detect_result.width > 2.000 || detect_result.wheel_base > 3.15 || detect_result.wheel_base < 2.200)
+    if (detect_result.width < 1.25 || detect_result.width > 1.95 || detect_result.wheel_base > 3.15 || detect_result.wheel_base < 2.200)
     {
-        error_info.append(std::string("宽度(1.25, 2.00) 轴距(2.20, 3.15): ").append(
+        error_info.append(std::string("宽度(1.25, 1.950) 轴距(2.20, 3.15): ").append(
                 std::to_string(detect_result.width).append(", ").append(
                         std::to_string(detect_result.wheel_base)).append("\t")));
         return false;