修改流程为测试版,跳过laser测试,新增system_manager

CMakeLists.txt

@@ -37,13 +37,14 @@ 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})
 #
 
 
 add_executable(locate  main.cpp ${LOCATE_SRC} ${PLC_SRC} ${TERMINOR_SRC} ${TASK_MANAGER_SRC} ${LASER_SRC} ${ERROR_SRC}
-        ${TOOL_SRC})
+        ${TOOL_SRC} ${SYS_SRC})
 target_link_libraries(locate ${OpenCV_LIBS}
         ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ipopt libtensorflow_cc.so
         tf_3dcnn_api.so pointSIFT_API.so dark.so /usr/local/lib/libmodbus.so /usr/local/lib/libglog.a

error_code/error_code.h

@@ -129,7 +129,9 @@ enum Error_code
     LOCATER_3DCNN_IIU_FAILED,
     LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
 
-    //terminor error from 0x04010000-0x0401FFFF
+    //System_manager error from 0x04010000-0x0401FFFF
+    SYSTEM_READ_PARAMETER_ERROR=0x04010100,
+    SYSTEM_PARAMETER_ERROR,
 
     //terminor_command_executor.cpp from 0x04010200-0x040102FF
     TERMINOR_NOT_READY=0x04010200,

main.cpp

@@ -8,10 +8,10 @@
 #include "plc/plc_communicator.h"
 #include "locate/locater.h"
 #include "terminor/terminal_command_executor.h"
+#include "system_manager/System_Manager.h"
+#include "tool/pathcreator.h"
 #include <glog/logging.h>
-#include <google/protobuf/io/coded_stream.h>
-#include <google/protobuf/io/zero_copy_stream_impl.h>
-#include <google/protobuf/text_format.h>
+
 using google::protobuf::io::FileInputStream;
 using google::protobuf::io::FileOutputStream;
 using google::protobuf::io::ZeroCopyInputStream;
@@ -19,87 +19,76 @@ using google::protobuf::io::CodedInputStream;
 using google::protobuf::io::ZeroCopyOutputStream;
 using google::protobuf::io::CodedOutputStream;
 using google::protobuf::Message;
-
-bool read_proto_param(std::string path, ::google::protobuf::Message& param)
+GOOGLE_GLOG_DLL_DECL void ShutdownGoogleLogging(const char* data, int size)
 {
-    int fd = open(path.c_str(), O_RDONLY);
-    if (fd == -1) return false;
-    FileInputStream* input = new FileInputStream(fd);
-    bool success = google::protobuf::TextFormat::Parse(input, &param);
-    delete input;
-    close(fd);
-    return success;
-}
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
+            t->tm_year + 1900,
+            t->tm_mon + 1,
+            t->tm_mday,
+            t->tm_hour,
+            t->tm_min,
+            t->tm_sec);
 
-int main(int argc,char* argv[])
-{
-    Error_manager code;
-    //第一步,读取各个模块的配置
-    //读取laser配置,根据配置创建laser
+    FILE* tp_file=fopen(buf,"w");
+    fprintf(tp_file,data,size);
+    fclose(tp_file);
 
-    //读取locate配置
-    Measure::LocateParameter locater_parameter;
-    if(!read_proto_param("",locater_parameter))
-    {
-        LOG(ERROR)<<"read locate parameter failed";
-        return -1;
-    }
-    //读取terminor配置
-    Terminal::Terminor_parameter_all terminor_parameter_all;
-    if(!read_proto_param("",terminor_parameter_all))
-    {
-        LOG(ERROR)<<"read terminors parameter failed";
-        return -1;
-    }
-    //第二步,根据配置,创建各个模块
-    //创建雷达
-    //创建plc
-    Plc_Communicator* plc=0;//new Plc_Communicator()
-    //创建测量模块
-    Locater* p_locater=new Locater();
-    code=p_locater->init(locater_parameter);
-    if(code!=SUCCESS)
-    {
-        LOG(ERROR)<<"locater init failed code:"<<code.to_string();
-        return -1;
-    }
-    //创建terminor
-    int terminor_count=terminor_parameter_all.terminor_parameters_size();
-    if(terminor_count<=0)
-    {
-        LOG(ERROR)<<"parameter error terminor num is 0";
-        return -1;
-    }
-    Terminor_Command_Executor** p_terminor_executors=0;
-    p_terminor_executors=new Terminor_Command_Executor*[terminor_count];
-    for(int i=0;i<terminor_count;++i)
-    {
-        p_terminor_executors[i]=new Terminor_Command_Executor(terminor_parameter_all.terminor_parameters(i));
-    }
-    //检查laser ,plc ,locater模块状态
-
-    //plc模块设置指令回调函数回调
+}
 
