Measure/Modules/MeasureNode/wanji_lidar/region_worker.h

//
// Created by zx on 2019/12/9.
//

#ifndef REGION_WORKER_H
#define REGION_WORKER_H

#include "region_detect.h"
//#include "../tool/StdCondition.h"
#include <thread>
#include <mutex>
#include <iostream>
#include <atomic>
#include <chrono>
#include "error_code/error_code.hpp"
#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<pcl::PointXYZ>::Ptr p_cloud_collection, int index);

	// 公有点云更新函数，传入最新点云获得结果
	Error_manager update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

	//开始自动预测的算法线程
	Error_manager start_auto_detect();
	//关闭自动预测的算法线程
	Error_manager stop_auto_detect();

	// 获得中心点、角度等测量数据
	Error_manager detect_wheel_result(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
									  Common_data::Car_wheel_information& car_wheel_information);

	// 获得中心点、角度等测量数据,  新算法, 可以测量前轮旋转
	Error_manager detect_wheel_result_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr cloud, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0);

   // 超界判断，增加对后轮的判断，对轴距、车宽、角度超界的判断
   int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr m_cloud;             // 自动区域检测用点云
//    std::atomic<bool> 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<int> 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