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"
///增加获取wj电子围栏模块头文件
#include "terminor_msg.pb.h"
#include "MeasureRequest.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;
    mp_wj_lidar=NULL;
}

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,Fence_controller* wj_lidar,
    Plc_Communicator* plc,Locater* locater,Verify_result* verify_tool,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(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)
    {
        code=lasers[i]->check_laser();
        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;
        }
    }
    //第二步接受指令,保存输入参数,更新指令状态为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);
    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;
    int measure_times=0;
    while(terminor->mb_force_quit==false) {
        if (measure_times == 3)
            break;
        code = terminor->scanning_measuring();
        switch (code.get_error_code()) {
            case SUCCESS:
                break;
            case TERMINOR_LASER_TIMEOUT: {
                //雷达扫描超时,代表雷达故障,此时需要重置雷达模块
            }
        }

        if (code != SUCCESS ) {
            if(measure_times==2)
            {
                LOG(ERROR) << " Measure times("<<measure_times<<")  failed : "<< code.to_string();
            }
            else
            {
                LOG(WARNING) << " Measure times("<<measure_times<<")  failed : "<< code.to_string();
            }
            measure_times++;
        }
        else {
            LOG(INFO) << " Measure complete!!! times:"<<measure_times;
            break;
        }
    }
    //根据结果执行上传任务
    code=terminor->post_measure_information();
    //更新终端状态为Ready状态
    terminor->m_terminor_statu=TERMINOR_READY;

    if (code != SUCCESS) {
        LOG(ERROR) << "Command measure 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;
    //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;
}

Error_manager Terminor_Command_Executor::scanning_measuring()
{
    //计时起点
    auto t_start_point=std::chrono::system_clock::now();

    //准备目录
    time_t tt;
    time( &tt );
    tt = tt + 8*3600;  // transform the time zone
    tm* t= gmtime( &tt );
    char path[255]={0};
    sprintf(path,"%s/%4d/%02d/%02d",m_save_root_path.c_str(),
            t->tm_year + 1900,
            t->tm_mon + 1,
            t->tm_mday);
    PathCreator path_creator;
    path_creator.CreateDatePath(path);
    std::string project_path=path_creator.GetCurPath();

    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;

    std::vector<Laser_base*> tp_lasers;
    for(int i=0;i<m_terminor_parameter.laser_parameter_size();++i)
    {
        int laser_id=m_terminor_parameter.laser_parameter(i).id();
        int frame_count=m_terminor_parameter.laser_parameter(i).frame_count();
        if(frame_count<=0)
        {
            LOG(WARNING)<<"terminal parameter laser["<<laser_id<<"] frame count is <0";
            continue;
        }
        if(laser_id>=0&&laser_id<mp_laser_vector.size())
        {
            //判断该id的雷达是否已经添加进去, 放置配置中出现重复的雷达编号
            bool tb_repeat=false;
            for(int j=0;j<tp_lasers.size();++j)
            {
                if(tp_lasers[j]==mp_laser_vector[laser_id])
                {
                    tb_repeat=true;
                    break;
                }
            }
            if(tb_repeat==false)
            {
                tp_lasers.push_back(mp_laser_vector[laser_id]);
                //创建扫描任务,
                Laser_task* laser_task=new Laser_task();
                //
                laser_task->task_init(TASK_CREATED,scan_cloud,&cloud_lock);
                laser_task->set_task_frame_maxnum(frame_count);
                laser_task->set_task_save_path(project_path);
                laser_task_vector.push_back(laser_task);
                //发送任务单给雷达
                code=tp_lasers[i]->execute_task(laser_task);
                if(code!=SUCCESS)
                {
                    return code;
                }
            }
        }
    }
    if(tp_lasers.size()==0)
    {
        return Error_manager(TERMINOR_NOT_CONTAINS_LASER,NORMAL,"terminal not contains laser");
    }
    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,MAJOR_ERROR,"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);
    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;
}

cv::RotatedRect Terminor_Command_Executor::create_rotate_rect(float length,float width,float angle,float cx,float cy)
{
    const double C_PI=3.14159265;
    float theta=C_PI*(angle/180.0);
    float a00=cos(theta);
    float a01=-sin(theta);
    float a10=sin(theta);
    float a11=cos(theta);
    cv::Point2f point[4];
    point[0].x=-length/2.0;
    point[0].y=-width/2.0;

    point[1].x=-length/2.0;
    point[1].y=width/2.0;

    point[2].x=length/2.0;
    point[2].y=-width/2.0;

    point[3].x=length/2.0;
    point[3].y=width/2.0;

    std::vector<cv::Point2f> point_vec;
    for(int i=0;i<4;++i)
    {
        float x=point[i].x*a00+point[i].y*a01+cx;
        float y=point[i].x*a10+point[i].y*a11+cy;
        point_vec.push_back(cv::Point2f(x,y));
    }
    return cv::minAreaRect(point_vec);
}