修复雷达开始后没有停止的bug,修改流程,扫描测量与plc上传分开

CMakeLists.txt

@@ -11,7 +11,7 @@ FIND_PACKAGE(PCL REQUIRED)
 FIND_PACKAGE(Protobuf REQUIRED)
 FIND_PACKAGE(Glog REQUIRED)
 set(CMAKE_MODULE_PATH "/usr/local/share/")
-
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wl,--no-as-needed")
 set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
 set(CMAKE_BUILD_TYPE "RELEASE")
 
@@ -30,13 +30,13 @@ include_directories(
 )
 link_directories("/usr/local/lib")
 
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/error_code ERROR_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/laser LASER_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc PLC_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/locate LOCATE_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/terminor TERMINOR_SRC )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/error_code ERROR_SRC )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/system_manager SYS_SRC )
 
 #add_executable(LidarMeasure ./main.cpp ${LASER_SRC} ${PLC_SRC} ${TERMINOR_SRC} ${LOCATE_SRC} ${TASK_MANAGER_SRC})

error_code/error_code.h

@@ -151,6 +151,7 @@ enum Error_code
     TERMINOR_CREATE_WORKING_THREAD_FAILED,
     TERMINOR_FORCE_QUIT,
     TERMINOR_LASER_TIMEOUT,
+    TERMINOR_POST_PLC_TIMEOUT,
 
 
 

laser/Laser.cpp

@@ -128,6 +128,7 @@ Error_manager Laser_base::check_error()
 
 bool Laser_base::Start()
 {
+
     m_bscan_start=true;
 	m_statu = eLaser_busy;
 	m_bStart_capture.Notify(true);
@@ -146,10 +147,11 @@ bool Laser_base::Stop()
 	//在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
 	if(mp_laser_task !=NULL)
     {
-	    if(mp_laser_task->get_statu() == TASK_WORKING)
+        mp_laser_task->set_task_statu(TASK_OVER);
+	    /*if(mp_laser_task->get_statu() == TASK_WORKING)
         {
-            mp_laser_task->set_task_statu(TASK_OVER);
-        }
+
+        }*/
     }
 
 	return true;
@@ -214,48 +216,7 @@ void Laser_base::Disconnect()
 //input：laser_task 雷达任务单，必须是子类，并且任务正确。（传引用）
 Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
 {
-    Error_manager t_error_manager;
-    //检查指针
-    if(p_laser_task == NULL)
-    {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "Laser_base::porform_task failed, POINTER_IS_NULL");
-    }
-    if(p_laser_task->get_task_type()!=LASER_TASK)
-    {
-        return Error_manager(Error_code::LASER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
-                             "laser task type error");
-    }
-
-
-    //接受任务，并将任务的状态改为TASK_SIGNED已签收
-    mp_laser_task = (Laser_task*)p_laser_task;
-    mp_laser_task->set_task_statu(TASK_SIGNED);
-
-
-    //检查消息内容是否正确，
-    //检查点云指针
-    if(mp_laser_task->get_task_point3d_cloud_vector().get() == NULL)
-    {
-        t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
-                                            Error_level::MINOR_ERROR,
-                                            "Laser_base::porform_task failed, POINTER_IS_NULL");
-    }
-    else
-    {
-        //将任务的状态改为 TASK_WORKING 处理中
-        mp_laser_task->set_task_statu(TASK_WORKING);
-        std::string save_path=mp_laser_task->get_save_path();
-        SetSaveDir(save_path);
-        //启动雷达扫描
-        Start();
-    }
-    //返回错误码
-    if(t_error_manager != Error_code::SUCCESS)
-    {
-        mp_laser_task->set_task_error_manager(t_error_manager);
-    }
-    return t_error_manager;
+    return SUCCESS;
 }
 
 
@@ -362,9 +323,11 @@ void Laser_base::thread_toXYZ()
 
                     //此时cloud为雷达采集数据的结果，转换后的三维点云。
                     //将每个点存入任务单里面的点云容器。
