5-27 现场 增加区域四角万集点云有无判断，筛出雷达的异常状态。增加大疆车轮点云与万集点云传出，病根据结果校验点云是否处于机械手内空，根据外点个数决定是否判断测量失败。修改角度权重为以万集为主，偏差越大，越倾向于万集角度。

locate/locater.cpp

@@ -104,6 +104,7 @@ Error_manager Locater::execute_task(Task_Base* task,double time_out)
     std::lock_guard<std::mutex> t_lock(m_mutex_lock);
     locate_task->update_statu(TASK_WORKING);
     code=locate(t_cloud_input,locate_information,save_path);
+    locate_task->set_locate_cloud(t_cloud_input);
     locate_task->set_locate_information(locate_information);
     if(code!=SUCCESS)
     {
@@ -301,7 +302,7 @@ void Locater::save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::stri
     fclose(pfile);
 }
 
-Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src,
+Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src,
                   Locate_information& locate_information, std::string work_dir)
 {
     if(cloud_src.get()==0)
@@ -336,6 +337,8 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src,
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
     code=locate_sift(cloud_in,t_yolo_boxes,cloud_wheel,cloud_car,work_dir);
+    // added by yct, 传出车轮点云，放入任务中替换完整点云
+    cloud_src = cloud_wheel;
     if(code!=SUCCESS)
     {
         return code;

locate/locater.h

@@ -31,7 +31,7 @@ protected:
     //cloud_in:输入点云
     //locate_information:测量结果
     //work_dir:中间文件保存路径
-    Error_manager locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+    Error_manager locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in,
         Locate_information& locate_information, std::string work_dir);
     //yolo定位车辆外接矩形
     //cloud_in:输入点云,定位算法内部不会修改点云内容

plc/plc_communicator.cpp

@@ -136,6 +136,11 @@ Error_manager Plc_Communicator::execute_task(Task_Base *task)
     if (err.is_equal_error_manager(Error_manager(Error_code::SUCCESS)))
     {
         Error_manager write_err = write_result_to_plc(measure_result_temp);
+//        // added by yct, 检查为何2号位测量88度，发送86.9到plc
+//        char description[480] = {0};
+//        sprintf(description, "to plc, id %d center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f",
+//                measure_result_temp.terminal_id, measure_result_temp.x, measure_result_temp.y, measure_result_temp.wheel_base, measure_result_temp.width, measure_result_temp.angle);
+//        LOG(INFO) << description;
         plc_task_temp->update_statu(Task_statu::TASK_OVER, "executed by plc_communicator.");
         return write_err;
     }

terminor/terminal_command_executor.cpp

@@ -295,6 +295,22 @@ void Terminor_Command_Executor::set_save_root_path(std::string root)
     m_save_root_path=root;
 }
 
