DJ分支用于采集livox_mid40扫描单只轮胎点云

.gitignore

@@ -44,3 +44,7 @@ _ReSharper*/
 packages/
 build/
 >>>>>>> origin/yct
+cmake-build-debug/
+.vscode/
+.idea/
+

CMakeLists.txt

@@ -12,7 +12,7 @@ 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_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
 set(CMAKE_BUILD_TYPE "RELEASE")
 
 
@@ -44,9 +44,11 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/wj_lidar WJLIDAR_SRC )
 
 
 # plc test module
-add_executable(plc_test  ./test/plc_test.cpp ${PLC_SRC} ${TASK_MANAGER_SRC} ${TOOL_SRC} ${ERROR_CODE_SRC})
+add_executable(plc_test  ./test/plc_test.cpp ${PLC_SRC} ${TASK_MANAGER_SRC} ${TOOL_SRC} ${ERROR_SRC})
 target_link_libraries(plc_test ${OpenCV_LIBS}
-        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ipopt modbus libnanomsg.so libnnxx.a libglog.a libgflags.a)
+        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} snap7 ipopt modbus libnanomsg.so libnnxx.a libglog.a libgflags.a)
+
+
 add_executable(fence_debug  test/plc_s7.cpp wj_lidar/wj_lidar_msg.pb.cc)
 target_link_libraries(fence_debug ${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES} /usr/local/lib/libglog.a
         /usr/local/lib/libgflags.a nnxx nanomsg)

laser/Laser.cpp

