普爱现场版本,20201209

terminor/terminal_command_executor.cpp

@@ -482,8 +482,8 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     float offset_width = fabs(dj_locate_information.locate_width - wj_locate_information.locate_width);
     float offset_wheel_base = fabs(
             dj_locate_information.locate_wheel_base - wj_locate_information.locate_wheel_base);
-    if (offset_x > 100 || offset_y > 150 || offset_angle > 3 || offset_wheel_base > 550 || offset_width > 250) {
-        LOG(WARNING) << "chekc failed. (offset x>100, y>150, angle>3, wheel_base>550, width>250): " << offset_x
+    if (offset_x > 100 || offset_y > 150 || offset_angle > 2.5 || offset_wheel_base > 550 || offset_width > 250) {
+        LOG(WARNING) << "chekc failed. (offset x>100, y>150, angle>2.5, wheel_base>550, width>250): " << offset_x
                      << " " << offset_y << " " << offset_angle << " " << offset_wheel_base << " " << offset_width;
         return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
     }

test/plc_s7.cpp

@@ -32,10 +32,10 @@ int main()
                     continue;
                 }
                 char buf[255] = {0};
-                sprintf(buf, " terminal status: ");
+                sprintf(buf, " terminal status: -----------------------------------------\n");
                 for (int i = 0; i < fence_statu.fence_statu_size(); ++i) {
-                    sprintf(buf+strlen(buf), "[%d, %d]   ", fence_statu.fence_statu(i).cloud_statu(),
-                            fence_statu.fence_statu(i).position_statu());
+                    sprintf(buf+strlen(buf), "%d: [%d, %d]   %s\n", fence_statu.fence_statu(i).terminal_id(), fence_statu.fence_statu(i).cloud_statu(),
+                            fence_statu.fence_statu(i).position_statu(), fence_statu.fence_statu(i).has_error_info()?fence_statu.fence_statu(i).error_info().c_str():"");
                 }
                 std::cout << buf << std::endl;
             }

wj_lidar/detect_wheel_ceres.cpp

@@ -7,7 +7,7 @@
 #include <pcl/common/transforms.h>
 
 
-constexpr float kMinProbability = 0.0f;
+constexpr float kMinProbability = 0.01f;
 constexpr float kMaxProbability = 1.f - kMinProbability;
 constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
 constexpr int kPadding = INT_MAX / 4;
@@ -63,10 +63,13 @@ public:
         m_costs_lr = 0.0;
         m_costs_rr = 0.0;
     }
-    template <typename T>
+        template <typename T>
     bool operator()(const T* const variable, T* residual) const {
         // 每一轮重新初始化残差值
         T costs[4]={T(0),T(0),T(0),T(0)};
+	std::vector<std::vector<T> > deviation_calc_vector;
+	deviation_calc_vector.resize(4);
+	T deviation_weight = T(0.05);
 
         T cx = variable[0];
         T cy = variable[1];
@@ -74,6 +77,13 @@ public:
         T length = variable[3];
         T width = variable[4];
         T theta_front = variable[5];
+        T theta_front_weight = T(0.8);
+        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);
+        // [0.25, 0.35]
+        T limited_wheel_length_ratio = T(0.25) / (T(1) + ceres::exp(T(-wheel_width_length_ratio))) + T(0.25);
 
         //整车旋转
         Eigen::Rotation2D<T> rotation(theta);
@@ -107,11 +117,12 @@ public:
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
             //旋转
             const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[i]);
+            //residual[i] += theta_front*theta_front_weight;
             costs[0] += residual[i];
-
+	    deviation_calc_vector[0].push_back(residual[i]);
         }
         //右前轮
         int right_front_num = m_right_front_cloud.size();
@@ -122,10 +133,12 @@ public:
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
             //旋转
             const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[left_front_num + i]);
+            //residual[left_front_num + i] += theta_front*theta_front_weight;
             costs[1] += residual[left_front_num+i];
+	    deviation_calc_vector[1].push_back(residual[left_front_num+i]);
         }
         //左后轮
         int left_rear_num = m_left_rear_cloud.size();
@@ -136,44 +149,63 @@ public:
             //减去经过车辆旋转计算的左前中心
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
 
-            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[left_front_num + right_front_num + i]);
             costs[2] += residual[left_front_num + right_front_num + i];
+	    deviation_calc_vector[2].push_back(residual[left_front_num + right_front_num + i]);
         }
 
         //右后轮
-
+	int right_rear_num = m_right_rear_cloud.size();
         for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
             const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
                                                (T(m_right_rear_cloud[i].y) - cy));
             //减去经过车辆旋转计算的左前中心
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
 
-            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[left_front_num + right_front_num + left_rear_num + i]);
             costs[3] += residual[left_front_num + right_front_num + left_rear_num + i];
+	    deviation_calc_vector[3].push_back(residual[left_front_num + right_front_num + left_rear_num + i]);
         }
 
