LidarMeasure/terminor/terminal_command_executor.cpp

#include "terminal_command_executor.h"
#include <glog/logging.h>
#include <chrono>
#include <pcl/filters/passthrough.h>
#include "../tool/pathcreator.h"

//////以下两个函数用于测试,读取指定点云
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),mb_force_quit(false)
{
    m_terminor_parameter=parameter;
}

Error_manager Terminor_Command_Executor::force_stop_command()
{
    mb_force_quit=true;
    //等待执行线程退出
    if(mp_command_thread!=NULL)
    {
        if(mp_command_thread->joinable())
        {
            mp_command_thread->join();
            delete mp_command_thread;
            mp_command_thread=NULL;
        }
    }
    return SUCCESS;
}

Terminor_Command_Executor::~Terminor_Command_Executor()
{
    if(mp_command_thread!=0)
    {
        force_stop_command();
        if(mp_command_thread->joinable())
        {
            mp_command_thread->join();
            delete mp_command_thread;
            mp_command_thread=0;
        }
    }
}

TerminorStatu Terminor_Command_Executor::get_terminor_statu()
{
    return m_terminor_statu;
}

Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*> lasers,Plc_Communicator* plc,
                    Locater* locater,float timeout)
{
    Error_manager code;
    //第一步检查输入参数合法性
    for(int i=0;i<lasers.size();++i)
    {
        if(lasers[i]==0)
        {
            char description[255]={0};
            sprintf(description,"terminor input laser* is null index:%d, terminor id:%d",i,m_terminor_parameter.id());
            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(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);
    }

    //第二步,雷达,plc以及定位模块是否能正常接收指令;
    for(int i=0;i<lasers.size();++i)
    {
        code=lasers[i]->check_error();
        if(code!=SUCCESS)
        {
            LOG(ERROR)<<" terminor check laser error:"<<code.to_string()<<"   terminor id :"<<m_terminor_parameter.id();
            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)
    {
        char description[255]={0};
        sprintf(description,"terminor is not ready, terminor id:%d",m_terminor_parameter.id());
        return Error_manager(TERMINOR_NOT_READY,NORMAL,description);
    }
    //检查上次线程是否退出,并等待线程退出
    if(mp_command_thread!=0)
    {
        if(mp_command_thread->joinable())
        {
            mp_command_thread->join();
            delete mp_command_thread;
            mp_command_thread=0;
        }
    }
    //第二步接受指令,保存输入参数,更新指令状态为,启动内部工作线程.返回SUCCESS
    mp_laser_vector=lasers;
    mp_plc=plc;
    mp_locater=locater;
    m_terminor_statu=TERMINOR_BUSY;
    m_timeout_second=timeout;
    mp_command_thread=new std::thread(thread_command_working,this);
    if(mp_command_thread==0)
    {
        return Error_manager(TERMINOR_CREATE_WORKING_THREAD_FAILED,NORMAL,"terminor create thread failed");
    }
    return SUCCESS;
}

void Terminor_Command_Executor::thread_command_working(Terminor_Command_Executor* terminor)
{
    if(terminor==NULL)
    {
        return ;
    }
    //执行指令,阻塞直到指令执行完成或者异常结束
    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::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,&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;
    //等待雷达完成任务单
    while(1)
    {
        //判断是否强制退出
        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);
        }
        //判断雷达任务单是否全部完成
        bool tb_laser_complete=true;
        for(int i=0;i<laser_task_vector.size();++i)
        {
            tb_laser_complete &=(TASK_OVER==laser_task_vector[i]->get_statu());
        }
        if(tb_laser_complete)
        {
            break;
        }
        //计算扫描时间,若超时,并且没有点,则返回错误.
        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)
        {
            //扫描超时,点云中没有点,则返回错误
            if(scan_cloud->size()==0)
            {
                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)
    {
        if(laser_task_vector[i]!=0)
        {
            delete laser_task_vector[i];
            laser_task_vector[i]=NULL;
        }
    }
    //第二步,根据区域筛选点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PassThrough<pcl::PointXYZ> passX;
    pcl::PassThrough<pcl::PointXYZ> passY;
    pcl::PassThrough<pcl::PointXYZ> passZ;
    passX.setInputCloud(scan_cloud);
    passX.setFilterFieldName("x");
    passX.setFilterLimits(m_terminor_parameter.area_3d().min_x(),m_terminor_parameter.area_3d().max_x());
    passX.filter(*locate_cloud);

    passY.setInputCloud(locate_cloud);
    passY.setFilterFieldName("y");
    passY.setFilterLimits(m_terminor_parameter.area_3d().min_y(),m_terminor_parameter.area_3d().max_y());
    passY.filter(*locate_cloud);

    passY.setInputCloud(locate_cloud);
    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)
    {
        //对于失败情况作分别处理
    }
    //获取测量结果
    m_measure_information=locate_task->get_locate_information();
    //释放locate task
    if(locate_task!=NULL)
    {
        delete locate_task;
        locate_task=NULL;
    }



    return code;
}