+
                     if(mp_laser_task!=NULL)
                     {
-                        mp_laser_task->get_task_point3d_cloud_vector()->push_back(pcl::PointXYZ(point.x,point.y,point.z));
+                        Error_manager code=mp_laser_task->push_point(pcl::PointXYZ(point.x,point.y,point.z));
+
                     }
                 }
                 m_points_count+=cloud.size();

laser/LivoxLaser.cpp

@@ -233,8 +233,7 @@ void CLivoxLaser::Disconnect()
 }
 bool CLivoxLaser::Start()
 {
-	LOG(INFO) << " livox start :"<<this; 
-	//???????????
+	LOG(INFO) << " livox start :"<<this;
 	m_queue_livox_data.clear();
 	g_count[m_handle] = 0;
 	return Laser_base::Start();

laser/LivoxMid100Laser.cpp

@@ -79,6 +79,23 @@ eLaserStatu CLivoxMid100Laser::GetStatu()
 
 bool CLivoxMid100Laser::Connect()
 {
+    //设置点云变换矩阵
+    double matrix[12]={0};
+    matrix[0]=m_laser_param.mat_r00();
+    matrix[1]=m_laser_param.mat_r01();
+    matrix[2]=m_laser_param.mat_r02();
+    matrix[3]=m_laser_param.mat_r03();
+
+    matrix[4]=m_laser_param.mat_r10();
+    matrix[5]=m_laser_param.mat_r11();
+    matrix[6]=m_laser_param.mat_r12();
+    matrix[7]=m_laser_param.mat_r13();
+
+    matrix[8]=m_laser_param.mat_r20();
+    matrix[9]=m_laser_param.mat_r21();
+    matrix[10]=m_laser_param.mat_r22();
+    matrix[11]=m_laser_param.mat_r23();
+    SetMetrix(matrix);
     return CLivoxLaser::Connect();
 }
 void CLivoxMid100Laser::Disconnect()
@@ -88,12 +105,12 @@ void CLivoxMid100Laser::Disconnect()
 
 bool CLivoxMid100Laser::Stop()
 {
+    LOG(INFO)<<" livoxmid100 stoped !!!"<<m_laser_param.sn();
     return CLivoxLaser::Stop();
 }
 bool CLivoxMid100Laser::Start()
 {
-		LOG(INFO) << " livoxMid100 start :" << this;
-		//������ݣ���ʼ
+		LOG(INFO) << " livoxMid100 start :" << m_laser_param.sn();
 		m_queue_livox_data.clear();
 		g_count[m_handle1] = 0;
 		g_count[m_handle2] = 0;
@@ -101,6 +118,47 @@ bool CLivoxMid100Laser::Start()
 		return Laser_base::Start();
 }
 
+Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
+{
+    Error_manager t_error_manager;
+//检查指针
+    if (p_laser_task == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "Laser_base::porform_task failed, POINTER_IS_NULL");
+    }
+    if (p_laser_task->get_task_type() != LASER_TASK) {
+        return Error_manager(Error_code::LASER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+                             "laser task type error");
+    }
+
+
+//接受任务，并将任务的状态改为TASK_SIGNED已签收
+    mp_laser_task = (Laser_task *) p_laser_task;
+    mp_laser_task->set_task_statu(TASK_SIGNED);
+
+
+//检查消息内容是否正确，
+//检查点云指针
+    if (mp_laser_task->get_task_point3d_cloud_vector().get() == NULL) {
+        t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
+                                            Error_level::MINOR_ERROR,
+                                            "Laser_base::porform_task failed, POINTER_IS_NULL");
+    }
+    else{
+        m_frame_maxnum=mp_laser_task->get_task_points_number();
+//将任务的状态改为 TASK_WORKING 处理中
+        mp_laser_task->set_task_statu(TASK_WORKING);
+        std::string save_path = mp_laser_task->get_save_path();
+        SetSaveDir(save_path);
+//启动雷达扫描
+        Start();
+    }
+//返回错误码
+    if (t_error_manager != Error_code::SUCCESS) {
+        mp_laser_task->set_task_error_manager(t_error_manager);
+    }
+    return t_error_manager;
+}
 CLivoxMid100Laser::~CLivoxMid100Laser()
 {
 }