+	// 前轮旋转loss
+	residual[left_front_num + right_front_num + left_rear_num + right_rear_num] = theta_front*theta_front_weight;
+	// 四轮方差loss
+	residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] = T(0);
+	//printf("--------------\n");
+	T avg_costs[4] = {T(costs[0]/T(left_front_num)), T(costs[1]/T(right_front_num)), T(costs[2]/T(left_rear_num)), T(costs[3]/T(right_rear_num))};
+	for (int i = 0; i < deviation_calc_vector.size(); ++i) {
+		T t_deviation = T(0);
+		for (int j = 0; j < deviation_calc_vector[i].size(); ++j) {
+			t_deviation += deviation_weight * ceres::abs(deviation_calc_vector[i][j] - avg_costs[i]);
+		}
+		residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] += t_deviation;
+		//printf("??? dev: %.6f\n", t_deviation);
+		//costs[i] = t_deviation;
+		//printf("dev: %.6f\n", costs[i]);
+	}
+
         char buf[30];
         memset(buf, 0, 30);
-        sprintf(buf, "%.7f", costs[0]);
+        sprintf(buf, "%.12f", costs[0]);
         m_costs_lf = std::stod(buf) / left_front_num;
 
         memset(buf, 0, 30);
-        sprintf(buf, "%.7f", costs[1]);
+        sprintf(buf, "%.12f", costs[1]);
         m_costs_rf = std::stod(buf) / right_front_num;
 
         memset(buf, 0, 30);
-        sprintf(buf, "%.7f", costs[2]);
+        sprintf(buf, "%.12f", costs[2]);
         m_costs_lr = std::stod(buf) / left_rear_num;
 
-
         memset(buf, 0, 30);
-        sprintf(buf, "%.7f", costs[3]);
-        m_costs_rr = std::stod(buf) / m_right_rear_cloud.size();
+        sprintf(buf, "%.12f", costs[3]);
+        m_costs_rr = std::stod(buf) / right_rear_num;
         // m_costs_lf = costs[0];
+	//printf("%.6f %.6f %.6f %.6f\n",m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr);
         return true;
     }
     void get_costs(double &lf, double &rf, double &lr, double &rr)
@@ -202,12 +234,12 @@ bool detect_wheel_ceres::update_mat(int rows, int cols)
         cv::Size size(n+2,n+2);
         cv::Rect rect(center-cv::Point(n/2,n/2),size);
         double x = n / double(L);
-        double sigma = 0.06;
+        double sigma = 0.05;
         double miu = 0.0;
         double scale = 0.02;
-        double translation = -0.05;
+        double translation = 0.03;
         double gauss_value = (scale/(sqrt(2*M_PI)*sigma))*exp(-pow(x-miu, 2)/(2*sigma*sigma))+translation;
-        double quadratic_value = x-0.08;
+        double quadratic_value = std::max(2*x-0.25, 0.045);
         // LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
         // 对内外分别取色
         cv::rectangle(map,rect,std::max(gauss_value, quadratic_value));
@@ -227,15 +259,16 @@ bool detect_wheel_ceres::update_mat(int rows, int cols)
 detect_wheel_ceres::detect_wheel_ceres()
 {
     int cols=800;
-    int rows=200;
+    int rows=800;
     update_mat(rows, cols);
 }
 detect_wheel_ceres::~detect_wheel_ceres(){}
 
 
 
-bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result)
+bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result, std::string &error_info)
 {
+    error_info="";
     //清理点云
     m_left_front_cloud.clear();
     m_right_front_cloud.clear();
@@ -357,14 +390,16 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
     bool solve_result = false;
     double total_solve_time = 0;
     bool stop_sign = false;
-    for (int j = 2; j < 4&&!stop_sign; j++)
-    {
-        // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
-        double map_rows = map_cols * 1.0 / j ;
-        update_mat(map_rows, map_cols);
+    // for (int j = 2; j < 4&&!stop_sign; j++)
+    // {
+    //     // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
+    //     double map_rows = map_cols * 1.0 / j ;
+    //     update_mat(map_rows, map_cols);
+
+    // !!! 将车轮比例添加到优化模型中
         Detect_result t_detect_result = detect_result;
-        // 寻找最小loss值对应的初始旋转角
-        for (int i = -5; i < 6&&!stop_sign; i+=2)
+       // 寻找最小loss值对应的初始旋转角
+        for (int i = -3; i < 4&&!stop_sign; i+=6)
         {
             t_detect_result.theta = (-angle_x + i) * M_PI / 180.0;
             // printf("double x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
@@ -377,7 +412,9 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
                 t_detect_result.front_theta *= (-M_PI) / 180.0;
             }
             auto t1=std::chrono::system_clock::now();
-            bool current_result = Solve(t_detect_result, t_loss_result);
+	    std::string t_error_info;
+            bool current_result = Solve(t_detect_result, t_loss_result, t_error_info);
+    	    error_info = t_error_info;
             auto t2=std::chrono::system_clock::now();
             auto duration=t2-t1;
             static double second=0.0;
@@ -395,17 +432,17 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
                 detect_result = t_detect_result;
                 solve_result = current_result;
                 loss_result = t_loss_result;
-                optimized_map_rows = map_rows;
+                // optimized_map_rows = map_rows;
                 calc_count++;
-                /*// 新增，优化时间足够长则认为已找到正确结果
-                if(second > 0.02)
-                {
-                    stop_sign = true;
-                    break;
-                }*/
+                // // 新增，优化时间足够长则认为已找到正确结果
+                // if(second > 0.02)
+                // {
+                //     stop_sign = true;
+                //     break;
+                // }
             }
         }
