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

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,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);
        }
    }

    //第二步,雷达,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;
        }
    }

    // 检查当前终端指令执行器的状态是否空闲;如果空闲,进入测量流程,设置忙碌状态
    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;

    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;
        }
    }
    //更新终端状态为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!!!";
    }
}

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();

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