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>
#include <livox_sdk.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()
{
    MeasureTopicPublisher::GetInstance();
    Error_manager code;
    //读laser配置
    Laser_proto::Laser_parameter_all laser_parameters;
    if(!read_proto_param("./setting/laser.prototxt",laser_parameters))
    {
        return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read laser 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");
    }
    //读取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");
    }
    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)
    {
        m_laser_vector[i]=LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(),i,
                                                      laser_parameters.laser_parameters(i));
        if(m_laser_vector[i]!=NULL)
        {
            if(!m_laser_vector[i]->Connect())
            {
                char description[255]={0};
                sprintf(description,"Laser %d connect failed...",i);
                return Error_manager(LASER_CONNECT_FAILED,NORMAL,description);
            }
        }
    }
    //查询是否有livox雷达,初始化库
    for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
    {
        std::string type = laser_parameters.laser_parameters(i).type();
        if (type.find("Livox") != type.npos)
        {
            if (Start() == false)
            {
                Uninit();
                return Error_manager(LASER_LIVOX_SKD_INIT_FAILED,NORMAL,"Livox laser init failed...");
            }
            break;
        }
    }
    //创建测量模块
    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);
    return code;

}

//plc指令回调函数,当plc收到测量指令后,调用此回调函数
//terminal_id:收到指令的终端编号
//owner:this指针
Error_manager System_Manager::plc_command_callback(int terminal_id_plc,void* owner)
{
    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);
    }
    //第二步,根据terminal_id,启动对应terminaor执行指令流程
    //根据配置筛选雷达,当前测试使用所有雷达
    std::vector<Laser_base*> tp_lasers;
    for(int i=0;i<system_manager->m_terminal_parameter_all.terminor_parameters(terminal_id).laser_parameter_size();++i)
    {
        int laser_id=system_manager->m_terminal_parameter_all.terminor_parameters(terminal_id).laser_parameter(i).id();
        if(laser_id>=0&&laser_id<system_manager->m_laser_vector.size())
        {
            //判断该id的雷达是否已经添加进去, 放置配置中出现重复的雷达编号
            bool tb_repeat=false;
            for(int j=0;j<tp_lasers.size();++j)
            {
                if(tp_lasers[j]==system_manager->m_laser_vector[laser_id])
                {
                    tb_repeat=true;
                    break;
                }
            }
            if(tb_repeat==false)
                tp_lasers.push_back(system_manager->m_laser_vector[laser_id]);
        }
    }
    if(tp_lasers.size()==0)
    {
        return Error_manager(SYSTEM_INPUT_TERMINOR_NO_LASERS,NORMAL,"ternimal command has no laser");
    }
    code=system_manager->m_terminal_vector[terminal_id]->execute_command(tp_lasers,
        system_manager->mp_plc,system_manager->mp_locater,10);
    switch(code.get_error_code())
    {
        case SUCCESS:break;
        case TERMINOR_CREATE_WORKING_THREAD_FAILED:
        {
            LOG(ERROR)<<code.to_string();
            break;
        }
        default:
        {
            LOG(WARNING)<<code.to_string();
            break;
        }
    }
    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;
}