-    } 
+    // }
     // std::cout<<"solve time "<<total_solve_time<<std::endl;
     // LOG(INFO) << "final avg cost for four wheels: "<<whole_loss[0]<<" "<<whole_loss[1]<<" "<<whole_loss[2]<<" "<<whole_loss[3]<<" ";
 
@@ -420,7 +457,8 @@ 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);
-
+    if(solve_result)
+        error_info="";
     return solve_result;
 
 }
@@ -569,7 +607,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, double &avg_loss)
 }
 
 
-bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img)
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info, bool save_img)
 {
     double SCALE=300.0;
     double cx=detect_result.cx;
@@ -578,17 +616,21 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     double init_wheel_base=2.7;
     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;
+    // 车轮宽长比, 默认长为1,比例取值范围0.2-0.8, 当前变量范围无限制
+    double init_wheel_width_length_ratio = 0;
 
-    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
+    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]);
     // 第二部分：构建寻优问题
     ceres::Problem problem;
     CostFunctor *cost_func = new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE);
     //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
     ceres::CostFunction* cost_function =new
-            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 8>(
                     cost_func,
-                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size()+2);
     problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
 
@@ -596,12 +638,18 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     ceres::Solver::Options options;
     options.use_nonmonotonic_steps=false;
     options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    options.logging_type = ceres::LoggingType::SILENT;
     options.max_num_iterations=60;
     options.num_threads=3;
     options.minimizer_progress_to_stdout = false;//输出到cout
     ceres::Solver::Summary summary;//优化信息
     ceres::Solve(options, &problem, &summary);//求解!!!
 
+//    if(!summary.IsSolutionUsable())
+//    {
+//        error_info.append("检测失败\t");
+//        return false;
+//    }
     // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
     // debug_img(detect_result, loss_result, true);
     double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
@@ -621,14 +669,16 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     //检验
     // printf("loss: %.5f\n", loss);
     // printf("middle 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]);
-    if(loss>0.0115)
+    if(loss>0.005)
     {
+        error_info.append("总loss过大 ").append(std::to_string(loss)).append("\t");
 //        LOG(WARNING) <<"总loss过大 "<<loss;
         return false;
     }
-    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.150 || detect_result.wheel_base < 2.200)
+    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.200 || detect_result.wheel_base < 2.200)
     {
-//        LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.15): "<<detect_result.width<<", "<<detect_result.wheel_base;
+        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")));
+//        LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.30): "<<detect_result.width<<", "<<detect_result.wheel_base;
         return false;
     }
 
@@ -638,11 +688,13 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     // //added by yct
     if(detect_result.theta > 120 || detect_result.theta < 60)
     {
+        error_info.append("总角度错误 ").append(std::to_string(detect_result.theta)).append("\t");
 //        LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
         return false;
     }
     if(fabs(detect_result.front_theta)>35)
     {
+        error_info.append("前轮角度错误 ").append(std::to_string(detect_result.front_theta)).append("\t");
 //        LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
         return false;
     }
@@ -659,17 +711,23 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
         loss_result.rb_loss = costs_rr;
         loss_result.total_avg_loss = loss;
         // std::cout << "222"<< std::endl;
-        if (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
+        if (costs_lf == costs_rf && costs_lf == costs_lf && costs_lf == costs_rr)// (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
         {
+	    return false;
             loss_result.lf_loss = loss;
             loss_result.rf_loss = loss;
             loss_result.lb_loss = loss;
             loss_result.rb_loss = loss;
         }
         // 判断每个轮子平均loss是否满足条件
-        double single_wheel_loss_threshold = 0.109;
+        double single_wheel_loss_threshold = 0.5;
         if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold  || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
         {
+            error_info.append("四轮loss过大 ").append(std::to_string(loss_result.lf_loss)).append(", ")
+                    .append(std::to_string(loss_result.rf_loss)).append(", ")
+                    .append(std::to_string(loss_result.lb_loss)).append(", ")
+                    .append(std::to_string(loss_result.rb_loss)).append("\t");
+//            LOG(WARNING)<<error_info;
 //            LOG(WARNING) <<"四轮loss过大"<<loss_result.lf_loss<<", "<<loss_result.rf_loss<<", "<<loss_result.lb_loss<<", "<<loss_result.rb_loss;
             return false;
         }

wj_lidar/detect_wheel_ceres.h

@@ -60,13 +60,13 @@ public:
     };
     detect_wheel_ceres();
     ~detect_wheel_ceres();
-    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result);
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result, std::string &error_info);
 
 protected:
     void transform_src(Detect_result detect_result);
     bool update_mat(int rows, int cols);
     bool Solve(Detect_result &detect_result, double &avg_loss);
-    bool Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img=false);
+    bool Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info, bool save_img=false);
     void debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img=false, std::string out_img_path="");
 
     void create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud);

wj_lidar/fence_controller.cpp

@@ -366,6 +366,11 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                 wj_measure_temp.wheel_base = wheelbase*1000.0;
                 wj_measure_temp.width = width * 1000.0;
                 wj_measure_temp.correctness = correctness;
