普爱现场版本,wj增加滤波，暂时只打印滤波结果，输出不变。wj轴距限制3.15并传出状态，优化比例初值调整，模型调整，限制点云最小比例

error_code/error_code.h

@@ -213,7 +213,8 @@ enum Error_code
     WJ_LIDAR_TASK_INVALID_TASK,
     WJ_LIDAR_TASK_MEASURE_FAILED,
 
-
+    WJ_FILTER_LACK_OF_RESULT,
+    WJ_FILTER_FLUCTUATING,
 
 };
 

terminor/terminal_command_executor.cpp

@@ -497,13 +497,14 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     m_measure_information.locate_theta =angle_weight * wj_locate_information.locate_theta +
             (1.0-angle_weight) * dj_locate_information.locate_theta;
 
-    float dj_distance = fabs(dj_locate_information.locate_wheel_base - 2750);
+   /* float dj_distance = fabs(dj_locate_information.locate_wheel_base - 2750);
     float wj_distance = fabs(wj_locate_information.locate_wheel_base - 2750);
     float weight_dj = wj_distance / (dj_distance + wj_distance);
-    float weight_wj = dj_distance / (dj_distance + wj_distance);
-    m_measure_information.locate_wheel_base = weight_dj * dj_locate_information.locate_wheel_base +
-                                              weight_wj * wj_locate_information.locate_wheel_base;
-    m_measure_information.locate_wheel_base=m_measure_information.locate_wheel_base>3000?3000:m_measure_information.locate_wheel_base;
+    float weight_wj = dj_distance / (dj_distance + wj_distance);*/
+    m_measure_information.locate_wheel_base = 0.1 * dj_locate_information.locate_wheel_base +
+                                              0.9 * wj_locate_information.locate_wheel_base;
+//    m_measure_information.locate_wheel_base += 400;
+    //m_measure_information.locate_wheel_base=m_measure_information.locate_wheel_base>3000?3000:m_measure_information.locate_wheel_base;
     m_measure_information.locate_length = dj_locate_information.locate_length;
     m_measure_information.locate_width = wj_locate_information.locate_width;
     m_measure_information.locate_hight = dj_locate_information.locate_hight;

wj_lidar/detect_wheel_ceres.cpp

@@ -81,7 +81,7 @@ public:
         T wheel_length_img_ratio = variable[6];
         T wheel_width_length_ratio = variable[7];
         // [1/3, 5/3]
-        T limited_wheel_length_img_ratio = T(0.8) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(0.75);
+        T limited_wheel_length_img_ratio = T(0.6) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(0.9);
         // [0.25, 0.35]
         T limited_wheel_length_ratio = T(0.25) / (T(1) + ceres::exp(T(-wheel_width_length_ratio))) + T(0.25);
 
@@ -620,9 +620,9 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     double init_width=1.55;
     double init_theta_front=0*M_PI/180.0;
     // 车轮长图比, 比例取值1/3-5/3, 当前变量无限制
-    double init_wheel_length_img_ratio = 0;
+    double init_wheel_length_img_ratio = 0.5;
     // 车轮宽长比, 默认长为1,比例取值范围0.2-0.8, 当前变量范围无限制
-    double init_wheel_width_length_ratio = 0;
+    double init_wheel_width_length_ratio = 0.5;
 
     double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, init_wheel_length_img_ratio, init_wheel_width_length_ratio};
     // printf("solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
@@ -678,9 +678,9 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
 //        LOG(WARNING) <<"总loss过大 "<<loss;
         return false;
     }
-    if (detect_result.width < 1.25 || 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.15 || detect_result.wheel_base < 2.200)
     {
-        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")));
+        error_info.append(std::string("宽度(1.25, 2.00) 轴距(2.20, 3.15): ").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;
     }

wj_lidar/plc_data.cpp

@@ -82,6 +82,11 @@ void Plc_data::update_data(int state_code, int border_status, int id)
     std::lock_guard<std::mutex> lock(g_lock);
     m_data[2 * id] = state_code;
     m_data[2 * id + 1] = border_status;
+    // added by yct, 轴距超长限制
+    if(m_current_error_info[id].find("轴距") < m_current_error_info[id].size())
+    {
+        m_data[2 * id + 1] |= 0x0010;
+    }
 }
 
 void Plc_data::update_error_info(int id, std::string error_info)

wj_lidar/region_detect.cpp

@@ -432,10 +432,10 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud
         width = detect_result.width;
         front_wheel_theta = detect_result.front_theta;
         cloud_in->push_back(pcl::PointXYZ(x, y, 0));