laser/LivoxMid100Laser.h

@@ -15,6 +15,7 @@ public:
     virtual bool Start();
     virtual bool Stop();
     virtual eLaserStatu GetStatu();
+    Error_manager execute_task(Task_Base* p_laser_task);
 
 protected:
 	virtual bool IsScanComplete();

laser/laser_parameter.proto

@@ -0,0 +1,38 @@
+syntax = "proto2";
+package Laser_proto;
+
+message laser_parameter {
+    optional string laser_ip = 1;
+    optional int64 laser_port = 2;
+    optional int64 laser_port_remote = 3;
+    optional double mat_r00 = 4 [default = 1.0];
+    optional double mat_r01 = 5 [default = 1.0];
+    optional double mat_r02 = 6 [default = 1.0];
+    optional double mat_r03 = 7 [default = 1.0];
+    optional double mat_r10 = 8 [default = 1.0];
+    optional double mat_r11 = 9 [default = 1.0];
+    optional double mat_r12 = 10 [default = 1.0];
+    optional double mat_r13 = 11 [default = 1.0];
+    optional double mat_r20 = 12 [default = 1.0];
+    optional double mat_r21 = 13 [default = 1.0];
+    optional double mat_r22 = 14 [default = 1.0];
+    optional double mat_r23 = 15 [default = 1.0];
+    optional double axis_x_theta = 16;
+    optional double axis_y_theta = 17;
+    optional double axis_z_theta = 18;
+    optional double translation_x = 19;
+    optional double translation_y = 20;
+    optional double translation_z = 21;
+
+    optional double install_angle = 22 [default = 0.0];
+    optional bool scan_direction = 23 [default = true];
+    optional string sn = 24;
+    optional int64 frame_num = 25 [default = 3000];
+    optional string type = 26 [default = ""];
+
+}
+
+message Laser_parameter_all
+{
+    repeated laser_parameter        laser_parameters=1;
+}

laser/laser_task_command.cpp

@@ -30,13 +30,19 @@ Laser_task::~Laser_task()
 // output:p_task_point3d_cloud_vector三维点云容器指针
 //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
 Error_manager Laser_task::task_init( Task_statu task_statu,
-                                     pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector)
+                                     pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector,
+                                     std::mutex* lock)
 {
     if(p_task_point3d_cloud_vector.get() == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init failed");
     }
+    if(lock==NULL)
+    {
+        return Error_manager(PARAMETER_ERROR,NORMAL,"laser task input lock is null");
+    }
+    mp_cloud_lock=lock;
     m_task_statu = task_statu;
     m_task_statu_information.clear();
 
@@ -57,13 +63,19 @@ Error_manager Laser_task::task_init( Task_statu task_statu,
 Error_manager Laser_task::task_init(Task_statu task_statu,
                                     std::string & task_statu_information,
                                     int task_points_number,
-                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector)
+                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector,
+                                    std::mutex* lock)
 {
     if(p_task_point3d_cloud_vector.get() == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init failed");
     }
+    if(lock==NULL)
+    {
+        return Error_manager(PARAMETER_ERROR,NORMAL,"laser task input lock is null");
+    }
+    mp_cloud_lock=lock;
     m_task_statu = task_statu;
     m_task_statu_information = task_statu_information;
 
@@ -74,6 +86,24 @@ Error_manager Laser_task::task_init(Task_statu task_statu,
     return Error_code::SUCCESS;
 }
 
+//push点云
+Error_manager Laser_task::push_point(pcl::PointXYZ point)
+{
+    if(mp_cloud_lock==NULL)
+    {
+        return Error_manager(PARAMETER_ERROR,NORMAL,"push_point laser task input lock is null");
+    }
+    if(pm_task_point3d_cloud_vector.get()==NULL)
+    {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                            "Laser_task::push_point failed");
+    }
+    mp_cloud_lock->lock();
+    pm_task_point3d_cloud_vector->push_back(point);
+    mp_cloud_lock->unlock();
+    return SUCCESS;
+}
+
 //获取 点云的采集数量
 int Laser_task::get_task_points_number()
 {

laser/laser_task_command.h

@@ -16,7 +16,7 @@
 #include "../error_code/error_code.h"
 
 #include <vector>
-
+#include <mutex>
 
 #include "../task/task_command_manager.h"
 
@@ -36,7 +36,8 @@ public:
     // output:p_task_point3d_cloud_vector三维点云容器指针
     //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
     Error_manager task_init(Task_statu task_statu,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector);
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector,
+                            std::mutex* lock);
 
     //初始化任务单，必须初始化之后才可以使用，（可选参数）
     //    input:task_type 任务类型
@@ -44,14 +45,18 @@ public:
     //    input:task_statu_information状态说明
     //    input:task_points_number点云的采集数量
     //    output:p_task_point3d_cloud_vector三维点云容器指针
+    //    lock:
     //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
     Error_manager task_init(Task_statu task_statu,
                             std::string & task_statu_information,
                             int task_points_number,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector);
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector,
+                            std::mutex* lock);
 
     //获取 点云的采集数量
     int get_task_points_number();
