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"

std::mutex Terminor_Command_Executor::ms_mutex_scan_measure;

//////以下两个函数用于测试,读取指定点云
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;
}

// !!!!!!!!!!!! 临时使用，保存点云debug
void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
{
    std::ofstream os;
    os.open(txt, std::ios::out);
    for (int i = 0; i < pCloud->points.size(); i++)
    {
        pcl::PointXYZ point = pCloud->points[i];
        char buf[255];
        memset(buf, 0, 255);
        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
        os.write(buf, strlen(buf));
    }
    os.close();
}

Error_manager Terminor_Command_Executor::scanning_measuring() {
    std::lock_guard<std::mutex> lock(ms_mutex_scan_measure);
    Error_manager code;
    LOG(INFO) << ">>>>>>>>>>>> Enter terminator ID:" << m_terminor_parameter.id();
    //计时起点
    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();


    //第一步,启动雷达扫描点云,切换当前状态为扫描中
    //根据配置筛选雷达
    pcl::PointCloud<pcl::PointXYZ> scan_cloud;
    std::mutex cloud_lock;
    std::vector<Laser_task *> 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_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.makeShared());
    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();
    //added by yct, 获取车轮点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_combined_wheel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    t_combined_wheel_cloud = locate_task->get_locate_cloud();
    //释放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) {
        delete wj_task;
        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) {
        delete wj_task;
        return code;
    }
    ///万集测量正确,对比两数据
    wj_measure_result wj_result;
    code = wj_task->get_result(wj_result);
    //added by yct, wj wheel cloud
    pcl::PointCloud<pcl::PointXYZ> t_wj_wheel = wj_task->get_cloud();
    for (int m = 0; m < t_wj_wheel.size(); ++m) {
        t_wj_wheel[m].x *= 1000.0;
        t_wj_wheel[m].y *= 1000.0;
        t_wj_wheel[m].z *= 1000.0;
    }
    t_combined_wheel_cloud->operator+=(t_wj_wheel);
    if (code != SUCCESS) {
        delete wj_task;
        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_wheel_base - wj_locate_information.locate_wheel_base);
    if (offset_x > 100 || offset_y > 150 || offset_angle > 2.5 || offset_wheel_base > 350 || offset_width > 250) {
        LOG(WARNING) << "chekc failed. (offset x>100, y>150, angle>2.5, wheel_base>130, width>250): " << offset_x
                     << " " << offset_y << " " << offset_angle << " " << offset_wheel_base << " " << offset_width;
        return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
    }
    ///根据sigmod函数计算角度权重
//    double angle_weight=1.0-1.0/(1.0+exp(-offset_angle/1.8));
    // added by yct, 角度偏差越大，越倾向于万集，之后校验结果
    double angle_weight=1.0/(1.0+exp(-offset_angle/0.5));
    ///根据两个结果选择最优结果,中心点选择万集雷达,角度选择两值加权,轴距已以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 =angle_weight * wj_locate_information.locate_theta +
            (1.0-angle_weight) * 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 = 0.01 * dj_locate_information.locate_wheel_base +
                                              0.99 * wj_locate_information.locate_wheel_base;
//    m_measure_information.locate_wheel_base += 400;
    //m_measure_information.locate_wheel_base=m_measure_information.locate_wheel_base>3000?3000:m_measure_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;
    // 20210527 added by yct
    // 6号位标定地面偏低，高度增加21mm
    // 20210603 2号位加高10mm, 4号位加高5mm
    // 20210925 6号位加高改为13
    // 20220621 2号位轴距-10, 4号位y+1mm,轴距+20mm, 6号位y+1mm
    if(m_terminor_parameter.id()==5)
    {
        m_measure_information.locate_hight += 13;
        m_measure_information.locate_y += 1;
    }
    else if(m_terminor_parameter.id()==3)
    {
        m_measure_information.locate_hight += 5;
        m_measure_information.locate_y +=1;
        m_measure_information.locate_wheel_base += 20;
    }
    else if(m_terminor_parameter.id()==1)
    {
        m_measure_information.locate_hight += 10;
        m_measure_information.locate_wheel_base -= 10;
    }
    m_measure_information.locate_correct = true;

    // 20210527 added by yct, 融合大疆轮子与万集点云进行结果校验,机械手内空2150，取2000计算
    save_cloud_txt(project_path+"/comb.txt", t_combined_wheel_cloud);
    Eigen::Vector3d line_param_left, line_param_right;
    double sign = 1.0;
    if(m_measure_information.locate_theta==90.0)
    {
        line_param_left << 1.0, 0, -m_measure_information.locate_x + 1000.0;
        line_param_right << 1.0, 0, -m_measure_information.locate_x - 1000.0;
    }else{
        // 减去2度测试外点个数
        double k = (m_measure_information.locate_theta) * M_PI / 180.0;
        line_param_left << tan(k), -1, m_measure_information.locate_y - (m_measure_information.locate_x - 1000.0/sin(k))*tan(k);
        line_param_right << tan(k), -1, m_measure_information.locate_y - (m_measure_information.locate_x + 1000.0/sin(k))*tan(k);
        if(tan(k)<0)
        {
            sign = -sign;
        }
    }
    int outside_point_count = 0;
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_outside_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int l = 0; l < t_combined_wheel_cloud->size(); ++l) {
        Eigen::Vector3d t_point(t_combined_wheel_cloud->points[l].x,t_combined_wheel_cloud->points[l].y,1.0);
        double t_left_value = sign * (line_param_left.transpose() * t_point)[0];
        double t_right_value = sign * (line_param_right.transpose() * t_point)[0];
        // 点在线右侧为正，左侧为负
        if(t_left_value <= 0 || t_right_value >= 0)
        {
            outside_point_count++;
            t_outside_cloud->push_back(pcl::PointXYZ(t_combined_wheel_cloud->points[l].x, t_combined_wheel_cloud->points[l].y, t_combined_wheel_cloud->points[l].z));
        }
    }
    save_cloud_txt(project_path+"/outside.txt", t_outside_cloud);
    LOG(INFO) << "outside point count: "<<outside_point_count;
    // 安全判断，外点超过15个判断本次测量失败
    if((outside_point_count > 15 && m_terminor_parameter.id()!=1) || (m_terminor_parameter.id()==1 && outside_point_count > 25))
    {
        LOG(WARNING) << "机械手下降判断外点过多，存在风险: "<<outside_point_count;
        m_measure_information.locate_correct = false;
        code = Error_code::ERROR;
    }

    ////如果测量正确,检验结果
    if (mp_verify_tool != NULL && code==SUCCESS) {

        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);
        delete wj_task;
        return code;

    } else {
        delete wj_task;
    }

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