+                // 20201109 added by yct. 6号车位机械累计误差，测量17407与机械手17370同位置，软件修正-15mm
+                if(terminal_id == 5)
+                {
+                    wj_measure_temp.x -= 15;
+                }
                 if (wj_measure_temp.correctness){
                     task_temp->set_result(wj_measure_temp);
                     char description[255]={0};

wj_lidar/plc_data.cpp

@@ -84,6 +84,14 @@ void Plc_data::update_data(int state_code, int border_status, int id)
     m_data[2 * id + 1] = border_status;
 }
 
+void Plc_data::update_error_info(int id, std::string error_info)
+{
+    if(id<0 || id>=MAX_REGIONS)
+        return ;
+    std::lock_guard<std::mutex> lock(g_lock);
+    m_current_error_info[id] = error_info;
+}
+
 /**
  * plc更新线程，将区域状态写入plc
  * */
@@ -118,6 +126,7 @@ void Plc_data::plc_update_thread(Plc_data *p)
                 fence_message.mutable_fence_statu(index)->set_terminal_id(i);
                 fence_message.mutable_fence_statu(index)->set_cloud_statu(p->m_data[i * 2]);
                 fence_message.mutable_fence_statu(index)->set_position_statu(p->m_data[i * 2+1]);
+                fence_message.mutable_fence_statu(index)->set_error_info(p->m_current_error_info[i]);
 
                 g_lock.unlock();
                 usleep(10 * 1000);

wj_lidar/plc_data.h

@@ -33,6 +33,8 @@ public:
     // static void Release();
     // 更新区域状态数据
     void update_data(int state_code, int border_status, int id);
+    // 更新错误信息提示
+    void update_error_info(int id, std::string error_info);
     // plc更新线程
     static void plc_update_thread(Plc_data *p);
     // 获取plc实时状态
@@ -70,6 +72,7 @@ protected:
     short m_data[MAX_REGIONS*2];
     short m_last_data[MAX_REGIONS*2];
     std::string m_ip_str;
+    std::string m_current_error_info[6];
 };
 
 #endif //PLC_DATA_H

wj_lidar/region_detect.cpp

@@ -66,8 +66,8 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     // 3.离群点过滤
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_out);
-    sor.setMeanK(15);            //K近邻搜索点个数
-    sor.setStddevMulThresh(3.0); //标准差倍数
+    sor.setMeanK(10);            //K近邻搜索点个数
+    sor.setStddevMulThresh(1.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_out);      //保存滤波结果到cloud_filter
 
@@ -418,8 +418,51 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud
     if (result != SUCCESS)
         return result;
 
+    cloud_in->clear();
+    cloud_in->operator+=(*cloud_filtered);
+    detect_wheel_ceres::Detect_result detect_result{x,y,c,wheelbase,width,front_wheel_theta};
+    std::string error_info;
+    if(!m_detector_ceres.detect(seg_clouds, detect_result, error_info))
+        return WJ_REGION_CERES_SOLVE_ERROR;
+    else {
+        x = detect_result.cx;
+        y = detect_result.cy;
+        c = detect_result.theta;
+        wheelbase = detect_result.wheel_base;
+        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));
+    }
+
+    return Error_manager(Error_code::SUCCESS);
+}
+
+//增加异常信息传出
+Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,double& front_wheel_theta,
+                     double &wheelbase, double &width, std::string &error_info, bool print)
+{
+    Error_manager result = Error_manager(Error_code::SUCCESS);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    // 1.预处理
+    result = preprocess(cloud_in, cloud_filtered);
+    if (result != SUCCESS) {
+        error_info="预处理失败";
+        return result;
+    }
+
+    // 2.聚类
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
+    result = clustering_only(cloud_filtered, seg_clouds, print);
+    if (result != SUCCESS) {
+        error_info="聚类失败";
+        return result;
+    }
     detect_wheel_ceres::Detect_result detect_result{x,y,c,wheelbase,width,front_wheel_theta};
-    if(!m_detector_ceres.detect(seg_clouds, detect_result))
+    if(!m_detector_ceres.detect(seg_clouds, detect_result, error_info))
         return WJ_REGION_CERES_SOLVE_ERROR;
     else {
         x = detect_result.cx;

wj_lidar/region_detect.h

@@ -59,6 +59,9 @@ public:
     //通过ceres检测中心,旋转,轴距,宽度,前轮旋转
     Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,double& front_wheel_theta,
             double &wheelbase, double &width, bool print=true);
+    //通过ceres检测中心,旋转,轴距,宽度,前轮旋转,增加异常信息传出
+    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,double& front_wheel_theta,
+                         double &wheelbase, double &width, std::string &error_info, bool print=true);
     // 获得区域id
     int get_region_id();
 

wj_lidar/region_worker.cpp

@@ -134,7 +134,7 @@ void Region_worker::detect_loop(Region_worker *worker)
     if (worker->m_detector == 0)
         return;
     // 2.检测与更新循环