+    //push点云
+    Error_manager push_point(pcl::PointXYZ point);
     //获取 三维点云容器
     pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point3d_cloud_vector();
     //获取 错误码
@@ -82,6 +87,8 @@ protected:
 
     //错误码，任务故障信息，任务输出
     Error_manager                   m_task_error_manager;
+    //
+    std::mutex*                     mp_cloud_lock;
 };
 
 

locate/locater.cpp

@@ -97,9 +97,8 @@ Error_manager Locater::execute_task(Task_Base* task,double time_out)
     std::string save_path=locate_task->get_save_path();
     if(t_cloud_input.get()==0)
     {
-        LOG(ERROR)<<"Task set cloud is uninit";
         locate_task->update_statu(TASK_OVER,"Task set cloud is uninit");
-        return LOCATER_INPUT_CLOUD_UNINIT;
+        return Error_manager(LOCATER_INPUT_CLOUD_UNINIT,NORMAL,"Task set cloud is uninit");
     }
     //第二步,加锁,更新task状态,调用locate,进入测量中
     std::lock_guard<std::mutex> t_lock(m_mutex_lock);

main.cpp

@@ -35,7 +35,7 @@ GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
             t->tm_sec);
 
     FILE* tp_file=fopen(buf,"w");
-    fprintf(tp_file,data,size);
+    fprintf(tp_file,data,strlen(data));
     fclose(tp_file);
 
 }

plc/plc_communicator.cpp

@@ -1,4 +1,5 @@
 #include "plc_communicator.h"
+#include <glog/logging.h>
 
 // ××××××××××× 构造与析构 ×××××××××××
 Plc_Communicator::Plc_Communicator(plc_module::plc_connection_params connection_params) : mb_plc_initialized(false),
@@ -274,6 +275,9 @@ void Plc_Communicator::plc_update_thread(Plc_Communicator *plc_communicator)
                                     plc_communicator->m_plc_region_status[terminal_id_temp].last_time_point = std::chrono::steady_clock::now();
                                     modified = true;
                                 }
+                                else{
+                                    LOG(ERROR)<<ec.to_string();
+                                }
                             }
                         }
                         // 指令清空，暂不恢复心跳

terminor/terminal_command_executor.cpp

@@ -2,6 +2,7 @@
 #include <glog/logging.h>
 #include <chrono>
 #include <pcl/filters/passthrough.h>
+#include "../tool/pathcreator.h"
 
 //////以下两个函数用于测试,读取指定点云
 bool string2point(std::string str,pcl::PointXYZ& point)
@@ -176,26 +177,107 @@ void Terminor_Command_Executor::thread_command_working(Terminor_Command_Executor
         return ;
     }
     //执行指令,阻塞直到指令执行完成或者异常结束
