LidarMeasure/wj_lidar/region_worker.cpp

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

#include "../msg/message_base.pb.h"
#include "../msg/ground_measure_msg.pb.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();

    char port_buf[10]={0};
    sprintf(port_buf, "%02d", id);
    std::string pub_str = std::string(WJ_RESULT_PUB_STR)+port_buf;
    LOG(WARNING) <<"conn: "<<pub_str;
    m_sock.bind(pub_str);

}

/**
 * 析构
 * */
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);
		// 增加滤波后结果，赋值传出
        Common_data::Car_wheel_information t_wheel_info;
        result = Measure_filter::get_instance_references().get_filtered_wheel_information(get_id(), t_wheel_info);
        if(result ==SUCCESS) {
            char buf[512] = {0};
            sprintf(buf,
                    "\nx %.3f->%.3f\n"
                    "y %.3f->%.3f\n"
                    "c %.3f->%.3f\n"
                    "wheelbase %.3f->%.3f\n"
                    "width %.3f->%.3f\n"
                    "front %.3f->%.3f\n",
                    x, t_wheel_info.center_x, y, t_wheel_info.center_y, c, t_wheel_info.car_angle, wheelbase,
                    t_wheel_info.wheel_base, width, t_wheel_info.wheel_width, front_theta, t_wheel_info.front_theta
            );
            LOG(INFO) << buf;
            x = t_wheel_info.center_x;
            y = t_wheel_info.center_y;
            c = t_wheel_info.car_angle;
            wheelbase = t_wheel_info.wheel_base;
            width = t_wheel_info.wheel_width;
            front_theta = t_wheel_info.front_theta;
        }else{
            LOG(WARNING) << result.to_string();
        }
    }
    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(50))
    {
//        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;
            std::string t_error_info_str="";
            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, front_theta, wheelbase, width, t_error_info_str, false);
//            LOG_IF(WARNING, worker->get_id() == 3) << "worker id: " << worker->get_id()<<" front: "<<front_theta <<", "<<t_error_info_str;
            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;
                }
                // added by yct, check car width, width轮距+22cm
                if(width > 1.92 || width < 1.55)
                {
//                    LOG(WARNING) << "region worker width: " << width;
                    border_status |= 0x000020;//第六位为车辆宽度异常
                }

                // 测量正确,更新到滤波器
                if(terminal_id>=0) {
                    Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
                    t_wheel_info_stamped.wheel_data.correctness = true;
                    t_wheel_info_stamped.wheel_data.center_x = x;
                    t_wheel_info_stamped.wheel_data.center_y = y;
                    t_wheel_info_stamped.wheel_data.car_angle = angle;
                    t_wheel_info_stamped.wheel_data.front_theta = front_theta;
                    t_wheel_info_stamped.wheel_data.wheel_width = width;
                    t_wheel_info_stamped.wheel_data.wheel_base = wheelbase;
                    t_wheel_info_stamped.measure_time = std::chrono::system_clock::now();;
                    Measure_filter::get_instance_references().update_data(terminal_id, t_wheel_info_stamped);
//                    if(terminal_id == 5)
//                        LOG(WARNING) <<terminal_id<<", "<<Measure_filter::get_instance_references().has_enough_data(terminal_id);
                }

            }
            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() == 1) {
////                LOG(WARNING) << "worker id: " << worker->get_id()<<" str: "<<t_error_info_str;
//                LOG(WARNING) << "worker id: " << worker->get_id()<<" code: "<<code<<", "<<border_status << ", x y: "<<x<< ", "<<y;
//            }

            // 写入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();
            if(p!=0) {
                p->update_error_info(worker->m_detector->get_region_id(), t_error_info_str);
            }
            // 写入间隔必须超过100ms，当前状态不同于上次写入状态，且该状态已连续读到三次
            if (p!=0 && duration > 100 && ((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();
            }

            // 20210528 added by yct, 增加万集测量结果发布到总线，用于与多线结果比对
            if(worker->get_id() == 5)
            {
                message::Ground_measure_statu_msg t_ground_result_msg;
                message::Base_info t_base_info;
                t_base_info.set_msg_type(message::Message_type::eGround_measure_statu_msg);
                t_base_info.set_timeout_ms(1000);
                t_base_info.set_sender(message::Communicator::eEmpty);
                t_base_info.set_receiver(message::Communicator::eEmpty);
                t_ground_result_msg.mutable_base_info()->CopyFrom(t_base_info);
                t_ground_result_msg.set_id(worker->get_id());

                message::Ground_statu t_ground_status;
                if(code == REGION_WORKER_EMPTY_SPACE)
                {
                    t_ground_status = message::Ground_statu::Nothing;
                }else if(code == REGION_WORKER_DETECT_ERROR)
                {
                    t_ground_status = message::Ground_statu::Noise;
                }else{
                    if(border_status == 0){
                        t_ground_status = message::Ground_statu::Car_correct;
                    }else if(border_status & 1 > 0) {
                        t_ground_status = message::Ground_statu::Car_top_out;
                    }else if(border_status & 2 > 0) {
                        t_ground_status = message::Ground_statu::Car_bottom_out;
                    }else if(border_status & 4 > 0) {
                        t_ground_status = message::Ground_statu::Car_left_out;
                    }else if(border_status & 8 > 0) {
                        t_ground_status = message::Ground_statu::Car_right_out;
                    }
                }
                t_ground_result_msg.set_ground_statu(t_ground_status);
                message::Locate_information t_loc_info;
                t_loc_info.set_locate_x(x);
                t_loc_info.set_locate_y(y);
                t_loc_info.set_locate_angle(angle);
                t_loc_info.set_locate_wheel_base(wheelbase);
                t_loc_info.set_locate_width(width);
                t_loc_info.set_locate_front_theta(front_theta);
                t_loc_info.set_locate_correct(true);
                t_ground_result_msg.mutable_locate_information_realtime()->CopyFrom(t_loc_info);
//                LOG(WARNING)<<"发送： "<<t_ground_result_msg.DebugString();
                worker->m_sock.send(t_ground_result_msg.SerializeAsString());
            }
        }
    }
}



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