@@ -240,6 +240,7 @@ Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
 	else
 	{
 		m_save_path = save_path;
+		std::cout<<"path:"<<save_path<<std::endl;
 		char pts_file[255] = { 0 };
 		sprintf(pts_file, "%s/pts%d.txt", save_path.c_str(), m_laser_id+1);
 		m_points_log_tool.open(pts_file);
@@ -337,8 +338,8 @@ void Laser_base::thread_transform()
 			bool t_pop_flag = m_queue_laser_data.try_pop(tp_binaty_buf);
 			if ( t_pop_flag )
 			{
-			    std::cout << "huli m_queue_laser_data.size() last = " << huli_test << std::endl;
-				std::cout << "huli m_queue_laser_data.size() now = " << m_queue_laser_data.size() << std::endl;
+			    //std::cout << "huli m_queue_laser_data.size() last = " << huli_test << std::endl;
+				//std::cout << "huli m_queue_laser_data.size() now = " << m_queue_laser_data.size() << std::endl;
 
 			}
 
@@ -347,8 +348,8 @@ void Laser_base::thread_transform()
 			//注注注注注意了，扫描停止后，m_last_data一定要保证清空，不能有垃圾数据残留，这会阻塞线程的暂停
 			if (t_pop_flag || m_last_data.get_length() > 0)
 			{
-				std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
-				std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
+				//std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
+				//std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
 				//缓存类型
 				Buf_type t_buf_type = BUF_UNKNOW;
 				//雷达扫描结果，三维点云（雷达自身坐标系）

laser/LivoxLaser.cpp

@@ -90,7 +90,7 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 
 		//设置保存文件的路径
 		std::string save_path = mp_laser_task->get_task_save_path();
-		t_error = set_open_save_path(save_path);
+		t_error = set_open_save_path(save_path,true);
 		if ( t_error != Error_code::SUCCESS )
 		{
 			//文件保存文件的路径的设置 允许失败。继续后面的动作
@@ -172,7 +172,6 @@ bool CLivoxLaser::receive_buf_to_queue(Binary_buf& binary_buf)
 	{
 		binary_buf = *tp_livox_buf;
 		delete tp_livox_buf;
-		std::cout << "huli m_queue_livox_data.try_pop" << std::endl;
 		return true;
 	}
 	return false;
@@ -383,13 +382,13 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 		{
 			if (livox->IsScanComplete())
 			{
-				std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
+				//std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
 				livox->stop_scan();
 				return;
 			}
 			else
 			{
-				std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
+				//std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
 
 			}
 //			std::cout << "huli LidarDataCallback " << std::endl;
@@ -401,7 +400,7 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 
 			Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
 			livox->m_queue_livox_data.push(data_bin);
-			std::cout << "huli m_queue_livox_data.push " << std::endl;
+			//std::cout << "huli m_queue_livox_data.push " << std::endl;
 
 			g_count[handle]++;
 
@@ -418,7 +417,7 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 	}
 	else
 	{
-		std::cout << "huli 2z error " << "data = "<< data  << std::endl;
-		std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
+		//std::cout << "huli 2z error " << "data = "<< data  << std::endl;
+		//std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
 	}
 }

laser/laser_task_command.cpp

@@ -171,17 +171,6 @@ void Laser_task::set_task_error_manager(Error_manager & error_manager)
     m_task_error_manager = error_manager;
 }
 
-//获取采集的点云保存路径
-std::string Laser_task::get_save_path()
-{
-    return m_save_path;
-}
-//设置采集的点云保存路径
-void Laser_task::set_save_path(std::string save_path)
-{
-    m_save_path=save_path;
-}
-
 
 
 

laser/laser_task_command.h

@@ -83,10 +83,6 @@ public:
     //设置 错误码
     void set_task_error_manager(Error_manager & error_manager);
 
-    //获取采集的点云保存路径
-    std::string get_save_path();
-    //设置采集的点云保存路径
-    void set_save_path(std::string save_path);
 
 protected:
     //点云的采集帧数最大值，任务输入

main.cpp

@@ -85,6 +85,7 @@ int main(int argc,char* argv[])
     System_Manager system_manager;
     //初始化实例,包括雷达,plc,locater,terminal的创建及初始化
     code=system_manager.init();
+
     if(code!=SUCCESS)
     {
         LOG(ERROR)<<code.to_string();
@@ -95,6 +96,23 @@ int main(int argc,char* argv[])
         LOG(INFO)<<"SYSTEM Init OK !!!!";
     }
 
+    while(1)
+    {
+        char c;
+        std::cout<<"input 'c' to continue  'q' to quite"<<std::endl;
+        std::cin>>c;
+        if(c=='q')
+        {
+            break;
+        }
+        code=system_manager.start_DJ_scanning();
+        if(code!=SUCCESS)
+        {
+            LOG(ERROR)<<code.to_string();
+        }
+        usleep(100);
+    }
+
     getchar();
 
     return 0;

system_manager/System_Manager.cpp

@@ -19,22 +19,19 @@ using google::protobuf::Message;
 System_Manager::System_Manager()
 {
     m_laser_vector.clear();
-    m_terminal_vector.clear();
+    mp_terminal=NULL;
     mp_plc=NULL;
     mp_locater=NULL;
-    mp_verify_result_tool=NULL;
-    mp_wj_controller=NULL;
+
 }
 System_Manager::~System_Manager()
 {
     //第一步, 清除流程,保证不会再调用plc,然后最先析构plc,防止回调崩溃
-    for(int i=0;i<m_terminal_vector.size();++i)
+    if(mp_terminal!=NULL)
     {
-        if(m_terminal_vector[i]!=NULL)
-        {
-            m_terminal_vector[i]->force_stop_command();
-        }
+        mp_terminal->force_stop_command();
     }
+
     //析构plc
     if(mp_plc!=NULL)
     {
@@ -42,15 +39,11 @@ System_Manager::~System_Manager()
         mp_plc=NULL;
     }
     //第二步析构terminor
-    for(int i=0;i<m_terminal_vector.size();++i)
+    if(mp_terminal!=NULL)
     {
-        if(m_terminal_vector[i]!=NULL)
-        {
-            delete m_terminal_vector[i];
-            m_terminal_vector[i]=NULL;
-        }
+        delete mp_terminal;
+        mp_terminal=NULL;
     }
-    m_terminal_vector.clear();
 
     //析构locater
     if(mp_locater!=NULL)
@@ -69,16 +62,7 @@ System_Manager::~System_Manager()
     }
     m_laser_vector.clear();
 
-    if(mp_verify_result_tool!=NULL)
-    {
-        delete mp_verify_result_tool;
-        mp_verify_result_tool=NULL;
-    }
-    if(mp_wj_controller!=NULL)
-    {
-        delete mp_wj_controller;
-        mp_wj_controller=NULL;
-    }
+
 
 }
 //初始化各个模块,包括雷达,plc,locater,terminor