-    Error_manager code=terminor->working();
+    Error_manager code=terminor->scanning_measuring();
+    if(code!=SUCCESS)
+    {
+        LOG(ERROR)<<"Command execute failed : "<<code.to_string();
+    }
+    //根据结果执行上传任务
+    code=terminor->post_measure_information();
     //更新终端状态为Ready状态
     terminor->m_terminor_statu=TERMINOR_READY;
+    if(code!=SUCCESS)
+    {
+        LOG(ERROR)<<"Command execute failed : "<<code.to_string();
+    }
+    else
+    {
+        LOG(INFO)<<"Command execute complete!!!";
+    }
+}
+
+/*
+     * 执行上传plc任务
+     */
+Error_manager Terminor_Command_Executor::post_measure_information()
+{
+    //计时起点
+    auto t_start_point=std::chrono::system_clock::now();
+    Error_manager code;
+    //执行plc上传任务
+    m_terminor_statu=TERMINOR_POSTING;
+    Plc_Task* plc_task=new Plc_Task();
+
+    measure_result measure_information;
+    measure_information.x=m_measure_information.locate_x;
+    measure_information.y=m_measure_information.locate_y;
+    measure_information.angle=m_measure_information.locate_theta;
+    measure_information.length=m_measure_information.locate_length;
+    measure_information.width=m_measure_information.locate_width;
+    measure_information.height=m_measure_information.locate_hight;
+    measure_information.wheel_base=m_measure_information.locate_wheel_base;
+    measure_information.terminal_id=m_terminor_parameter.id();
+    measure_information.correctness=m_measure_information.locate_correct;
+
+    plc_task->set_result(measure_information);
+    code=mp_plc->execute_task(plc_task);
+    while(code!=SUCCESS)
+    {
+        //判断是否强制退出
+        if(mb_force_quit==true)
+        {
+            char description[255]={0};
+            sprintf(description,"ternimal is force quit terminal id : %d",m_terminor_parameter.id());
+            return Error_manager(TERMINOR_FORCE_QUIT,NORMAL,description);
+        }
+        //对于上传不成功,作失败处理,重新传输
+        code=mp_plc->execute_task(plc_task);
+        ///不成功,检测本次流程是否超时
+        if(code!=SUCCESS)
+        {
+            auto t_end_point=std::chrono::system_clock::now();
+            auto duration=std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
+            double second=double(duration.count()) *
+                std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
+            if(second>m_timeout_second)
+            {
+                //扫描超时处理
+                return Error_manager(TERMINOR_POST_PLC_TIMEOUT,NORMAL,"post plc timeout");
+            }
+            usleep(1000*100);
+        }
+
+    }
+    return code;
 }
-Error_manager Terminor_Command_Executor::working()
+
+Error_manager Terminor_Command_Executor::scanning_measuring()
 {
     //计时起点
     auto t_start_point=std::chrono::system_clock::now();
     Error_manager code;
     //第一步,启动雷达扫描点云,切换当前状态为扫描中
     pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    std::mutex cloud_lock;
     std::vector<Task_Base*> laser_task_vector;
+    PathCreator path_creator;
+    path_creator.CreateDatePath("/home/zx/data/scans");
+    std::string project_path;
+    path_creator.GetCurPath(project_path);
     for(int i=0;i<mp_laser_vector.size();++i)
     {
         Laser_task* laser_task=new Laser_task();
         //
-        laser_task->task_init(TASK_CREATED,scan_cloud);
+        laser_task->task_init(TASK_CREATED,scan_cloud,&cloud_lock);
         laser_task->set_task_points_number(1000);
+        laser_task->set_save_path(project_path);
         laser_task_vector.push_back(laser_task);
         //发送任务单给雷达
+        code=mp_laser_vector[i]->execute_task(laser_task);
+        if(code!=SUCCESS)
+        {
+            return code;
+        }
     }
     m_terminor_statu=TERMINOR_SCANNING;
     //等待雷达完成任务单
@@ -231,11 +313,12 @@ Error_manager Terminor_Command_Executor::working()
                 return Error_manager(TERMINOR_LASER_TIMEOUT,NORMAL,"laser scanning timeout");
             }
 
