LidarMeasure/system_manager/System_Manager.cpp

//
// 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;
}