// // Created by zx on 2019/12/9. // #ifndef REGION_WORKER_H #define REGION_WORKER_H #include "region_detect.h" //#include "../tool/StdCondition.h" #include #include #include #include #include #include "../error_code/error_code.h" #include "../verify/Verify_result.h" #include "../tool/thread_condition.h" #include "../tool/common_data.h" /** * 区域功能类，负责自动检测区域状态并更新到plc * */ class Region_worker { public: #define REGION_WORKER_RESULT_DEFAULT 0x0000 //#define REGION_WORKER_VERIFY_OK 0x0001 //#define REGION_WORKER_EMPTY_SPACE 0x0020 //#define REGION_WORKER_CLUSTER_SIZE_ERROR 0x0040 //#define REGION_WORKER_OTHER_ERROR 0x0080 //#define REGION_WORKER_NULL_POINTER 0x0100 #define REGION_WORKER_EMPTY_SPACE 1 #define REGION_WORKER_HAS_CAR 2 #define REGION_WORKER_DETECT_ERROR 3 //区域功能类 的预测算法的时间周期, 一般为100ms, 我们这里设一个较大值200ms #define REGION_WORKER_DETECT_CYCLE_MS 200 //万集区域功能的状态 enum Region_worker_status { E_UNKNOWN = 0, //未知 E_READY = 1, //准备，待机 E_BUSY = 2, //工作正忙 E_FAULT = 10, //故障 }; // 超界情况 enum Range_status_wj { Range_correct = 0x0000, // 未超界 Range_front = 0x0001, //前超界 Range_back = 0x0002, //后超界 Range_left = 0x0004, // 左超界 Range_right = 0x0008, // 右超界 Range_bottom = 0x0010, //底盘超界 Range_top = 0x0020, // 车顶超界 Range_car_width = 0x0040, // 车宽超界 Range_car_wheelbase = 0x0080, // 轴距超界 Range_angle_anti_clock = 0x0100, // 左(逆时针)旋转超界 Range_angle_clock = 0x0200, // 右(顺时针)旋转超界 Range_steering_wheel_nozero = 0x0400, // 方向盘未回正 Range_car_moving = 0x0800, // 车辆移动为1，静止为0 }; public: Region_worker(); ~Region_worker(); Error_manager init(wj::Region region); // // 初始化传入编号,仅普爱使用 // Error_manager init(wj::Region region, std::mutex* p_cloud_collection_mutex, // pcl::PointCloud::Ptr p_cloud_collection, int index); // 公有点云更新函数，传入最新点云获得结果 Error_manager update_cloud(pcl::PointCloud::Ptr cloud); //开始自动预测的算法线程 Error_manager start_auto_detect(); //关闭自动预测的算法线程 Error_manager stop_auto_detect(); // 获得中心点、角度等测量数据 Error_manager detect_wheel_result(std::mutex* p_cloud_mutex, pcl::PointCloud::Ptr cloud_in, Common_data::Car_wheel_information& car_wheel_information); // 获得中心点、角度等测量数据, 新算法, 可以测量前轮旋转 Error_manager detect_wheel_result_ex(std::mutex* p_cloud_mutex, pcl::PointCloud::Ptr cloud_in, Common_data::Car_wheel_information& car_wheel_information); //外部调用获取新的车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会等待车轮定位信息刷新, 直到超时, (内置while) Error_manager 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=1500); //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待 Error_manager get_current_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information, std::chrono::system_clock::time_point command_time); //外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待 Error_manager get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information, std::chrono::system_clock::time_point command_time); void get_region_cloud(pcl::PointCloud::Ptr &out_cloud) { out_cloud->clear(); if(mp_cloud_collection_mutex == nullptr) { return; } mp_cloud_collection_mutex->lock(); out_cloud->operator+=(*mp_cloud_collection); mp_cloud_collection_mutex->unlock(); } // 获取区域终端号, 注意！未初始化调用将返回-1 int get_terminal_id() { if(m_region_worker_status == E_UNKNOWN) return -1; else return m_region_param.region_id(); } // 获取区域状态 Region_worker_status get_status() { return m_region_worker_status; } // 获取更新时间 std::chrono::system_clock::time_point get_detect_update_time() { return m_detect_update_time; } // 获取配置参数 wj::Region get_param() { return m_region_param; } // 检查激光是否在区域中 bool check_lidar_inside(int lidar_id) { for (size_t i = 0; i < m_region_param.lidar_exts_size(); i++) { if(m_region_param.lidar_exts(i).lidar_id() == lidar_id) return true; } return false; } public: std::chrono::system_clock::time_point get_detect_updata_time(); protected: static cv::RotatedRect create_rotate_rect(float length,float width,float angle,float x,float y); //自动预测的线程函数 void auto_detect_thread_fun(); // 超界判断，旋转单位(deg)，默认无旋转 // 主要判断左右超界 int outOfRangeDetection(pcl::PointCloud::Ptr cloud, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0); // 超界判断，增加对后轮的判断，对轴距、车宽、角度超界的判断 int outOfRangeDetection(pcl::PointCloud::Ptr cloud, Common_data::Car_wheel_information measure_result, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0); protected: // int m_index;// 区域编号,仅普爱使用 wj::Region m_region_param; //状态 Region_worker_status m_region_worker_status; //万集区域功能的状态 Region_detector* mp_detector; // 区域检测算法实例 //万集雷达的自动功能, 定时收集所有点云, 然后通知detect去计算车轮信息. std::mutex* mp_cloud_collection_mutex; // 点云更新互斥锁, 内存由上级管理 pcl::PointCloud::Ptr mp_cloud_collection; //扫描点的点云集合, 内存由上级管理 Common_data::Car_wheel_information m_car_wheel_information; //车轮的定位结果, std::chrono::system_clock::time_point m_detect_update_time; //定位结果的更新时间. std::thread* mp_auto_detect_thread; //自动预测的线程指针，内存由本类管理 Thread_condition m_auto_detect_condition; //自动预测的条件变量 // pcl::PointCloud::Ptr m_cloud; // 自动区域检测用点云 // std::atomic mb_cloud_updated; // 点云更新指标 //// //// std::chrono::system_clock::time_point m_update_plc_time; // 更新plc状态时刻 //// int m_last_sent_code; // 上次写入plc的状态值 //// int m_last_read_code; // 上次检查获得的状态值 //// int m_last_border_status; // 上次超界提示 //// std::atomic m_read_code_count; // 检查后重复获取相同状态值次数 // // Thread_condition m_cond_exit; // 系统退出标志 // std::thread *m_detect_thread; // 实时检测线程 // std::mutex m_mutex; // 点云互斥锁 // // Verify_result* mp_verify_handle; // 边缘检测 }; #endif //REGION_WORKER_H