+
         }
         usleep(1000*100);
     }
     //检查雷达任务完成情况,是否是正常完成
-
+    LOG(INFO)<<" laser scanning over cloud size:"<<scan_cloud->size();
     //释放扫描任务单
     for(int i=0;i<laser_task_vector.size();++i)
     {
@@ -264,11 +347,12 @@ Error_manager Terminor_Command_Executor::working()
     passY.setFilterFieldName("z");
     passY.setFilterLimits(m_terminor_parameter.area_3d().min_z(),m_terminor_parameter.area_3d().max_z());
     passY.filter(*locate_cloud);
-
+    LOG(INFO)<<"筛选点云点数 :"<<locate_cloud->size();
 
     //第三步,创建测量任务, 进入测量中
     Locate_task* locate_task=new Locate_task();
     locate_task->set_locate_cloud(locate_cloud);
+    locate_task->set_save_path(project_path);
     m_terminor_statu=TERMINOR_MEASURING;
     code=mp_locater->execute_task(locate_task);
     if(code!=SUCCESS)
@@ -276,56 +360,15 @@ Error_manager Terminor_Command_Executor::working()
         //对于失败情况作分别处理
     }
     //获取测量结果
-    Locate_information locate_information=locate_task->get_locate_information();
+    m_measure_information=locate_task->get_locate_information();
     //释放locate task
     if(locate_task!=NULL)
     {
         delete locate_task;
         locate_task=NULL;
     }
-    //第四步,测量完成,上传数据
-    m_terminor_statu=TERMINOR_POSTING;
-    measure_result measure_information;
-    measure_information.x=locate_information.locate_x;
-    measure_information.y=locate_information.locate_y;
-    measure_information.angle=locate_information.locate_theta;
-    measure_information.length=locate_information.locate_length;
-    measure_information.width=locate_information.locate_width;
-    measure_information.height=locate_information.locate_hight;
-    measure_information.wheel_base=locate_information.locate_wheel_base;
-    measure_information.terminal_id=m_terminor_parameter.id();
-    measure_information.correctness=locate_information.locate_correct;
 
-    //执行plc上传任务
-    Plc_Task* plc_task=new Plc_Task();
-    plc_task->set_result(measure_information);
-    code=mp_plc->execute_task(plc_task);
-    while(code!=SUCCESS)
-    {
-        //判断是否强制退出
-        if(mb_force_quit==true)
-        {
-            char description[255]={0};
-            sprintf(description,"ternimal is force quit terminal id : %d",m_terminor_parameter.id());
-            return Error_manager(TERMINOR_FORCE_QUIT,NORMAL,description);
-        }
-        //对于上传不成功,作失败处理,重新传输
-        code=mp_plc->execute_task(plc_task);
-        ///不成功,检测本次流程是否超时
-        if(code!=SUCCESS)
-        {
-            auto t_end_point=std::chrono::system_clock::now();
-            auto duration=std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
-            double second=double(duration.count()) *
-                std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
-            if(second>m_timeout_second)
-            {
-                //扫描超时处理
 
-            }
-            usleep(1000*100);
-        }
 
-    }
     return code;
 }

terminor/terminal_command_executor.h

@@ -62,10 +62,13 @@ protected:
     /*
      * 执行指令流程函数
      * 扫描
-     * 测量
-     * 上传数据
+     * 测量,保存测量结果到成员变量
      */
-    Error_manager working();
+    Error_manager scanning_measuring();
+    /*
+     * 执行上传plc任务
+     */
+    Error_manager post_measure_information();
 protected:
     TerminorStatu               m_terminor_statu;
     //指令流程线程
@@ -84,6 +87,8 @@ protected:
     float                       m_timeout_second;
     //强制退出标示
     bool                        mb_force_quit;
+    //保存当前指令测量结果
+    Locate_information          m_measure_information;
 
 };