-    while (!worker->m_cond_exit.WaitFor(100))
+    while (!worker->m_cond_exit.WaitFor(50))
     {
 //        LOG(INFO) << "worker detect loop";
         // 检查当前状态
@@ -145,8 +145,9 @@ void Region_worker::detect_loop(Region_worker *worker)
             int border_status = REGION_WORKER_RESULT_DEFAULT;
             worker->mb_cloud_updated = false;
             double x,y,angle,wheelbase,width,front_theta;
-            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, wheelbase, width,front_theta, false);
-
+            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;
             if(worker->mp_verify_handle == nullptr)
             {
                 LOG(WARNING) << "verify handle null pointer";
@@ -191,13 +192,18 @@ void Region_worker::detect_loop(Region_worker *worker)
             worker->m_read_code_count += 1;
 
 //            if(worker->get_id() == 1) {
+////                LOG(WARNING) << "worker id: " << worker->get_id()<<" str: "<<t_error_info_str;
 //                LOG(WARNING) << "worker id: " << worker->get_id()<<" code: "<<code<<", "<<border_status << ", x y: "<<x<< ", "<<y;
 //            }
+
             // 写入plc，加入写入频率限制
             int duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - worker->m_update_plc_time).count();
             Plc_data *p = Plc_data::get_instance();
-            // 写入间隔必须超过200ms，当前状态不同于上次写入状态，且该状态已连续读到三次
-            if (p!=0 && duration > 200 && ((worker->m_last_sent_code != worker->m_last_read_code) || worker->m_last_border_status != border_status) && worker->m_read_code_count > 3)
+            if(p!=0) {
+                p->update_error_info(worker->m_detector->get_region_id(), t_error_info_str);
+            }
+            // 写入间隔必须超过50ms，当前状态不同于上次写入状态，且该状态已连续读到两次
+            if (p!=0 && duration > 50 && ((worker->m_last_sent_code != worker->m_last_read_code) || worker->m_last_border_status != border_status) && worker->m_read_code_count >= 2)
             {
                 worker->m_last_sent_code = worker->m_last_read_code;
 		        worker->m_last_border_status = border_status;

wj_lidar/wj_716_lidar_protocol.cpp

@@ -391,7 +391,13 @@ Error_manager Wj_716_lidar_protocol::protocol(const char *data, const int len)
           point.x = m_scandata[j];
           point.y = 0;
           point.z = 0;
-          mp_scan_points->push_back(point);
+	  //LOG(INFO)<<"indensity: "<<scaninden[j];
+	  if(scaninden[j] <49.0f || scaninden[j] > 1500.0f)
+	  {
+          	point.x = INT_MAX;
+                //LOG(INFO)<<"limited indensity: "<<scaninden[j];
+          }
+	  mp_scan_points->push_back(point);
           // mp_scan_points->points[j - index_min].x = m_scandata[j];
           // mp_scan_points->points[j - index_min].y = 0;
           // mp_scan_points->points[j - index_min].z = 0;
@@ -402,7 +408,7 @@ Error_manager Wj_716_lidar_protocol::protocol(const char *data, const int len)
         }
         m_scan_mutex.unlock();
         m_scan_cloud_time = std::chrono::steady_clock::now();
-        // LOG(INFO) << "protocol cloud size: " << mp_scan_points->size();
+        //LOG(INFO) << "protocol cloud size: " << mp_scan_points->size();
       }
       else
       {
@@ -529,4 +535,4 @@ bool Wj_716_lidar_protocol::get_initialize_status()
   return mb_initialized;
 }
 
-} // namespace wj_lidar
+} // namespace wj_lidar

wj_lidar/wj_lidar_msg.pb.cc

@@ -86,9 +86,11 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::wj_lidar_message::Terminal_fence_statu, terminal_id_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::wj_lidar_message::Terminal_fence_statu, cloud_statu_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::wj_lidar_message::Terminal_fence_statu, position_statu_),
-  0,
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::wj_lidar_message::Terminal_fence_statu, error_info_),
   1,
   2,
+  3,
+  0,
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::wj_lidar_message::Fence_statu_message, _has_bits_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::wj_lidar_message::Fence_statu_message, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -98,8 +100,8 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   ~0u,
 };
 static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
-  { 0, 8, sizeof(::wj_lidar_message::Terminal_fence_statu)},
-  { 11, 17, sizeof(::wj_lidar_message::Fence_statu_message)},
+  { 0, 9, sizeof(::wj_lidar_message::Terminal_fence_statu)},
+  { 13, 19, sizeof(::wj_lidar_message::Fence_statu_message)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
@@ -129,15 +131,15 @@ void protobuf_RegisterTypes(const ::std::string&) {
 void AddDescriptorsImpl() {
   InitDefaults();
   static const char descriptor[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
-      "\n\022wj_lidar_msg.proto\022\020wj_lidar_message\"X"
+      "\n\022wj_lidar_msg.proto\022\020wj_lidar_message\"l"
       "\n\024Terminal_fence_statu\022\023\n\013terminal_id\030\001 "
       "\002(\003\022\023\n\013cloud_statu\030\002 \002(\003\022\026\n\016position_sta"
-      "tu\030\003 \002(\003\"R\n\023Fence_statu_message\022;\n\013fence"
-      "_statu\030\001 \003(\0132&.wj_lidar_message.Terminal"
-      "_fence_statu"
+      "tu\030\003 \002(\003\022\022\n\nerror_info\030\004 \001(\t\"R\n\023Fence_st"
+      "atu_message\022;\n\013fence_statu\030\001 \003(\0132&.wj_li"
+      "dar_message.Terminal_fence_statu"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 212);
+      descriptor, 232);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "wj_lidar_msg.proto", &protobuf_RegisterTypes);
 }
