// // 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::Ptr(new pcl::PointCloud); 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 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::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width) { Error_manager result; std::lock_guard 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::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width,double& front_theta) { Error_manager result; std::lock_guard 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::Ptr cloud_in) { if(cloud_in->size() <=0){ return; } // LOG(INFO) << "worker update cloud"; std::lock_guard 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 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: "<(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 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); }