@@ -94,30 +78,7 @@ Error_manager System_Manager::init()
     {
         return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read laser parameter failed");
     }
-    //读取万集雷达配置
-    wj::wjManagerParams wj_parameter;
-    if(!read_proto_param("./setting/wj_manager_settings.prototxt",wj_parameter))
-    {
-        return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read wl_lidar parameter failed");
-    }
-    //读取plc配置
-    plc_module::plc_connection_params plc_parameter;
-    if(!read_proto_param("./setting/plc.prototxt",plc_parameter))
-    {
-        return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read plc parameter failed");
-    }
-    //读取locate配置
-    Measure::LocateParameter locater_parameter;
-    if(!read_proto_param("./setting/locate.prototxt",locater_parameter))
-    {
-        return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read locate parameter failed");
-    }
-    //读取结果检验器配置
-    Hardware_limit::Hardware_parameter hardware_parameter;
-    if(!read_proto_param("./setting/limit.prototxt",hardware_parameter))
-    {
-        return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read limit parameter failed");
-    }
+
     //读取terminor配置
     Terminal::Terminor_parameter_all terminor_parameter_all;
     if(!read_proto_param("./setting/terminal.prototxt",terminor_parameter_all))
@@ -126,7 +87,7 @@ Error_manager System_Manager::init()
     }
     m_terminal_parameter_all=terminor_parameter_all;
     //第二步,根据配置,创建各个模块
-    //创建大疆雷达
+    //创建雷达
     int laser_cout=laser_parameters.laser_parameters_size();
     m_laser_vector.resize(laser_cout);
     for(int i=0;i<laser_parameters.laser_parameters_size();++i)
@@ -157,45 +118,15 @@ Error_manager System_Manager::init()
             break;
         }
     }
-    //创建limit模块
-    mp_verify_result_tool=new Verify_result(hardware_parameter);
-    LOG(WARNING)<<"0 verify addr : "<<mp_verify_result_tool;
-    //创建万集雷达
-    mp_wj_controller=new Fence_controller(mp_verify_result_tool);
-    code=mp_wj_controller->initialize(wj_parameter);
     if(code!=SUCCESS)
     {
         return code;
     }
 
-    //创建测量模块
-    mp_locater=new Locater();
-    code=mp_locater->init(locater_parameter);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
     //创建terminor
-    int terminor_count=terminor_parameter_all.terminor_parameters_size();
-    if(terminor_count<=0)
-    {
-        return Error_manager(SYSTEM_PARAMETER_ERROR,NORMAL,"parameter error terminor num is 0");
-    }
-    m_terminal_vector.resize(terminor_count);
-    for(int i=0;i<terminor_count;++i)
-    {
-        m_terminal_vector[i]=new Terminor_Command_Executor(terminor_parameter_all.terminor_parameters(i));
-        m_terminal_vector[i]->set_save_root_path(terminor_parameter_all.save_root_path());
-    }
-    //最后创建plc,连接,设置回调
-    mp_plc=new Plc_Communicator(plc_parameter);
-    code=mp_plc->get_error();
-    if(code!=SUCCESS)
-    {
-        code.set_error_description("plc create error");
-        return code;
-    }
-    code=mp_plc->set_plc_callback(plc_command_callback,this);
+    mp_terminal=new Terminor_Command_Executor(terminor_parameter_all.terminor_parameters(0));
+    mp_terminal->set_save_root_path(terminor_parameter_all.save_root_path());
+
     return code;
 
 }
@@ -203,37 +134,22 @@ Error_manager System_Manager::init()
 //plc指令回调函数,当plc收到测量指令后,调用此回调函数
 //terminal_id:收到指令的终端编号
 //owner:this指针
