CT_project/测量节点/velodyne_lidar/ground_region.h

//
// Created by zx on 2021/5/20.
//

#ifndef LIDARMEASURE__GROUD_REGION_HPP_
#define LIDARMEASURE__GROUD_REGION_HPP_

#include <thread>

#include "../tool/thread_condition.h"
#include "../tool/common_data.h"
#include "../error_code/error_code.h"
#include "match3d/detect_wheel_ceres3d.h"
#include "velodyne_config.pb.h"
#include "../message/measure_message.pb.h"
#include "../verify/Verify_result.h"
#include "car_pose_detector.h"
#include "chassis_ceres_solver.h"

#include <opencv2/opencv.hpp>
#include <glog/logging.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/approximate_voxel_grid.h>


//huli add fangdou
#define ANTI_SHAKE  1

class Ground_region
{
public:
   enum Region_cloud_type
   {
      total = 0,
      left_front = 1,
      left_back = 2,
      right_front = 3,
      right_back = 4,
      filtered = 5,
      detect_z=6,
   };
   enum Ground_region_status
   {
      E_UNKNOWN = 0, //未知
      E_READY = 1,   //准备，待机
      E_BUSY = 2,    //工作正忙

      E_FAULT = 10, //故障
   };
   enum Range_status
   {
      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
   };

#define GROUND_REGION_DETECT_CYCLE_MS 150

   Ground_region();
   ~Ground_region();
   // 区域类初始化
   Error_manager init(velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model, pcl::PointCloud<pcl::PointXYZ>::Ptr right_model);
   // 公有检测函数,根据初始化设置的区域进行裁剪，并识别获得结果
   Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &result);
   // 公有点云更新函数，传入最新点云获得结果
   Error_manager update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
   //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
	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, Region_cloud_type type)
   {
      out_cloud->clear();
      if(m_detector==nullptr)
         return;
      switch (type)
      {
      case Region_cloud_type::total:
         m_cloud_collection_mutex.lock();
         out_cloud->operator+=(*mp_cloud_collection);
         m_cloud_collection_mutex.unlock();
         // LOG(WARNING) << "region cloud size: " << mp_cloud_collection->size();
         break;
      case Region_cloud_type::left_front:
         out_cloud->operator+=(m_detector->m_left_front_cloud);
         break;
      case Region_cloud_type::left_back:
         out_cloud->operator+=(m_detector->m_left_rear_cloud);
         break;
      case Region_cloud_type::right_front:
         out_cloud->operator+=(m_detector->m_right_front_cloud);
         break;
      case Region_cloud_type::right_back:
         out_cloud->operator+=(m_detector->m_right_rear_cloud);
         break;
      case Region_cloud_type::filtered:
         m_filtered_cloud_mutex.lock();
         out_cloud->operator+=(*mp_cloud_filtered);
         m_filtered_cloud_mutex.unlock();
         break;
      case Region_cloud_type::detect_z:
         m_detect_z_cloud_mutex.lock();
         out_cloud->operator+=(*mp_cloud_detect_z);
         // std::cout<<"get cloud: "<<out_cloud->size()<<std::endl;
         m_detect_z_cloud_mutex.unlock();
         break;
      }
   }
   // 获取区域终端号, 注意！未初始化调用将返回-1
   int get_terminal_id()
   {
      if(m_region_status == E_UNKNOWN)
         return -1;
      else
         return m_region.region_id();
   }
   // 获取区域状态
   Ground_region_status get_status() { return m_region_status; }
   // 获取更新时间
   std::chrono::system_clock::time_point get_detect_update_time() { return m_detect_update_time; }
   // 获取配置参数
   velodyne::Region get_param() { return m_region; }
   // 检查激光是否在区域中
   bool check_lidar_inside(int lidar_id)
   {
      for (size_t i = 0; i < m_region.lidar_exts_size(); i++)
      {
         if(m_region.lidar_exts(i).lidar_id() == lidar_id)
            return true;
      }
      return false;
   }

private:
   // 点云分类，z切，3d优化
   bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                              detect_wheel_ceres3d::Detect_result &result);
   // 自动检测线程函数,频率由外部更新点云频率控制
   void thread_measure_func();

   std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);

   // double distance(cv::Point2f p1, cv::Point2f p2);

   bool isRect(std::vector<cv::Point2f> &points);

   // 超界判断，旋转单位(deg)，默认无旋转
   // 主要判断左右超界
   int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double cx, double cy, 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);

   int outOfTerminalRangeDetection(Common_data::Car_wheel_information &measure_result, const velodyne::OutOfRegion &outOfRegion);

private:


   velodyne::Region m_region; // 区域配置
   detect_wheel_ceres3d *m_detector; // 检测器
   Ground_region_status m_region_status; // 区域状态

   // 自动检测线程
   std::thread *m_measure_thread;
   Thread_condition m_measure_condition;

   std::mutex	 								      m_cloud_collection_mutex;       // 点云更新互斥锁, 内存由上级管理
	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由上级管理
   std::chrono::system_clock::time_point     m_cloud_collection_time; // 点云更新时间

   std::mutex	 								      m_filtered_cloud_mutex;       // 点云更新互斥锁, 内存由上级管理
   pcl::PointCloud<pcl::PointXYZ>::Ptr       mp_cloud_filtered;      // 区域切除后的点

   std::mutex	 								      m_detect_z_cloud_mutex;       // 点云更新互斥锁, 内存由上级管理
   pcl::PointCloud<pcl::PointXYZ>::Ptr       mp_cloud_detect_z;      // 底盘切除后的点

   Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
	std::chrono::system_clock::time_point		m_detect_update_time;			//定位结果的更新时间.
   std::mutex m_car_result_mutex; // 测量结果互斥锁

#ifdef ANTI_SHAKE
    int                m_range_status_last;
#endif //ANTI_SHAKE
};

#endif //LIDARMEASURE__GROUD_REGION_HPP_