普爱现场版本,wj debug,车宽过窄判断调整,20201209

wj_lidar/detect_wheel_ceres.cpp

@@ -457,6 +457,9 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
     // Loss_result t_loss_result;
     // // LOG(INFO) <<"going to show img ";
     // Solve(detect_result, t_loss_result, true);
+
+//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+//         detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
     if(solve_result)
         error_info="";
     return solve_result;
@@ -675,15 +678,16 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
 //        LOG(WARNING) <<"总loss过大 "<<loss;
         return false;
     }
-    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.200 || detect_result.wheel_base < 2.200)
+    if (detect_result.width < 1.25 || detect_result.width > 2.000 || detect_result.wheel_base > 3.200 || detect_result.wheel_base < 2.200)
     {
-        error_info.append(std::string("宽度(1.35, 2.00) 轴距(2.20, 3.20): ").append(std::to_string(detect_result.width).append(", ").append(std::to_string(detect_result.wheel_base)).append("\t")));
+        error_info.append(std::string("宽度(1.25, 2.00) 轴距(2.20, 3.20): ").append(std::to_string(detect_result.width).append(", ").append(std::to_string(detect_result.wheel_base)).append("\t")));
 //        LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.30): "<<detect_result.width<<", "<<detect_result.wheel_base;
         return false;
     }
 
     detect_result.width += 0.22; //车宽一边+11cm
-    // printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
+//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+//             detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
 
     // //added by yct
     if(detect_result.theta > 120 || detect_result.theta < 60)
@@ -743,7 +747,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     Detect_result t_detect_result = detect_result;
     t_detect_result.theta *= -M_PI/180.0;
     t_detect_result.front_theta *= -M_PI/180.0;
-    t_detect_result.width -= 0.15;
+    t_detect_result.width -= 0.22;
     transform_src(t_detect_result);
 
     return true;

wj_lidar/region_worker.cpp

@@ -146,8 +146,8 @@ void Region_worker::detect_loop(Region_worker *worker)
             worker->mb_cloud_updated = false;
             double x,y,angle,wheelbase,width,front_theta;
             std::string t_error_info_str="";
-            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, wheelbase, width,front_theta, t_error_info_str, false);
-//            LOG(WARNING) << "worker id: " << worker->get_id()<<" str: "<<t_error_info_str;
+            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, front_theta, wheelbase, width, t_error_info_str, false);
+//            LOG(WARNING) << "worker id: " << worker->get_id()<<" width: "<<width;
             if(worker->mp_verify_handle == nullptr)
             {
                 LOG(WARNING) << "verify handle null pointer";
@@ -178,9 +178,10 @@ void Region_worker::detect_loop(Region_worker *worker)
                     //LOG(WARNING) << "region worker verify result: " << code;
                 }
                 // added by yct, check car width, width轮距+22cm
-                if(width > 1.92)
+                if(width > 1.92 || width < 1.55)
                 {
-                    border_status |= 0x000020;//第六位为车辆超宽
+//                    LOG(WARNING) << "region worker width: " << width;
+                    border_status |= 0x000020;//第六位为车辆宽度异常
                 }
             }
             else