-Error_manager System_Manager::plc_command_callback(int terminal_id_plc,void* owner)
+Error_manager System_Manager::start_DJ_scanning()
 {
-    int terminal_id=terminal_id_plc-1;
     Error_manager code;
-    LOG(INFO)<<"RECEIVE terminal command , ID : "<<terminal_id;
+
     //第一步判断输入参数是否合法
-    if(owner==NULL)
-    {
-        return Error_manager(PARAMETER_ERROR,NEGLIGIBLE_ERROR,"plc command callback input parameter(owner) NULL");
-    }
-    System_Manager* system_manager=(System_Manager*)owner;
-    if(terminal_id<0||terminal_id>=system_manager->m_terminal_vector.size())
-    {
-        char description[255]={0};
-        sprintf(description,"terminal command id(%d) out of range, terminal size:%d",
-            terminal_id,system_manager->m_terminal_vector.size());
-        return Error_manager(PARAMETER_ERROR,NEGLIGIBLE_ERROR,description);
-    }
+
+    System_Manager* system_manager=this;
+
     //第二步,根据terminal_id,启动对应terminaor执行指令流程,
 
     if(system_manager->m_laser_vector.size()==0)
     {
         return Error_manager(SYSTEM_INPUT_TERMINOR_NO_LASERS,NORMAL,"laser parameter has no laser");
     }
-    if(system_manager->mp_wj_controller==NULL)
-    {
-        Error_manager(PARAMETER_ERROR,NEGLIGIBLE_ERROR,"wj lidar is null");
-    }
-    code=system_manager->m_terminal_vector[terminal_id]->execute_command(system_manager->m_laser_vector,
-                                                                         system_manager->mp_wj_controller,
-        system_manager->mp_plc,system_manager->mp_locater,system_manager->mp_verify_result_tool,10);
+
+    code=system_manager->mp_terminal->execute_command(system_manager->m_laser_vector,10);
     switch(code.get_error_code())
     {
         case SUCCESS:break;

system_manager/System_Manager.h

@@ -5,11 +5,7 @@
 #ifndef SYSTEM_MANAGER_H
 #define SYSTEM_MANAGER_H
 #include "../laser/Laser.h"
-#include "../plc/plc_communicator.h"
-#include "../locate/locater.h"
 #include "../terminor/terminal_command_executor.h"
-#include "../verify/Verify_result.h"
-#include "../wj_lidar/fence_controller.h"
 
 class System_Manager
 {
@@ -24,26 +20,22 @@ public:
     //file:文件
     //parameter:要读取的配置
     static bool read_proto_param(std::string file, ::google::protobuf::Message& parameter);
-protected:
+public:
     //plc指令回调函数,当plc收到测量指令后,调用此回调函数
     //terminal_id:收到指令的终端编号
     //owner:this指针
-    static Error_manager plc_command_callback(int terminal_id,void* owner);
+    Error_manager start_DJ_scanning();
 
 
 
 protected:
     std::vector<Laser_base*>                    m_laser_vector;
-    std::vector<Terminor_Command_Executor*>     m_terminal_vector;
+    Terminor_Command_Executor*                  mp_terminal;
     Locater*                                    mp_locater;
     Plc_Communicator*                           mp_plc;
 
-    //结果检验工具
-    Verify_result*                              mp_verify_result_tool;
     ///terminoal 配置
     Terminal::Terminor_parameter_all            m_terminal_parameter_all;
-    //万集雷达管理对象
-    Fence_controller*                           mp_wj_controller;
 
 };
 

terminor/terminal_command_executor.cpp

@@ -53,7 +53,6 @@ Terminor_Command_Executor::Terminor_Command_Executor(Terminal::Terminor_paramete
     :m_terminor_statu(TERMINOR_READY), mp_command_thread(0),m_timeout_second(15),mb_force_quit(false)
 {
     m_terminor_parameter=parameter;
-    mp_wj_lidar=NULL;
 }
 
 Error_manager Terminor_Command_Executor::force_stop_command()
@@ -91,8 +90,7 @@ TerminorStatu Terminor_Command_Executor::get_terminor_statu()
     return m_terminor_statu;
 }
 
-Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*> lasers,Fence_controller* wj_lidar,
-    Plc_Communicator* plc,Locater* locater,Verify_result* verify_tool,float timeout)
+Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*> lasers,float timeout)
 {
     Error_manager code;
     //第一步检查输入参数合法性
@@ -105,25 +103,6 @@ Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*
             return Error_manager(TERMINOR_INPUT_LASER_NULL,NORMAL,description);
         }
     }