+void InitGlog()
+{
+    time_t tt = time(0);//时间cuo
+    struct tm* t = localtime(&tt);
 
-    getchar();
+    char strYear[255]={0};
+    char strMonth[255]={0};
+    char strDay[255]={0};
 
+    sprintf(strYear,"%04d", t->tm_year+1900);
+    sprintf(strMonth,"%02d", t->tm_mon);
+    sprintf(strDay,"%02d", t->tm_mday);
 
+    char buf[255]={0};
+    getcwd(buf,255);
+    char strdir[255]={0};
+    sprintf(strdir,"%s/log/%s/%s/%s", buf,strYear,strMonth,strDay);
+    PathCreator creator;
+    creator.Mkdir(strdir);
 
+    char logPath[255] = { 0 };
+    sprintf(logPath, "%s/", strdir);
+    FLAGS_max_log_size = 100;
+    FLAGS_logbufsecs = 0;
+    google::InitGoogleLogging("LidarMeasurement");
+    google::SetStderrLogging(google::INFO);
+    google::SetLogDestination(0, logPath);
+    google::InstallFailureSignalHandler();
+    google::InstallFailureWriter(&ShutdownGoogleLogging);
+    FLAGS_colorlogtostderr = true;        // Set log color
+    FLAGS_logbufsecs = 0;                // Set log output speed(s)
+    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
+    FLAGS_stop_logging_if_full_disk = true;
+}
 
+int main(int argc,char* argv[])
+{
+    Error_manager code;
+    InitGlog();
 
-    /////释放对象,先释放terminor,再释放plc,locater ,laser
-    for(int i=0;i<terminor_count;++i)
+    System_Manager system_manager;
+    code=system_manager.init();
+    if(code!=SUCCESS)
     {
-        if(p_terminor_executors[i]!=0)
-        {
-            delete p_terminor_executors[i];
-            p_terminor_executors[i]=0;
-        }
+        LOG(ERROR)<<code.to_string();
+        printf("print ctrl-c to quite");
     }
-    delete[] p_terminor_executors;
-
-
 
+    getchar();
 
     return 0;
 }

system_manager/System_Manager.cpp

