// // Created by zx on 2019/12/9. // #include "region_worker.h" //#include "plc_data.h" // 测量结果滤波,不影响现有结构 #include "../tool/measure_filter.h" Region_worker::Region_worker() { m_region_worker_status = E_UNKNOWN; mp_cloud_collection_mutex = NULL; mp_cloud_collection = NULL; m_detect_updata_time = std::chrono::system_clock::now() - std::chrono::hours(1); mp_auto_detect_thread = NULL; } /** * 析构 * */ Region_worker::~Region_worker() { } Error_manager Region_worker::init(Point2D_tool::Point2D_box point2D_box, std::mutex* p_cloud_collection_mutex, pcl::PointCloud::Ptr p_cloud_collection) { m_detector.init(point2D_box); mp_cloud_collection_mutex = p_cloud_collection_mutex; mp_cloud_collection = p_cloud_collection; m_auto_detect_condition.reset(false, false, false); mp_auto_detect_thread = new std::thread(&Region_worker::auto_detect_thread_fun, this); m_region_worker_status = E_READY; return Error_code::SUCCESS; } Error_manager Region_worker::init(Point2D_tool::Point2D_box point2D_box, std::mutex* p_cloud_collection_mutex, pcl::PointCloud::Ptr p_cloud_collection, int index) { m_index = index; return init(point2D_box, p_cloud_collection_mutex, p_cloud_collection); } //开始自动预测的算法线程 Error_manager Region_worker::start_auto_detect() { //唤醒一次 m_auto_detect_condition.notify_all(false, true); return Error_code::SUCCESS; } //关闭自动预测的算法线程 Error_manager Region_worker::stop_auto_detect() { //唤醒一次 m_auto_detect_condition.notify_all(false); return Error_code::SUCCESS; } // 获得中心点、角度等测量数据 Error_manager Region_worker::detect_wheel_result(std::mutex* p_cloud_mutex, pcl::PointCloud::Ptr p_cloud_in, Common_data::Car_wheel_information& car_wheel_information) { return m_detector.detect(p_cloud_mutex, p_cloud_in, car_wheel_information); } // 获得中心点、角度等测量数据, 新算法, 可以测量前轮旋转 Error_manager Region_worker::detect_wheel_result_ex(std::mutex* p_cloud_mutex, pcl::PointCloud::Ptr p_cloud_in, Common_data::Car_wheel_information& car_wheel_information) { return m_detector.detect_ex(p_cloud_mutex, p_cloud_in, car_wheel_information); } //外部调用获取新的车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会等待车轮定位信息刷新, 直到超时, (内置while) Error_manager Region_worker::get_new_wheel_information_and_wait(Common_data::Car_wheel_information* p_car_wheel_information, std::chrono::system_clock::time_point command_time, int timeout_ms) { if ( p_car_wheel_information == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } //判断是否超时 while (std::chrono::system_clock::now() - command_time < std::chrono::milliseconds(timeout_ms)) { //获取指令时间之后的信息, 如果没有就会循环 if(m_detect_updata_time > command_time ) { *p_car_wheel_information = m_car_wheel_information; return Error_code::SUCCESS; } //else 等待下一次数据更新 //等1ms std::this_thread::sleep_for(std::chrono::milliseconds(1)); } //超时退出就报错 return Error_manager(Error_code::WJ_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR, " Region_worker::get_new_wheel_information_and_wait error "); } //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待 Error_manager Region_worker::get_current_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information, std::chrono::system_clock::time_point command_time) { if ( p_car_wheel_information == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } //获取指令时间之后的信息, 如果没有就会报错, 不会等待 if( m_detect_updata_time > command_time ) { *p_car_wheel_information = m_car_wheel_information; return Error_code::SUCCESS; } else { return Error_manager(Error_code::WJ_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR, " Region_worker::get_current_wheel_information error "); } } //外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待 Error_manager Region_worker::get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information, std::chrono::system_clock::time_point command_time) { if ( p_car_wheel_information == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } //将指令时间提前一个扫描周期, 然后获取这个时间后面的点云. //就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云. if( m_detect_updata_time > command_time - std::chrono::milliseconds(REGION_WORKER_DETECT_CYCLE_MS*2) ) { *p_car_wheel_information = m_car_wheel_information; return Error_code::SUCCESS; } else { return Error_manager(Error_code::WJ_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR, " Region_worker::get_last_wheel_information error "); } } Region_worker::Region_worker_status Region_worker::get_status() { return m_region_worker_status; } std::chrono::system_clock::time_point Region_worker::get_detect_updata_time() { return m_detect_updata_time; } 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); } //自动预测的线程函数 void Region_worker::auto_detect_thread_fun() { LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() start "<< this; Error_manager t_error; Error_manager t_result; while (m_auto_detect_condition.is_alive()) { //暂时固定为一个扫描周期, 就循环一次 //后期可以根据每个小雷达的刷新情况, 实时更改总点云..... // if(m_index==5) // LOG(INFO) << "before wait"; m_auto_detect_condition.wait(); // if(m_index==5) // LOG(INFO) << "after wait"; if ( m_auto_detect_condition.is_alive() ) { // std::cout << " huli test :::: " << " = " << "---------------------------------------------------------------------------------" << std::endl; // if(m_index==5) // LOG(INFO) << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size(); // // auto start = std::chrono::system_clock::now(); t_error = detect_wheel_result_ex(mp_cloud_collection_mutex, mp_cloud_collection, m_car_wheel_information); // auto end = std::chrono::system_clock::now(); // auto duration = std::chrono::duration_cast(end - start); // std::cout << "花费了" // << double(duration.count()*1000) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den << "毫秒" << std::endl; if ( t_error == SUCCESS ) { std::unique_lock t_lock(*mp_cloud_collection_mutex); m_car_wheel_information.correctness = true; // if(m_index==5) { // std::cout << " huli test :::: " << " x = " << m_car_wheel_information.center_x << std::endl; // std::cout << " huli test :::: " << " y = " << m_car_wheel_information.center_y << std::endl; // std::cout << " huli test :::: " << " c = " << m_car_wheel_information.car_angle << std::endl; // std::cout << " huli test :::: " << " wheelbase = " << m_car_wheel_information.wheel_base // << std::endl; // std::cout << " huli test :::: " << " width = " << m_car_wheel_information.wheel_width << std::endl; // std::cout << " huli test :::: " << " front_theta = " << m_car_wheel_information.front_theta // << std::endl; // std::cout << " huli test :::: " << " correctness = " << m_car_wheel_information.correctness // << std::endl; // } } else { std::unique_lock t_lock(*mp_cloud_collection_mutex); m_car_wheel_information.correctness = false; // if(m_index==5) // LOG(WARNING)<< " t_error = " << t_error; } //无论结果如何,都要刷新时间, 表示这次定位已经执行了. m_detect_updata_time = std::chrono::system_clock::now(); // if(m_index==5) // LOG(INFO) << "before filter"; // 测量正确,更新到滤波器 if(m_car_wheel_information.correctness) { Common_data::Car_wheel_information_stamped t_wheel_info_stamped; t_wheel_info_stamped.wheel_data = m_car_wheel_information; t_wheel_info_stamped.measure_time = m_detect_updata_time; Measure_filter::get_instance_references().update_data(m_index, t_wheel_info_stamped); } // if(m_index==5) // LOG(INFO) << "after filter"; } } LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this; return; }