-    if(plc==0)
-    {
-        char description[255]={0};
-        sprintf(description,"terminor input plc* is null terminor id:%d",m_terminor_parameter.id());
-        return Error_manager(TERMINOR_INPUT_PLC_NULL,NORMAL,description);
-    }
-    if(wj_lidar==NULL)
-    {
-        char description[255]={0};
-        sprintf(description,"terminor input wj_lidar* is null  terminor id:%d",m_terminor_parameter.id());
-        return Error_manager(TERMINOR_INPUT_LOCATER_NULL,NORMAL,description);
-    }
-    if(locater==0)
-    {
-        char description[255]={0};
-        sprintf(description,"terminor input locater* is null  terminor id:%d",m_terminor_parameter.id());
-        return Error_manager(TERMINOR_INPUT_LOCATER_NULL,NORMAL,description);
-    }
-    mp_verify_tool=verify_tool;
 
     //第二步,雷达,plc以及定位模块是否能正常接收指令;
     for(int i=0;i<lasers.size();++i)
@@ -135,20 +114,7 @@ Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*
             return code;
         }
     }
-    //检查plc状态
-    code=plc->get_error();
-    if(code!=SUCCESS)
-    {
-        LOG(ERROR)<<"terminor check plc error : "<<code.to_string()<<"  terminor id:"<<m_terminor_parameter.id();
-        return code;
-    }
-    //检查测量模块状态
-    code=locater->get_error();
-    if(code!=SUCCESS)
-    {
-        LOG(ERROR)<<"terminor check locater error : "<<code.to_string()<<"  terminor id:"<<m_terminor_parameter.id();
-        return code;
-    }
+
     // 检查当前终端指令执行器的状态是否空闲;如果空闲,进入测量流程,设置忙碌状态
     std::lock_guard<std::mutex> t_lock(m_mutex_lock);
     if(get_terminor_statu()!=TERMINOR_READY)
@@ -169,9 +135,7 @@ Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*
     }
     //第二步接受指令,保存输入参数,更新指令状态为TERMINOR_BUSY,启动内部工作线程.返回SUCCESS
     mp_laser_vector=lasers;
-    mp_plc=plc;
-    mp_wj_lidar=wj_lidar;
-    mp_locater=locater;
+
     m_terminor_statu=TERMINOR_BUSY;
     m_timeout_second=timeout;
     mp_command_thread=new std::thread(thread_command_working,this);
@@ -219,8 +183,6 @@ void Terminor_Command_Executor::thread_command_working(Terminor_Command_Executor
             break;
         }
     }
-    //根据结果执行上传任务
-    code=terminor->post_measure_information();
     //更新终端状态为Ready状态
     terminor->m_terminor_statu=TERMINOR_READY;
 
@@ -232,62 +194,6 @@ void Terminor_Command_Executor::thread_command_working(Terminor_Command_Executor
     }
 }
 