@@ -0,0 +1,170 @@
+//
+// Created by zx on 2020/1/3.
+//
+
+#include "System_Manager.h"
+#include <glog/logging.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+#include <fcntl.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+System_Manager::System_Manager()
+{
+    m_laser_vector.clear();
+    m_terminal_vector.clear();
+    mp_plc=NULL;
+    mp_locater=NULL;
+}
+System_Manager::~System_Manager()
+{
+    //第一步, 清除流程,保证不会再调用plc,然后最先析构plc,防止回调崩溃
+    for(int i=0;i<m_terminal_vector.size();++i)
+    {
+        if(m_terminal_vector[i]!=NULL)
+        {
+            m_terminal_vector[i]->force_stop_command();
+        }
+    }
+    //析构plc
+    if(mp_plc!=NULL)
+    {
+        delete mp_plc;
+        mp_plc=NULL;
+    }
+    //第二步析构terminor
+    for(int i=0;i<m_terminal_vector.size();++i)
+    {
+        if(m_terminal_vector[i]!=NULL)
+        {
+            delete m_terminal_vector[i];
+            m_terminal_vector[i]=NULL;
+        }
+    }
+    m_terminal_vector.clear();
+
+    //析构locater
+    if(mp_locater!=NULL)
+    {
+        delete mp_locater;
+        mp_locater=NULL;
+    }
+    //析构laser
+    for(int i=0;i<m_laser_vector.size();++i)
+    {
+        if(m_laser_vector[i]!=NULL)
+        {
+            delete m_laser_vector[i];
+            m_laser_vector[i]=NULL;
+        }
+    }
+    m_laser_vector.clear();
+
+
+
+}
+//初始化各个模块,包括雷达,plc,locater,terminor
+//1,读取运行环境下setting目录下的laser.prototxt,plc.prototxt,locater.prototxt,terminal.prototxt
+//2,根据配置创建雷达,plc,locater,terminal
+Error_manager System_Manager::init()
+{
+    Error_manager code;
+    //读取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");
+    }
+    //读取terminor配置
+    Terminal::Terminor_parameter_all terminor_parameter_all;
+    if(!read_proto_param("./setting/terminal.prototxt",terminor_parameter_all))
+    {
+        return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read terminal parameter failed");
+    }
+    //第二步,根据配置,创建各个模块
+    //创建雷达
+    //
+    //创建测量模块
+    Locater* p_locater=new Locater();
+    code=p_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));
+    }
+    //最后创建plc,连接,设置回调
+    Plc_Communicator* plc=new Plc_Communicator(plc_parameter);
+    code=plc->get_error();
+    if(code!=SUCCESS)
+    {
+        code.set_error_description("plc create error");
+        return code;
+    }
+    code=plc->set_plc_callback(plc_command_callback,this);
+    return code;
+
+}
+
+//plc指令回调函数,当plc收到测量指令后,调用此回调函数
+//terminal_id:收到指令的终端编号
+//owner:this指针
+Error_manager System_Manager::plc_command_callback(int terminal_id,void* owner)
+{
+    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);
+    }
+    //第二步,根据terminal_id,启动对应terminaor执行指令流程
+    //根据配置筛选雷达,当前测试使用所有雷达
+    code=system_manager->m_terminal_vector[terminal_id]->execute_command(system_manager->m_laser_vector,
+        system_manager->mp_plc,system_manager->mp_locater,10);
+    return code;
+
+}
+
+//读取protobuf 配置文件
+//file:文件
+//parameter:要读取的配置
+bool System_Manager::read_proto_param(std::string file, ::google::protobuf::Message& parameter)
+{
+    int fd = open(file.c_str(), O_RDONLY);
+    if (fd == -1) return false;
+    FileInputStream* input = new FileInputStream(fd);
+    bool success = google::protobuf::TextFormat::Parse(input, &parameter);
+    delete input;
+    close(fd);
+    return success;
+}

system_manager/System_Manager.h

@@ -0,0 +1,43 @@
+//
+// Created by zx on 2020/1/3.
+//
+
+#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"
+
+class System_Manager
+{
+public:
+    System_Manager();
+    ~System_Manager();
+    //初始化各个模块,包括雷达,plc,locater,terminor
+    //1,读取运行环境下setting目录下的laser.prototxt,plc.prototxt,locater.prototxt,terminal.prototxt
+    //2,根据配置创建雷达,plc,locater,terminal
+    Error_manager init();
+    //读取protobuf 配置文件
+    //file:文件
+    //parameter:要读取的配置
+    static bool read_proto_param(std::string file, ::google::protobuf::Message& parameter);
+protected:
+    //plc指令回调函数,当plc收到测量指令后,调用此回调函数
+    //terminal_id:收到指令的终端编号
+    //owner:this指针
+    static Error_manager plc_command_callback(int terminal_id,void* owner);
+
+
+
+protected:
+    std::vector<Laser_base*>                    m_laser_vector;
+    std::vector<Terminor_Command_Executor*>     m_terminal_vector;
+    Locater*                                    mp_locater;
+    Plc_Communicator*                           mp_plc;
+
+
+};
+
+
+#endif //SYSTEM_MANAGER_H

terminor/terminal_command_executor.cpp

@@ -1,6 +1,49 @@
 #include "terminal_command_executor.h"
 #include <glog/logging.h>
 #include <chrono>