@@ -163,6 +165,7 @@ void Terminal_fence_statu::InitAsDefaultInstance() {
 const int Terminal_fence_statu::kTerminalIdFieldNumber;
 const int Terminal_fence_statu::kCloudStatuFieldNumber;
 const int Terminal_fence_statu::kPositionStatuFieldNumber;
+const int Terminal_fence_statu::kErrorInfoFieldNumber;
 #endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
 
 Terminal_fence_statu::Terminal_fence_statu()
@@ -179,6 +182,10 @@ Terminal_fence_statu::Terminal_fence_statu(const Terminal_fence_statu& from)
       _has_bits_(from._has_bits_),
       _cached_size_(0) {
   _internal_metadata_.MergeFrom(from._internal_metadata_);
+  error_info_.UnsafeSetDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  if (from.has_error_info()) {
+    error_info_.AssignWithDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), from.error_info_);
+  }
   ::memcpy(&terminal_id_, &from.terminal_id_,
     static_cast<size_t>(reinterpret_cast<char*>(&position_statu_) -
     reinterpret_cast<char*>(&terminal_id_)) + sizeof(position_statu_));
@@ -187,6 +194,7 @@ Terminal_fence_statu::Terminal_fence_statu(const Terminal_fence_statu& from)
 
 void Terminal_fence_statu::SharedCtor() {
   _cached_size_ = 0;
+  error_info_.UnsafeSetDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
   ::memset(&terminal_id_, 0, static_cast<size_t>(
       reinterpret_cast<char*>(&position_statu_) -
       reinterpret_cast<char*>(&terminal_id_)) + sizeof(position_statu_));
@@ -198,6 +206,7 @@ Terminal_fence_statu::~Terminal_fence_statu() {
 }
 
 void Terminal_fence_statu::SharedDtor() {
+  error_info_.DestroyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
 }
 
 void Terminal_fence_statu::SetCachedSize(int size) const {
@@ -230,7 +239,11 @@ void Terminal_fence_statu::Clear() {
   (void) cached_has_bits;
 
   cached_has_bits = _has_bits_[0];
-  if (cached_has_bits & 7u) {
+  if (cached_has_bits & 0x00000001u) {
+    GOOGLE_DCHECK(!error_info_.IsDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited()));
+    (*error_info_.UnsafeRawStringPointer())->clear();
+  }
+  if (cached_has_bits & 14u) {
     ::memset(&terminal_id_, 0, static_cast<size_t>(
         reinterpret_cast<char*>(&position_statu_) -
         reinterpret_cast<char*>(&terminal_id_)) + sizeof(position_statu_));
@@ -291,6 +304,22 @@ bool Terminal_fence_statu::MergePartialFromCodedStream(
         break;
       }
 
+      // optional string error_info = 4;
+      case 4: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(34u /* 34 & 0xFF */)) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadString(
+                input, this->mutable_error_info()));
+          ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+            this->error_info().data(), static_cast<int>(this->error_info().length()),
+            ::google::protobuf::internal::WireFormat::PARSE,
+            "wj_lidar_message.Terminal_fence_statu.error_info");
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
       default: {
       handle_unusual:
         if (tag == 0) {
@@ -319,20 +348,30 @@ void Terminal_fence_statu::SerializeWithCachedSizes(
 
   cached_has_bits = _has_bits_[0];
   // required int64 terminal_id = 1;
-  if (cached_has_bits & 0x00000001u) {
+  if (cached_has_bits & 0x00000002u) {
     ::google::protobuf::internal::WireFormatLite::WriteInt64(1, this->terminal_id(), output);
   }
 
   // required int64 cloud_statu = 2;
-  if (cached_has_bits & 0x00000002u) {
+  if (cached_has_bits & 0x00000004u) {
     ::google::protobuf::internal::WireFormatLite::WriteInt64(2, this->cloud_statu(), output);
   }
 
   // required int64 position_statu = 3;
-  if (cached_has_bits & 0x00000004u) {
+  if (cached_has_bits & 0x00000008u) {
     ::google::protobuf::internal::WireFormatLite::WriteInt64(3, this->position_statu(), output);
   }
 
+  // optional string error_info = 4;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->error_info().data(), static_cast<int>(this->error_info().length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "wj_lidar_message.Terminal_fence_statu.error_info");
+    ::google::protobuf::internal::WireFormatLite::WriteStringMaybeAliased(
+      4, this->error_info(), output);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
         _internal_metadata_.unknown_fields(), output);
@@ -349,20 +388,31 @@ void Terminal_fence_statu::SerializeWithCachedSizes(
 
   cached_has_bits = _has_bits_[0];
   // required int64 terminal_id = 1;
-  if (cached_has_bits & 0x00000001u) {
+  if (cached_has_bits & 0x00000002u) {
     target = ::google::protobuf::internal::WireFormatLite::WriteInt64ToArray(1, this->terminal_id(), target);
   }
 
   // required int64 cloud_statu = 2;
-  if (cached_has_bits & 0x00000002u) {
+  if (cached_has_bits & 0x00000004u) {
     target = ::google::protobuf::internal::WireFormatLite::WriteInt64ToArray(2, this->cloud_statu(), target);
   }
 
   // required int64 position_statu = 3;
-  if (cached_has_bits & 0x00000004u) {
+  if (cached_has_bits & 0x00000008u) {
     target = ::google::protobuf::internal::WireFormatLite::WriteInt64ToArray(3, this->position_statu(), target);
   }
 
+  // optional string error_info = 4;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->error_info().data(), static_cast<int>(this->error_info().length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "wj_lidar_message.Terminal_fence_statu.error_info");
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteStringToArray(
+        4, this->error_info(), target);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields(), target);
@@ -407,7 +457,7 @@ size_t Terminal_fence_statu::ByteSizeLong() const {
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
         _internal_metadata_.unknown_fields());
   }
-  if (((_has_bits_[0] & 0x00000007) ^ 0x00000007) == 0) {  // All required fields are present.
+  if (((_has_bits_[0] & 0x0000000e) ^ 0x0000000e) == 0) {  // All required fields are present.
     // required int64 terminal_id = 1;
     total_size += 1 +
       ::google::protobuf::internal::WireFormatLite::Int64Size(
@@ -426,6 +476,13 @@ size_t Terminal_fence_statu::ByteSizeLong() const {
   } else {
     total_size += RequiredFieldsByteSizeFallback();
   }
+  // optional string error_info = 4;
+  if (has_error_info()) {
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::StringSize(
+        this->error_info());
+  }
+
   int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = cached_size;
@@ -456,14 +513,18 @@ void Terminal_fence_statu::MergeFrom(const Terminal_fence_statu& from) {
   (void) cached_has_bits;
 
   cached_has_bits = from._has_bits_[0];
-  if (cached_has_bits & 7u) {
+  if (cached_has_bits & 15u) {
     if (cached_has_bits & 0x00000001u) {
-      terminal_id_ = from.terminal_id_;
+      set_has_error_info();
+      error_info_.AssignWithDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), from.error_info_);
     }
     if (cached_has_bits & 0x00000002u) {
-      cloud_statu_ = from.cloud_statu_;
+      terminal_id_ = from.terminal_id_;
     }
     if (cached_has_bits & 0x00000004u) {
+      cloud_statu_ = from.cloud_statu_;
+    }
+    if (cached_has_bits & 0x00000008u) {
       position_statu_ = from.position_statu_;
     }
     _has_bits_[0] |= cached_has_bits;
@@ -485,7 +546,7 @@ void Terminal_fence_statu::CopyFrom(const Terminal_fence_statu& from) {
 }
 
 bool Terminal_fence_statu::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000007) != 0x00000007) return false;
+  if ((_has_bits_[0] & 0x0000000e) != 0x0000000e) return false;
   return true;
 }
 
@@ -495,6 +556,7 @@ void Terminal_fence_statu::Swap(Terminal_fence_statu* other) {
 }
 void Terminal_fence_statu::InternalSwap(Terminal_fence_statu* other) {
   using std::swap;
+  error_info_.Swap(&other->error_info_);
   swap(terminal_id_, other->terminal_id_);
   swap(cloud_statu_, other->cloud_statu_);
   swap(position_statu_, other->position_statu_);

wj_lidar/wj_lidar_msg.pb.h

@@ -152,6 +152,21 @@ class Terminal_fence_statu : public ::google::protobuf::Message /* @@protoc_inse
 
   // accessors -------------------------------------------------------
 
+  // optional string error_info = 4;
+  bool has_error_info() const;
+  void clear_error_info();
+  static const int kErrorInfoFieldNumber = 4;
+  const ::std::string& error_info() const;
+  void set_error_info(const ::std::string& value);
+  #if LANG_CXX11
+  void set_error_info(::std::string&& value);
+  #endif
+  void set_error_info(const char* value);
+  void set_error_info(const char* value, size_t size);
+  ::std::string* mutable_error_info();
+  ::std::string* release_error_info();
+  void set_allocated_error_info(::std::string* error_info);
+
   // required int64 terminal_id = 1;
   bool has_terminal_id() const;
   void clear_terminal_id();
@@ -181,6 +196,8 @@ class Terminal_fence_statu : public ::google::protobuf::Message /* @@protoc_inse
   void clear_has_cloud_statu();
   void set_has_position_statu();
   void clear_has_position_statu();
+  void set_has_error_info();
+  void clear_has_error_info();
 
   // helper for ByteSizeLong()
   size_t RequiredFieldsByteSizeFallback() const;
@@ -188,6 +205,7 @@ class Terminal_fence_statu : public ::google::protobuf::Message /* @@protoc_inse
   ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
   ::google::protobuf::internal::HasBits<1> _has_bits_;
   mutable int _cached_size_;
+  ::google::protobuf::internal::ArenaStringPtr error_info_;
   ::google::protobuf::int64 terminal_id_;
   ::google::protobuf::int64 cloud_statu_;
   ::google::protobuf::int64 position_statu_;
@@ -320,13 +338,13 @@ class Fence_statu_message : public ::google::protobuf::Message /* @@protoc_inser
 
 // required int64 terminal_id = 1;
 inline bool Terminal_fence_statu::has_terminal_id() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
+  return (_has_bits_[0] & 0x00000002u) != 0;
 }
 inline void Terminal_fence_statu::set_has_terminal_id() {
-  _has_bits_[0] |= 0x00000001u;
+  _has_bits_[0] |= 0x00000002u;
 }
 inline void Terminal_fence_statu::clear_has_terminal_id() {
-  _has_bits_[0] &= ~0x00000001u;
+  _has_bits_[0] &= ~0x00000002u;
 }
 inline void Terminal_fence_statu::clear_terminal_id() {
   terminal_id_ = GOOGLE_LONGLONG(0);
@@ -344,13 +362,13 @@ inline void Terminal_fence_statu::set_terminal_id(::google::protobuf::int64 valu
 
 // required int64 cloud_statu = 2;
 inline bool Terminal_fence_statu::has_cloud_statu() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
+  return (_has_bits_[0] & 0x00000004u) != 0;
 }
 inline void Terminal_fence_statu::set_has_cloud_statu() {
-  _has_bits_[0] |= 0x00000002u;
+  _has_bits_[0] |= 0x00000004u;
 }
 inline void Terminal_fence_statu::clear_has_cloud_statu() {
-  _has_bits_[0] &= ~0x00000002u;
+  _has_bits_[0] &= ~0x00000004u;
 }
 inline void Terminal_fence_statu::clear_cloud_statu() {
   cloud_statu_ = GOOGLE_LONGLONG(0);
@@ -368,13 +386,13 @@ inline void Terminal_fence_statu::set_cloud_statu(::google::protobuf::int64 valu
 
 // required int64 position_statu = 3;
 inline bool Terminal_fence_statu::has_position_statu() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
+  return (_has_bits_[0] & 0x00000008u) != 0;
 }
 inline void Terminal_fence_statu::set_has_position_statu() {
-  _has_bits_[0] |= 0x00000004u;
+  _has_bits_[0] |= 0x00000008u;
 }
 inline void Terminal_fence_statu::clear_has_position_statu() {
-  _has_bits_[0] &= ~0x00000004u;
+  _has_bits_[0] &= ~0x00000008u;
 }
 inline void Terminal_fence_statu::clear_position_statu() {
   position_statu_ = GOOGLE_LONGLONG(0);
@@ -390,6 +408,69 @@ inline void Terminal_fence_statu::set_position_statu(::google::protobuf::int64 v
   // @@protoc_insertion_point(field_set:wj_lidar_message.Terminal_fence_statu.position_statu)
 }
 
+// optional string error_info = 4;
+inline bool Terminal_fence_statu::has_error_info() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void Terminal_fence_statu::set_has_error_info() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void Terminal_fence_statu::clear_has_error_info() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void Terminal_fence_statu::clear_error_info() {
+  error_info_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  clear_has_error_info();
+}
+inline const ::std::string& Terminal_fence_statu::error_info() const {
+  // @@protoc_insertion_point(field_get:wj_lidar_message.Terminal_fence_statu.error_info)
+  return error_info_.GetNoArena();
+}
+inline void Terminal_fence_statu::set_error_info(const ::std::string& value) {
+  set_has_error_info();
+  error_info_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
+  // @@protoc_insertion_point(field_set:wj_lidar_message.Terminal_fence_statu.error_info)
+}
+#if LANG_CXX11
+inline void Terminal_fence_statu::set_error_info(::std::string&& value) {
+  set_has_error_info();
+  error_info_.SetNoArena(
+    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
+  // @@protoc_insertion_point(field_set_rvalue:wj_lidar_message.Terminal_fence_statu.error_info)
+}
+#endif
+inline void Terminal_fence_statu::set_error_info(const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  set_has_error_info();
+  error_info_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
+  // @@protoc_insertion_point(field_set_char:wj_lidar_message.Terminal_fence_statu.error_info)
+}
+inline void Terminal_fence_statu::set_error_info(const char* value, size_t size) {
+  set_has_error_info();
+  error_info_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
+      ::std::string(reinterpret_cast<const char*>(value), size));
+  // @@protoc_insertion_point(field_set_pointer:wj_lidar_message.Terminal_fence_statu.error_info)
+}
+inline ::std::string* Terminal_fence_statu::mutable_error_info() {
+  set_has_error_info();
+  // @@protoc_insertion_point(field_mutable:wj_lidar_message.Terminal_fence_statu.error_info)
+  return error_info_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline ::std::string* Terminal_fence_statu::release_error_info() {
+  // @@protoc_insertion_point(field_release:wj_lidar_message.Terminal_fence_statu.error_info)
+  clear_has_error_info();
+  return error_info_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline void Terminal_fence_statu::set_allocated_error_info(::std::string* error_info) {
+  if (error_info != NULL) {
+    set_has_error_info();
+  } else {
+    clear_has_error_info();
+  }
+  error_info_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), error_info);
+  // @@protoc_insertion_point(field_set_allocated:wj_lidar_message.Terminal_fence_statu.error_info)
+}
+
 // -------------------------------------------------------------------
 
 // Fence_statu_message

wj_lidar/wj_lidar_msg.proto

@@ -6,6 +6,7 @@ message Terminal_fence_statu
     required int64 terminal_id=1;
     required int64 cloud_statu=2;
     required int64 position_statu=3;
+    optional string error_info=4;
 }
 
 message Fence_statu_message