Measure/Modules/MeasureNode/velodyne_lidar/ground_region.h

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

#pragma once

#include <thread>
#include <fcntl.h>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include "tool/point_tool.h"
#include "tool/measure_filter.h"
#include "tool/region_status_checker.h"
#include "tool/thread_condition.h"
#include "tool/common_data.h"
#include "tool/point3D_tool.h"
#include "tool/static_tool.hpp"
#include "tool/memory_box.hpp"
#include "system/system_communication_mq.h"
#include "error_code/error_code.hpp"
#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 "match3d/WheelFactor.h"

class Ground_region {
public:
    // 点云类型，用于外部获取存储的不同的点云
    enum Region_cloud_type {
        total = 0,
        left_front = 1,
        left_back = 2,
        right_front = 3,
        right_back = 4,
        filtered = 5,
        failure = 6,
        trans = 7,
    };
    // 工作状态
    enum Ground_region_status {
        E_UNKNOWN = 0, //未知
        E_READY = 1,   //准备，待机
        E_BUSY = 2,    //工作正忙
        E_FAULT = 10, //故障
    };

#define GROUND_REGION_DETECT_CYCLE_MS 500

    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(std::vector<Common_data::Car_wheel_information> &car_wheel_information,
                                                     std::chrono::system_clock::time_point command_time);
    /**
     * @description 获取最后一次算法运行结果的plc正向算法结果消息；
     * @param p_car_wheel_information 测量结果结果指针；
     * @param command_time 时间点，获取该时间点500ms以前到该时间点的算法结果；
     * @return 如果没有就会报错, 不会等待；
    **/
    Error_manager get_last_forward_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                             std::chrono::system_clock::time_point command_time);

    /**
     * @description 获取最后一次算法运行结果的plc反向算法结果消息；
     * @param p_car_wheel_information 测量结果结果指针；
     * @param command_time 时间点，获取该时间点500ms以前到该时间点的算法结果；
     * @return 如果没有就会报错, 不会等待；
    **/
    Error_manager get_last_reverse_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                             std::chrono::system_clock::time_point command_time);

    /**
     * @description 获取最后一次算法运行结果的终端算法结果消息；
     * @param p_car_wheel_information 测量结果结果指针；
     * @param command_time 时间点，获取该时间点500ms以前到该时间点的算法结果；
     * @return 如果没有就会报错, 不会等待；
    **/
    Error_manager get_last_terminal_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                             std::chrono::system_clock::time_point command_time);

    /**
     * @description 获取算法最后一次计算时的各种点云，需要指出哪个点云；
     * @param out_cloud 点云引用，结果在这个里面，会对输入点云进行一次清除操作；
     * @param type 点云种类，根据Region_cloud_type确定；
     * @return 无；
    **/
    void get_region_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud, const Region_cloud_type &type);

    /**
     * @brief 获取区域终端号；
     * @param 无；
     * @return 未初始化调用将返回-1；
    **/
    int get_terminal_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; }

    /**
     * @description 检查激光是否在区域中；
     * @param lidar_id 雷达id；
     * @return 在为true， 不在为false；
    **/
    bool check_lidar_inside(const int &lidar_id);

private:
    /**
     * @description 对点云进行裁减滤波，然后通过欧式聚类判断是否存在四块点云，如果存在且可以构成矩形，通过ceres对点云进行车轮参数寻优；
     * @param cloud 输入点云；
     * @param thresh_z 保留该值以下的点云；
     * @param result ceres库寻优结果；
     * @return true：成功，false：失败；
    **/
    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                               detect_wheel_ceres3d::Detect_result &result, std::string &error);

    /**
     * @description 算法测量线程函数，在这个线程中不断对点云进行算法检测；
    **/
    void thread_measure_func();

    /**
     * @description 欧式聚类，pcl接口二次封装；
     * @param sor_cloud 输入点云；
     * @param tolerance 聚类距离参数
     * @return 各个类的点云，存储在std的vector中；
    **/
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr &sor_cloud, const float &tolerance);

    /**
     * @description 判断输入点是否是一个近似的矩形（直角边与坐标轴的夹角 <20°）；
     * @param points 四个点的坐标信息（如果是三个点会模拟补全）；
     * @return true:类矩形，false：非矩形；
    **/
    bool isRect(std::vector<cv::Point2f> &points);

    /**
     * @description 对坐标经过plc偏移转换过的算法检测结果根据输入的超界限制给出超界判断结果；
     * @param measure_result 算法检测的结果；
     * @param out_info 超界范围限制信息；
     * @return 超界结果，按Range_status进行位组合；
    **/
    int outOfRangeDetection(Common_data::Car_wheel_information &measure_result, const velodyne::outOfRangeInfo &out_info, detect_wheel_ceres3d::Detect_result &result);

    void getMSE(MemoryBox<double> &box, float &value);

    /**
     * @description 对输入的两个测量结果作对比，给出误差率；
     * @param first 测量结果1；
     * @param second 测量结果2；
     * @return 测量结果平均波动与波动标准的比；
    **/
    bool CompareCeresResult(detect_wheel_ceres3d::Detect_result &first, detect_wheel_ceres3d::Detect_result &second);
private:

    std::chrono::steady_clock::time_point m_last_move_time = std::chrono::steady_clock::now();
    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_detect_failture_cloud_collection_mutex;                                // 点云检测失败更新互斥锁, 内存由上级管理
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_detect_failture_cloud_collection;            // 扫描点的点云检测失败集合, 内存由上级管理
    std::chrono::steady_clock::time_point m_detect_failture_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;      // 底盘切除后的点

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


    MemoryBox<double> m_car_cx_box;                 // 存储的近期车辆轴线坐标
    MemoryBox<double> m_car_theta_box;              // 存储的近期车辆车身偏转角度
    MemoryBox<double> m_car_width_box;              // 存储的近期车辆宽度信息
    MemoryBox<double> m_car_wheel_base_box;         // 存储的近期车辆轴距信息

    // 裁剪块
    std::vector<pcl::Vertices> m_polygons;                    //设置动态数组用于保存凸包顶点
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_surface_hull;       //设置点云用于描述凸包的形状

    // 车辆中心优化
    WheelsPredictor m_predictor;

    int last_range_statu;
#define SaveWheelsData 1
#if SaveWheelsData
    int current_index = 0;
#endif
};