LidarMeasure/wj_lidar/region_worker.cpp

//
// Created by zx on 2019/12/9.
//
#include "region_worker.h"
#include "plc_data.h"

/**
 * 有参构造
 * */
Region_worker::Region_worker(int id, wj::Region region, Verify_result* verify_handle):
m_read_code_count(0),
mb_cloud_updated(0),
mp_verify_handle(0)
{
    if(verify_handle!= nullptr)
    {
        mp_verify_handle = verify_handle;
    }
    m_update_plc_time = std::chrono::steady_clock::now();
    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    m_detector = new Region_detector(id, region);
    m_cond_exit.Notify(false);
    m_detect_thread = new std::thread(detect_loop, this);
    m_detect_thread->detach();
}

/**
 * 析构
 * */
Region_worker::~Region_worker()
{
    m_cond_exit.Notify(true);
    std::lock_guard<std::mutex> lck(m_mutex);
    if (m_detect_thread)
    {
        if (m_detect_thread->joinable())
            m_detect_thread->join();
        delete m_detect_thread;
        m_detect_thread = 0;
    }
    if (m_detector)
    {
        delete m_detector;
        m_detector = 0;
    }
}

/**
 * 获取区域id号
 * */
int Region_worker::get_id()
{
    if (m_detector)
    {
        return m_detector->get_region_id();
    }
    else
    {
        LOG(ERROR) << "failed to get worker id";
        return -1;
    }
}

/**
 * 获取轮距等测量结果
 * */
Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width)
{
    Error_manager result;
    std::lock_guard<std::mutex> lck(m_mutex);
    if (m_detector)
    {
        LOG(INFO) << "worker getting result";
        result = m_detector->detect(cloud_in, x, y, c, wheelbase, width);
    }
    else
    {
        result = Error_manager(Error_code::POINTER_IS_NULL);
    }
    return result;
}


/**
 * ceres结果
 * 获取轮距等测量结果,包括前轮旋转
 * */
Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
        double &wheelbase, double &width,double& front_theta)
{
    Error_manager result;
    std::lock_guard<std::mutex> lck(m_mutex);
    if (m_detector)
    {
        // LOG(INFO) << "worker getting result";
		result = m_detector->detect(cloud_in, x, y, c, wheelbase, width, front_theta, false);
		//if(result != SUCCESS)
			// result = m_detector->detect(cloud_in, x, y, c, wheelbase, width);
    }
    else
    {
        result = Error_manager(Error_code::POINTER_IS_NULL);
    }
    return result;
}

/**
 * 外部调用更新区域检测用点云
 * */
void Region_worker::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
    if(cloud_in->size() <=0){
        return;
    }
    // LOG(INFO) << "worker update cloud";
    std::lock_guard<std::mutex> lck(m_mutex);
    m_cloud->clear();
    m_cloud->operator+=(*cloud_in);
    // for (int i = 0; i < cloud_in->size(); ++i)
    // {
    //     m_cloud->push_back(cloud_in->points[i]);
    // }
    mb_cloud_updated = true;
}



/**
 * 实时检测与更新线程函数
 * */
void Region_worker::detect_loop(Region_worker *worker)
{
    // 1.参数检查
    if (worker == 0)
        return;
    if (worker->m_detector == 0)
        return;
    // 2.检测与更新循环
    while (!worker->m_cond_exit.WaitFor(200))
    {
//        LOG(INFO) << "worker detect loop";
        // 检查当前状态
        if (worker->mb_cloud_updated)
        {
            std::lock_guard<std::mutex> lck(worker->m_mutex);
            int code = REGION_WORKER_RESULT_DEFAULT;
            int border_status = REGION_WORKER_RESULT_DEFAULT;
            worker->mb_cloud_updated = false;
            double x,y,angle,wheelbase,width,front_theta;
            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, wheelbase, width,front_theta, false);

            if(worker->mp_verify_handle == nullptr)
            {
                LOG(WARNING) << "verify handle null pointer";
                continue;
            }
//            LOG(INFO) << "worker detect loop end detect";
            if (result == WJ_REGION_EMPTY_CLOUD)
            {
                code = REGION_WORKER_EMPTY_SPACE;
            }
            else if (result == SUCCESS)
            {
                cv::RotatedRect car_border;
                float ext_length=720;
                float new_length=wheelbase*1000.0+2.0*ext_length;
                float new_width=2650;
                car_border=create_rotate_rect(new_length,new_width,angle,1000.0*x,1000.0*y);

                int verify_return_code = 0;
                int terminal_id=worker->get_id();
                Error_manager verify_error_code = worker->mp_verify_handle->verify(car_border, terminal_id, verify_return_code);
//                Error_manager verify_error_code = SUCCESS;
                if(verify_error_code == SUCCESS) {
                    code = REGION_WORKER_HAS_CAR;
                }else{
                    code = REGION_WORKER_HAS_CAR;
                    border_status = verify_return_code;
                    //LOG(WARNING) << "region worker verify result: " << code;
                }
            }
            else
            {
                code = REGION_WORKER_DETECT_ERROR;
            }
            // LOG(INFO) << "worker detect loop 000";
            // 判断与上次读取是否相同，并计数
            if(worker->m_last_read_code != code)
            {
                worker->m_read_code_count = 0;
            }
            worker->m_last_read_code = code;
            worker->m_read_code_count += 1;

            /*if(worker->get_id() == 3) {
                LOG(WARNING) << "worker id: " << worker->get_id()<<" code: "<<code;
            }*/
            // 写入plc，加入写入频率限制
            int duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - worker->m_update_plc_time).count();
            Plc_data *p = Plc_data::get_instance();
            // 写入间隔必须超过500ms，当前状态不同于上次写入状态，且该状态已连续读到三次
            if (p!=0 && duration > 500 && ((worker->m_last_sent_code != worker->m_last_read_code) || worker->m_last_border_status != border_status) && worker->m_read_code_count > 3)
            {
                worker->m_last_sent_code = worker->m_last_read_code;
		        worker->m_last_border_status = border_status;
                p->update_data(code, border_status, worker->m_detector->get_region_id());
                worker->m_update_plc_time = std::chrono::steady_clock::now();
            }
        }
    }
}



cv::RotatedRect Region_worker::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);
}