+
+//////以下两个函数用于测试,读取指定点云
+bool string2point(std::string str,pcl::PointXYZ& point)
+{
+    std::istringstream iss;
+    iss.str(str);
+    float value;
+    float data[3]={0};
+    for(int i=0;i<3;++i)
+    {
+        if(iss>>value)
+        {
+            data[i]=value;
+        }
+        else
+        {
+            return false;
+        }
+    }
+    point.x=data[0];
+    point.y=data[1];
+    point.z=data[2];
+    return true;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
+{
+    std::ifstream fin(file.c_str());
+    const int line_length=64;
+    char str[line_length]={0};
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    while(fin.getline(str,line_length))
+    {
+        pcl::PointXYZ point;
+        if(string2point(str,point))
+        {
+            cloud->push_back(point);
+        }
+    }
+    return cloud;
+}
+//////////
+
 Terminor_Command_Executor::Terminor_Command_Executor(Terminal::Terminor_parameter parameter)
     :m_terminor_statu(TERMINOR_READY), mp_command_thread(0),m_timeout_second(15)
 {
@@ -56,7 +99,6 @@ Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*
     }
 
     //第二步,雷达,plc以及定位模块是否能正常接收指令;
-    // 检查当前终端指令执行器的状态是否空闲;
     for(int i=0;i<lasers.size();++i)
     {
         code=lasers[i]->get_error();
@@ -66,18 +108,21 @@ 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)
     {
@@ -164,7 +209,8 @@ Error_manager Terminor_Command_Executor::working()
     }
     //第二步,根据区域筛选点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-
+    ///测试读取指令点云
+    locate_cloud=ReadTxtCloud("/home/zx/zzw/code/Detection_src/LidarCppAD/extract/black_car_sample/20191217-221612.txt");
     //第三步,创建测量任务, 进入测量中
     Locate_task* locate_task=new Locate_task();
     locate_task->set_locate_cloud(locate_cloud);

terminor/terminal_command_executor.h

@@ -44,9 +44,11 @@ public:
     TerminorStatu get_terminor_statu();
     /*
      * 执行扫描,测量任务,阻塞知道任务完成或超时(单位秒)
+     * 函数体只检测指令是否能执行,并启动线程执行指令,不等待指令完成.
      *lasers:需要启动的雷达
      * plc:上传结果工具
      * locater:测量算法对象
+     * 返回指令是否启动成功
      */
     Error_manager execute_command(std::vector<Laser_base*> lasers,Plc_Communicator* plc,
         Locater* locater,float timeout=15);
@@ -55,6 +57,7 @@ public:
      */
     Error_manager force_stop_command();
 protected:
+
     static void thread_command_working(Terminor_Command_Executor* terminor);
     /*
      * 执行指令流程函数

tool/pathcreator.cpp

@@ -0,0 +1,84 @@
+#include "pathcreator.h"
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include "../error_code/error_code.h"
+
+PathCreator::PathCreator()
+{
+
+}
+
+PathCreator::~PathCreator()
+{
+
+}
+
+int PathCreator::GetCurPath(std::string& path)
+{
+     path=m_current_path;
+     return SUCCESS;
+}
+int PathCreator::Mkdir(std::string dirName)
+{
+    uint32_t beginCmpPath = 0;
+    uint32_t endCmpPath = 0;
+    std::string fullPath = "";
+
+        if('/' != dirName[0])
+        {
+            fullPath = getcwd(nullptr, 0);
+            beginCmpPath = fullPath.size();
+            fullPath = fullPath + "/" + dirName;
+        }
+        else
+        {
+            //Absolute path
+            fullPath = dirName;
+            beginCmpPath = 1;
+        }
+        if (fullPath[fullPath.size() - 1] != '/')
+        {
+            fullPath += "/";
+        }
+        endCmpPath = fullPath.size();
+
+        //create dirs;
+        for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
+        {
+            if('/' == fullPath[i])
+            {
+                std::string curPath = fullPath.substr(0, i);
+                if(access(curPath.c_str(), F_OK) != 0)
+                {
+                    if(mkdir(curPath.c_str(), /*S_IRUSR|S_IRGRP|S_IROTH|S_IWUSR|S_IWGRP|S_IWOTH*/0777) == -1)
+                    {
+                        printf("mkdir(%s) failed\n", curPath.c_str());
+                        return FAILED;
+                    }
+                }
+            }
+        }
+        m_current_path=fullPath;
+        return SUCCESS;
+
+}
+
+int PathCreator::CreateDatePath(std::string root)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    sprintf(buf,"%s/%d%02d%02d-%02d%02d%02d",root.c_str(),
+               t->tm_year + 1900,
+               t->tm_mon + 1,
+               t->tm_mday,
+               t->tm_hour,
+               t->tm_min,
+               t->tm_sec);
+    return Mkdir(buf);
+}

tool/pathcreator.h

@@ -0,0 +1,17 @@
+#ifndef PATHCREATOR_H
+#define PATHCREATOR_H
+#include <string>
+
+class PathCreator
+{
+public:
+    PathCreator();
+    ~PathCreator();
+    int GetCurPath(std::string& path);
+    int Mkdir(std::string dir);
+    int CreateDatePath(std::string root);
+protected:
+    std::string m_current_path;
+};
+
+#endif // PATHCREATOR_H