-        cloud_in->push_back(pcl::PointXYZ(x+(width-0.15)/2.0 * cos((c - 90)*M_PI/180.0) - wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(width-0.15)/2.0 * sin((c - 90)*M_PI/180.0) + wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
-        cloud_in->push_back(pcl::PointXYZ(x+(width-0.15)/2.0 * cos((c - 90)*M_PI/180.0) + wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(width-0.15)/2.0 * sin((c - 90)*M_PI/180.0) - wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
-        cloud_in->push_back(pcl::PointXYZ(x+(-width+0.15)/2.0 * cos((c - 90)*M_PI/180.0) - wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(-width+0.15)/2.0 * sin((c - 90)*M_PI/180.0) + wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
-        cloud_in->push_back(pcl::PointXYZ(x+(-width+0.15)/2.0 * cos((c - 90)*M_PI/180.0) + wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(-width+0.15)/2.0 * sin((c - 90)*M_PI/180.0) - wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
+        cloud_in->push_back(pcl::PointXYZ(x+(width-0.22)/2.0 * cos((c - 90)*M_PI/180.0) - wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(width-0.22)/2.0 * sin((c - 90)*M_PI/180.0) + wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
+        cloud_in->push_back(pcl::PointXYZ(x+(width-0.22)/2.0 * cos((c - 90)*M_PI/180.0) + wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(width-0.22)/2.0 * sin((c - 90)*M_PI/180.0) - wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
+        cloud_in->push_back(pcl::PointXYZ(x+(-width+0.22)/2.0 * cos((c - 90)*M_PI/180.0) - wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(-width+0.22)/2.0 * sin((c - 90)*M_PI/180.0) + wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
+        cloud_in->push_back(pcl::PointXYZ(x+(-width+0.22)/2.0 * cos((c - 90)*M_PI/180.0) + wheelbase/2.0*sin((c - 90)*M_PI/180.0), y+(-width+0.22)/2.0 * sin((c - 90)*M_PI/180.0) - wheelbase/2.0*cos((c - 90)*M_PI/180.0), 0));
     }
 
     return Error_manager(Error_code::SUCCESS);

wj_lidar/region_worker.cpp

@@ -3,6 +3,7 @@
 //
 #include "region_worker.h"
 #include "plc_data.h"
+#include "measure_filter.h"
 
 /**
  * 有参构造
@@ -94,6 +95,25 @@ Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Pt
     {
         LOG(INFO) << "worker getting result";
 		result = m_detector->detect(cloud_in, x, y, c, wheelbase, width, front_theta);
+		// 增加滤波后结果，暂时只打印对比结果
+        Common_data::Car_wheel_information t_wheel_info;
+        Error_manager ec = Measure_filter::get_instance_references().get_filtered_wheel_information(get_id(), t_wheel_info);
+        if(ec ==SUCCESS) {
+            char buf[512] = {0};
+            sprintf(buf,
+                    "\nx %.3f->%.3f\n"
+                    "y %.3f->%.3f\n"
+                    "c %.3f->%.3f\n"
+                    "wheelbase %.3f->%.3f\n"
+                    "width %.3f->%.3f\n"
+                    "front %.3f->%.3f\n",
+                    x, t_wheel_info.center_x, y, t_wheel_info.center_y, c, t_wheel_info.car_angle, wheelbase,
+                    t_wheel_info.wheel_base, width, t_wheel_info.wheel_width, front_theta, t_wheel_info.front_theta
+            );
+            LOG(WARNING) << buf;
+        }else{
+            LOG(WARNING) << ec.to_string();
+        }
     }
     else
     {
@@ -183,6 +203,23 @@ void Region_worker::detect_loop(Region_worker *worker)
 //                    LOG(WARNING) << "region worker width: " << width;
                     border_status |= 0x000020;//第六位为车辆宽度异常
                 }
+
+                // 测量正确,更新到滤波器
+                if(terminal_id>=0) {
+                    Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
+                    t_wheel_info_stamped.wheel_data.correctness = true;
+                    t_wheel_info_stamped.wheel_data.center_x = x;
+                    t_wheel_info_stamped.wheel_data.center_y = y;
+                    t_wheel_info_stamped.wheel_data.car_angle = angle;
+                    t_wheel_info_stamped.wheel_data.front_theta = front_theta;
+                    t_wheel_info_stamped.wheel_data.wheel_width = width;
+                    t_wheel_info_stamped.wheel_data.wheel_base = wheelbase;
+                    t_wheel_info_stamped.measure_time = std::chrono::system_clock::now();;
+                    Measure_filter::get_instance_references().update_data(terminal_id, t_wheel_info_stamped);
+//                    if(terminal_id == 5)
+//                        LOG(WARNING) <<terminal_id<<", "<<Measure_filter::get_instance_references().has_enough_data(terminal_id);
+                }
+
             }
             else
             {

wj_lidar/wj_lidar_encapsulation.cpp

@@ -172,7 +172,7 @@ Error_manager Wj_lidar_encapsulation::get_cloud(pcl::PointCloud<pcl::PointXYZ>::
       // 内部已打印错误信息
       return code;
     }
-    usleep(1000 * 50);
+    usleep(1000 * 10);
   }
   return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT);
 }