+// !!!!!!!!!!!! 临时使用，保存点云debug
+void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
+{
+    std::ofstream os;
+    os.open(txt, std::ios::out);
+    for (int i = 0; i < pCloud->points.size(); i++)
+    {
+        pcl::PointXYZ point = pCloud->points[i];
+        char buf[255];
+        memset(buf, 0, 255);
+        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
+        os.write(buf, strlen(buf));
+    }
+    os.close();
+}
+
 Error_manager Terminor_Command_Executor::scanning_measuring() {
     std::lock_guard<std::mutex> lock(ms_mutex_scan_measure);
     Error_manager code;
@@ -435,6 +451,9 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     m_measure_information.locate_correct = false;
     //获取测量结果
     Locate_information dj_locate_information = locate_task->get_locate_information();
+    //added by yct, 获取车轮点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_combined_wheel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    t_combined_wheel_cloud = locate_task->get_locate_cloud();
     //释放locate task
     if (locate_task != NULL) {
         delete locate_task;
@@ -465,6 +484,14 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     ///万集测量正确,对比两数据
     wj_measure_result wj_result;
     code = wj_task->get_result(wj_result);
+    //added by yct, wj wheel cloud
+    pcl::PointCloud<pcl::PointXYZ> t_wj_wheel = wj_task->get_cloud();
+    for (int m = 0; m < t_wj_wheel.size(); ++m) {
+        t_wj_wheel[m].x *= 1000.0;
+        t_wj_wheel[m].y *= 1000.0;
+        t_wj_wheel[m].z *= 1000.0;
+    }
+    t_combined_wheel_cloud->operator+=(t_wj_wheel);
     if (code != SUCCESS) {
         delete wj_task;
         return code;
@@ -488,7 +515,9 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
         return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
     }
     ///根据sigmod函数计算角度权重
-    double angle_weight=1.0-1.0/(1.0+exp(-offset_angle/1.8));
+//    double angle_weight=1.0-1.0/(1.0+exp(-offset_angle/1.8));
+    // added by yct, 角度偏差越大，越倾向于万集，之后校验结果
+    double angle_weight=1.0/(1.0+exp(-offset_angle/0.5));
     ///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以2750为基准作卡尔曼滤波
     ///车长以大疆雷达为准,车宽以万集雷达为准,高以大疆雷达为准
     m_measure_information.locate_x = wj_locate_information.locate_x;
@@ -508,7 +537,54 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     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;
+    // 20210527 added by yct
+    // 6号位标定地面偏低，高度增加21mm
+    if(m_terminor_parameter.id()==5)
+    {
+        m_measure_information.locate_hight += 21;
+    }
     m_measure_information.locate_correct = true;
+
+    // 20210527 added by yct, 融合大疆轮子与万集点云进行结果校验,机械手内空2150，取2000计算
+    save_cloud_txt(project_path+"/comb.txt", t_combined_wheel_cloud);
+    Eigen::Vector3d line_param_left, line_param_right;
+    double sign = 1.0;
+    if(m_measure_information.locate_theta==90.0)
+    {
+        line_param_left << 1.0, 0, -m_measure_information.locate_x + 1000.0;
+        line_param_right << 1.0, 0, -m_measure_information.locate_x - 1000.0;
+    }else{
+        // 减去2度测试外点个数
+        double k = (m_measure_information.locate_theta) * M_PI / 180.0;
+        line_param_left << tan(k), -1, m_measure_information.locate_y - (m_measure_information.locate_x - 1000.0/sin(k))*tan(k);
+        line_param_right << tan(k), -1, m_measure_information.locate_y - (m_measure_information.locate_x + 1000.0/sin(k))*tan(k);
+        if(tan(k)<0)
+        {
+            sign = -sign;
+        }
+    }
+    int outside_point_count = 0;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_outside_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    for (int l = 0; l < t_combined_wheel_cloud->size(); ++l) {
+        Eigen::Vector3d t_point(t_combined_wheel_cloud->points[l].x,t_combined_wheel_cloud->points[l].y,1.0);
+        double t_left_value = sign * (line_param_left.transpose() * t_point)[0];
+        double t_right_value = sign * (line_param_right.transpose() * t_point)[0];
+        // 点在线右侧为正，左侧为负
+        if(t_left_value <= 0 || t_right_value >= 0)
+        {
+            outside_point_count++;
+            t_outside_cloud->push_back(pcl::PointXYZ(t_combined_wheel_cloud->points[l].x, t_combined_wheel_cloud->points[l].y, t_combined_wheel_cloud->points[l].z));
+        }
+    }
+    save_cloud_txt(project_path+"/outside.txt", t_outside_cloud);
+    LOG(INFO) << "outside point count: "<<outside_point_count;
+    // 安全判断，外点超过15个判断本次测量失败
+    if(outside_point_count > 15)
+    {
+        m_measure_information.locate_correct = false;
+        code = Error_code::ERROR;
+    }
+
     ////如果测量正确,检验结果
     if (mp_verify_tool != NULL) {
 

wj_lidar/fence_controller.cpp

@@ -318,12 +318,21 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                 // 获取最新点云并保存到total_cloud中
 
                 pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+                bool has_few_cloud = false;
                 for (int i = 0; i < m_lidars_vector.size(); ++i)
                 {
                     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
                     if (m_lidars_vector[i]->get_cloud(cloud, command_temp.command_time, command_temp.timeout_in_milliseconds) == SUCCESS)
                     {
                         total_cloud->operator+=(*cloud);
+
+                        // added yc yct, 区域四角雷达是否有点融合判断
+                        std::string t_laser_ip = m_lidars_vector[i]->get_ip();
+                        int t_laser_id = std::stoi(t_laser_ip.substr(t_laser_ip.size()-2));
+                        if(t_laser_id == terminal_id+1 || t_laser_id == terminal_id+2||t_laser_id == terminal_id+8||t_laser_id == terminal_id+9)
+                        {
+                            has_few_cloud |= cloud->size() <= 5;
+                        }
                     }
                 }
 
@@ -372,12 +381,20 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                     wj_measure_temp.x -= 15;
                 }
                 if (wj_measure_temp.correctness){
+                    // 20210527 added by yct. 一旦存在雷达没有点而结果正确，此时将结果改为错误
+                    Error_code ec=Error_code::SUCCESS;
+                    if(has_few_cloud)
+                    {
+                        wj_measure_temp.correctness = false;
+                        ec = Error_code::WJ_MANAGER_EMPTY_CLOUD;
+                    }
                     task_temp->set_result(wj_measure_temp);
-                    char description[255]={0};
-                    sprintf(description,"WJ center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f",
-                        x*1000.0,y*1000.0,wheelbase*1000.0,width*1000.0,c);
-                    LOG(INFO)<<description;
-                    return Error_manager(Error_code::SUCCESS);
+                    task_temp->set_cloud(*total_cloud);
+                    char description[480] = {0};
+                    sprintf(description, "WJ center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f %s",
+                            x * 1000.0, y * 1000.0, wheelbase * 1000.0, width * 1000.0, c, has_few_cloud?"存在空点云":"雷达正常");
+                    LOG(INFO) << description;
+                    return Error_manager(ec);
                 }
             }
         }

wj_lidar/wj_lidar_encapsulation.h

@@ -23,6 +23,11 @@ public:
     bool get_connection_status();
     // 获取初始化情况
     bool get_initialize_status();
+    // 获取ip
+    std::string get_ip()
+    {
+        return m_ip;
+    }
 
 private:
     // 初始化雷达连接线程函数

wj_lidar/wj_lidar_task.h

@@ -5,6 +5,9 @@
 #include <atomic>
 #include <chrono>
 
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+
 #include "../task/task_command_manager.h"
 #include "../error_code/error_code.h"
 
@@ -52,6 +55,17 @@ public:
     // 获取测量结果是否已存入该任务单的指标
     bool get_result_flag();
 
+    // 填入点云
+    void set_cloud(pcl::PointCloud<pcl::PointXYZ> cloud)
+    {
+        m_cloud = cloud;
+    }
+    // 获取万集点云
+    pcl::PointCloud<pcl::PointXYZ> get_cloud()
+    {
+        return m_cloud;
+    }
+
 private:
     // 测量指令信息
     wj_command m_command;
@@ -61,6 +75,8 @@ private:
     std::mutex m_mutex;
     // 标记结果是否已被设置
     std::atomic<bool> mb_result_flag;
+    // 任务中保存点云
+    pcl::PointCloud<pcl::PointXYZ> m_cloud;
 };
 
 #endif // !WJ_LIDAR_TASK_HH