-/*
-     * 执行上传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;
-    //plc终端编号从1开始,因此,上传给plc的终端编号必须+1
-    measure_information.terminal_id=m_terminor_parameter.id()+1;
-    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;
-}
-
 void Terminor_Command_Executor::set_save_root_path(std::string root)
 {
     m_save_root_path=root;
@@ -349,7 +255,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
                 //
                 laser_task->task_init(TASK_CREATED,scan_cloud,&cloud_lock);
                 laser_task->set_task_frame_maxnum(frame_count);
-                laser_task->set_save_path(project_path);
+                laser_task->set_task_save_path(project_path);
                 laser_task_vector.push_back(laser_task);
                 //发送任务单给雷达
                 code=tp_lasers[i]->execute_task(laser_task);
@@ -434,98 +340,6 @@ Error_manager Terminor_Command_Executor::scanning_measuring()
     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);
-    m_measure_information.locate_correct=false;
-    //获取测量结果
-    Locate_information dj_locate_information=locate_task->get_locate_information();
-    //释放locate task
-    if(locate_task!=NULL)
-    {
-        delete locate_task;
-        locate_task=NULL;
-    }
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-
-    //进入万集雷达流程--------
-    Wj_lidar_Task* wj_task=new Wj_lidar_Task();
-    code=wj_task->init();
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    wj_command t_wj_command;
-    t_wj_command.terminal_id=m_terminor_parameter.id();
-    t_wj_command.command_time=std::chrono::steady_clock::now();
-    t_wj_command.timeout_in_milliseconds=2000;
-    wj_task->set_command(t_wj_command);
-    code=mp_wj_lidar->execute_task(wj_task);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    ///万集测量正确,对比两数据
-    wj_measure_result wj_result;
-    code=wj_task->get_result(wj_result);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    Locate_information wj_locate_information;
-    wj_locate_information.locate_x=wj_result.x;
-    wj_locate_information.locate_y=wj_result.y;
-    wj_locate_information.locate_theta=wj_result.angle;
-    wj_locate_information.locate_wheel_base=wj_result.wheel_base;
-    wj_locate_information.locate_width=wj_result.width;
-    //对比两个数据
-    float offset_x=fabs(dj_locate_information.locate_x-wj_locate_information.locate_x);
-    float offset_y=fabs(dj_locate_information.locate_y-wj_locate_information.locate_y);
-    float offset_angle=fabs(dj_locate_information.locate_theta-wj_locate_information.locate_theta);
-    float offset_width=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
-    float offset_wheel_base=fabs(dj_locate_information.locate_width-wj_locate_information.locate_width);
-    if(offset_x>100 ||offset_y>100 ||offset_angle>2 || offset_wheel_base>200 ||offset_width>100)
-    {
-        return Error_manager(TERMINOR_CHECK_RESULTS_ERROR,NORMAL,"check results failed ");
-    }
-    ///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以2750为基准作卡尔曼滤波
-    ///车长以大疆雷达为准,车宽以万集雷达为准,高以大疆雷达为准
-    m_measure_information.locate_x=wj_locate_information.locate_x;
-    m_measure_information.locate_y=wj_locate_information.locate_y;
-    m_measure_information.locate_theta=0.5*(wj_locate_information.locate_theta+dj_locate_information.locate_theta);
-    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_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;
-    m_measure_information.locate_correct=true;
-    ////如果测量正确,检验结果
-    if(mp_verify_tool!=NULL) {
-
-        cv::RotatedRect rotated_rect;
-        rotated_rect.center = cv::Point2f(m_measure_information.locate_x, m_measure_information.locate_y);
-        rotated_rect.angle = m_measure_information.locate_theta;
-        float ext_length=720;
-        float robot_width=2650.0;
-        float new_length=m_measure_information.locate_length+ext_length*2.0;
-        rotated_rect=create_rotate_rect(new_length,robot_width,m_measure_information.locate_theta,
-                                        m_measure_information.locate_x, m_measure_information.locate_y);
-        code = mp_verify_tool->verify(rotated_rect,m_measure_information.locate_hight, false);
-        m_measure_information.locate_correct = (code == SUCCESS);
-        return code;
-
-    }
-
     return code;
 }
 

terminor/terminal_command_executor.h

@@ -54,9 +54,7 @@ public:
      * verify_tool:结果检验工具,当该参数为NULL时,测量结果不作检验
      * 返回指令是否启动成功
      */
-    Error_manager execute_command(std::vector<Laser_base*> lasers,Fence_controller* wj_lidar,
-        Plc_Communicator* plc,
-        Locater* locater,Verify_result* verify_tool,float timeout=15);
+    Error_manager execute_command(std::vector<Laser_base*> lasers,float timeout=15);
     /*
      * 强制正在执行的中断指令
      */
@@ -75,11 +73,6 @@ protected:
      * 测量,保存测量结果到成员变量
      */
     Error_manager scanning_measuring();
-    /*
-     * 执行上传plc任务
-     * plc终端编号从1开始
-     */
-    Error_manager post_measure_information();
     /*
      * 根据长宽,角度,生成cv::RotateRect
      */
@@ -92,12 +85,7 @@ protected:
     std::mutex                  m_mutex_lock;
     //保存输入进来的雷达指针
     std::vector<Laser_base*>    mp_laser_vector;
-    //万集雷达测量模块
-    Fence_controller*           mp_wj_lidar;
-    //plc
-    Plc_Communicator*           mp_plc;
-    //locater*
-    Locater*                    mp_locater;
+
     //配置参数
     Terminal::Terminor_parameter    m_terminor_parameter;
     //本次指令超时时间 单位秒
@@ -108,8 +96,7 @@ protected:
     Locate_information          m_measure_information;
     //保存文件的root目录
     std::string                 m_save_root_path;
-    //检验结果工具
-    Verify_result*              mp_verify_tool;
+
 
 };