puai_wj_2021/velodyne_lidar/region_status_checker.h

/*
 * @Description: 区域状态检查类，用于判断当前各区域车辆状态(已停好、运动中)，以及周围是否有障碍物
 * @Author: yct
 * @Date: 2021-12-21 19:54:34
 * @LastEditTime: 2021-12-28 11:53:11
 * @LastEditors: yct
 */

#ifndef REGION_STATUS_CHECKER_HH
#define REGION_STATUS_CHECKER_HH
#include "../tool/singleton.h"
#include "../tool/common_data.h"
#include "../error_code/error_code.h"
#include <map>
#include <mutex>
#include <deque>
#include <chrono>
#include <vector>
#include <algorithm>
#include <thread>
#include "glog/logging.h"

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
// #include <pcl/registration/ndt.h>
#include <pcl/filters/voxel_grid.h>

#include "icp_svd_registration.hpp"

// #include "opencv2/opencv.hpp"
#include "../tool/point_tool.h"

#define DEFAULT_REGION_STATUS_TIMEOUT_MILLI 600000
#define DEFAULT_REGION_STATUS_MAX_QUEUE_SIZE 600
// 检测到静止后稳定时间
#define MIN_STABLE_TIME_MILLI 1000
// 外点差异量
#define OUTSIDE_POINT_LIMIT 30

class Region_status_checker : public Singleton<Region_status_checker>
{
    // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
    friend class Singleton<Region_status_checker>;

struct Region_measure_info_stamped
{
    Common_data::Car_wheel_information_stamped measure_result;
    pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud;
    Region_measure_info_stamped()
    {
        p_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    }

    // ~Region_measure_info_stamped()
    // {
    //     p_cloud->clear();
    // }
};

public:
    // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
    Region_status_checker(const Region_status_checker &) = delete;
    Region_status_checker &operator=(const Region_status_checker &) = delete;
    ~Region_status_checker() = default;

    void Region_status_checker_init()
    {
        mb_exit = false;
        mp_icp_svd = new ICPSVDRegistration(0.1, 0.0018, 0.036, 20);
        // mp_ndt = pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>);
        // mp_ndt->setMaxCorrespondenceDistance(0.2);
        // mp_ndt->setTransformationEpsilon(0.01);
        // mp_ndt->setEuclideanFitnessEpsilon(0.09);
        // mp_ndt->setMaximumIterations(10);
        m_manager_thread = new std::thread(&Region_status_checker::thread_func, this);
    }

    void Region_status_checker_uninit()
    {
        mb_exit = true;
        if(m_manager_thread != nullptr)
        {
            if(m_manager_thread->joinable())
            {
                m_manager_thread->join();
            }
            delete m_manager_thread;
            m_manager_thread = nullptr;
        }
        if(mp_icp_svd!=nullptr)
        {
            delete mp_icp_svd;
            mp_icp_svd = nullptr;
        }
    }

    // 更新测量数据回调
    void add_measure_data(int terminal_id, Common_data::Car_wheel_information_stamped& measure_result, pcl::PointCloud<pcl::PointXYZ>& cloud)
    {
        Region_measure_info_stamped data;
        data.measure_result = measure_result;
        std::vector<int> index;
        // cloud.is_dense = false;
        // pcl::removeNaNFromPointCloud(cloud, data.cloud, index);
        data.p_cloud->operator+=(cloud);
        if (!data.measure_result.wheel_data.correctness)
            return;
//        LOG(INFO) << data.wheel_data.to_string();
        // 未创建队列
        std::lock_guard<std::mutex> lck(m_mutex);
        if (m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
        {
            std::deque<Region_measure_info_stamped> t_deque;
            t_deque.push_back(data);
            m_measure_results_map.insert(std::pair<int, std::deque<Region_measure_info_stamped>>(terminal_id, t_deque));
        }
        else//已创建
        {
            m_measure_results_map[terminal_id].push_back(data);
        }
    }

    // 根据当前帧判断区域停车状态
    // 当前帧不需要存在正确结果，利用点云判断是否车辆未移动即可
    Error_manager get_region_parking_status(int terminal_id, Common_data::Car_wheel_information_stamped& measure_result, pcl::PointCloud<pcl::PointXYZ>& cloud)
    {
        std::lock_guard<std::mutex> lck(m_mutex);
        if(cloud.size() <= 0)
        {
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("当前空点云 终端") + std::to_string(terminal_id)).c_str());
        }

        if(mp_icp_svd == nullptr)
        {
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("当前icp空指针 终端") + std::to_string(terminal_id)).c_str());
        }

        if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end() || m_measure_results_map[terminal_id].size() == 0)
        {
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("暂无最近历史数据用于推测当前状态 终端") + std::to_string(terminal_id)).c_str());
        }

        // 时间检查, 当前队列中数据全为正确
        // 计算最近一帧外框，x一边外扩4cm, y一边外扩100cm
        auto last_data_ref = m_measure_results_map[terminal_id].back();
        if(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now()-last_data_ref.measure_result.measure_time).count() > DEFAULT_REGION_STATUS_TIMEOUT_MILLI)
        {
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("最近历史数据超时 终端") + std::to_string(terminal_id)).c_str());
        }
// std::cout<<"000000"<<std::endl;
        // 截取当前帧框内与框外点云
        Eigen::Vector2d t_last_center(
            last_data_ref.measure_result.wheel_data.car_center_x,
            last_data_ref.measure_result.wheel_data.car_center_y);
        Eigen::Vector2d t_width_height(
            last_data_ref.measure_result.wheel_data.car_wheel_width + 0.08,
            last_data_ref.measure_result.wheel_data.car_wheel_base + 2.0);

        // 历史正确数据整车旋转
        Eigen::Rotation2Dd t_rot((90 - last_data_ref.measure_result.wheel_data.car_angle) * M_PI / 180.0);
        Eigen::Matrix2d t_rot_mat = t_rot.toRotationMatrix();

        // 计算历史(模板)框内点云与框外点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_inside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_outside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        for (size_t i = 0; i < last_data_ref.p_cloud->size(); i++)
        {
            Eigen::Vector2d t_point(last_data_ref.p_cloud->points[i].x - t_last_center.x(), last_data_ref.p_cloud->points[i].y - t_last_center.y());
            Eigen::Vector2d t_rot_point = t_rot_mat * t_point;
            if (t_rot_point.x() > -t_width_height.x() / 2.0 && t_rot_point.x() < t_width_height.x() / 2.0 && t_rot_point.y() > -t_width_height.y() / 2.0 && t_rot_point.y() < t_width_height.y() / 2.0)
            {
                t_prev_cloud_inside->push_back(last_data_ref.p_cloud->points[i]);
            }
            else
            {
                t_prev_cloud_outside->push_back(last_data_ref.p_cloud->points[i]);
            }

            // if(std::isfinite(last_data_ref.cloud[i].x) && std::isfinite(last_data_ref.cloud[i].y) && std::isfinite(last_data_ref.cloud[i].z))
            // {
            // }else
            // {
            //     std::cout << "!!!!!!!!!!!!!!!!!!!!!!!! prev nan point" << std::endl;

            // }
        }
// std::cout<<"111111"<<std::endl;
        // 计算当前帧在框内与框外点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_inside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_outside = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<int> index;
        pcl::PointCloud<pcl::PointXYZ> t_cloud_valid;
        t_cloud_valid = cloud;
        // t_cloud_valid.is_dense = false;
        // pcl::removeNaNFromPointCloud(t_cloud_valid, t_cloud_valid, index);
        for (size_t i = 0; i < t_cloud_valid.size(); i++)
        {
            Eigen::Vector2d t_point(t_cloud_valid[i].x - t_last_center.x(), t_cloud_valid[i].y - t_last_center.y());
            Eigen::Vector2d t_rot_point = t_rot_mat * t_point;
            if (t_rot_point.x() > -t_width_height.x() / 2.0 && t_rot_point.x() < t_width_height.x() / 2.0 && t_rot_point.y() > -t_width_height.y() / 2.0 && t_rot_point.y() < t_width_height.y() / 2.0)
            {
                t_curr_cloud_inside->push_back(t_cloud_valid[i]);
            }
            else
            {
                t_curr_cloud_outside->push_back(t_cloud_valid[i]);
            }
            // if(std::isfinite(t_cloud_valid[i].x) && std::isfinite(t_cloud_valid[i].y) && std::isfinite(t_cloud_valid[i].z))
            // {
            // }else{
            //     std::cout << "!!!!!!!!!!!!!!!!!!!!!!!! curr nan point" << std::endl;

            // }
        }
// std::cout<<"222222"<<std::endl;
        if(t_prev_cloud_inside->size() <= 30 || t_curr_cloud_inside->size() <= 30)
        {
            char buf[255];
            memset(buf, 0, 255);
            sprintf(buf, ",, prev %d, curr %d;; last center(%.3f, %.3f)", t_prev_cloud_inside->size(), t_curr_cloud_inside->size(), t_last_center.x(), t_last_center.y());
            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, (std::string("区域剪切后无点可用于匹配 终端") + std::to_string(terminal_id)+std::string(buf)).c_str());
        }
        // std::cout << "000" << std::endl;

        // 直接使用当前时间，测量时间传入存在变为0的情况!!!!!
        measure_result.measure_time = std::chrono::system_clock::now();
        double dtime = std::chrono::duration_cast<std::chrono::milliseconds>(measure_result.measure_time - last_data_ref.measure_result.measure_time).count();
        // double curr_time = std::chrono::duration_cast<std::chrono::milliseconds>(measure_result.measure_time.time_since_epoch()).count();
        // double last_time = std::chrono::duration_cast<std::chrono::milliseconds>(last_data_ref.measure_result.measure_time.time_since_epoch()).count();
        // std::cout << "dtime " << dtime / 1000.0 << ",, curr, last: " << curr_time << ", " << last_time << ", diff: " << curr_time - last_time << std::endl;
        
        // icp计算前后内点匹配情况
        //下采样
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_prev_cloud_inside_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_curr_cloud_inside_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::VoxelGrid<pcl::PointXYZ> vox;    //创建滤波对象
        vox.setInputCloud(t_prev_cloud_inside);         //设置需要过滤的点云给滤波对象
        vox.setLeafSize(0.03f, 0.03f, 0.03f); //设置滤波时创建的体素体积为1cm的立方体
        vox.filter(*t_prev_cloud_inside_filtered);             //执行滤波处理，存储输出

        vox.setInputCloud(t_curr_cloud_inside);         //设置需要过滤的点云给滤波对象
        vox.setLeafSize(0.03f, 0.03f, 0.03f); //设置滤波时创建的体素体积为1cm的立方体
        vox.filter(*t_curr_cloud_inside_filtered);             //执行滤波处理，存储输出

        mp_icp_svd->SetInputTarget(t_prev_cloud_inside_filtered);
        

        // static int count = 0;
        // if(count++ == 20)
        // {
        //     save_cloud_txt(t_prev_cloud_inside_filtered, "./t_prev_cloud.txt");
        //     save_cloud_txt(t_curr_cloud_inside_filtered, "./t_curr_cloud.txt");
        // }

        // std::cout << "111" << std::endl;
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_result_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        Eigen::Matrix4f t_pred_pose, t_res_pose;
        t_pred_pose = Eigen::Matrix4f::Identity();
        if (!mp_icp_svd->ScanMatch(t_curr_cloud_inside_filtered, t_pred_pose, t_result_cloud, t_res_pose))
        {
            return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("icp匹配失败，终端") + std::to_string(terminal_id)).c_str());
        }

        // std::cout << "222" << std::endl;
        if(is_significant(t_res_pose, dtime / 1000.0))
        {
            mb_is_stable = false;
            return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("检测到车辆移动，终端") + std::to_string(terminal_id)).c_str());
        }else{
            if (!mb_is_stable)
            {
                mb_is_stable = true;
                m_stable_time = std::chrono::system_clock::now();
                return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("开始等待稳定，终端") + std::to_string(terminal_id)).c_str());
            }else
            {
                auto current_time = std::chrono::system_clock::now();
                double duration = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - m_stable_time).count();
                if(duration > MIN_STABLE_TIME_MILLI)
                {
                    // 判断外点数量
                    // LOG_IF(WARNING, terminal_id == 4) << "ouside point prev curr: " << t_prev_cloud_outside->size() << ", " << t_curr_cloud_outside->size();
                    if (t_curr_cloud_outside->size() > (t_prev_cloud_outside->size() + OUTSIDE_POINT_LIMIT))
                    {
                        // debug提示用
                        // LOG_IF(WARNING, terminal_id == 4) << "检测到车身外侧噪点存在！！！";
                    }
                    return SUCCESS;
                }else
                {
                    return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, (std::string("等待稳定中，终端") + std::to_string(terminal_id)).c_str());
                }
            }
        }
        // std::cout << "333" << std::endl;

        return ERROR;
    }

    // 显著性判断，平移x方向速度0.02m/s,y方向速度0.02m/s误差，角度3deg/s误差
    bool is_significant(Eigen::Matrix4f trans, double dtime, double vx_eps = 0.02, double vy_eps = 0.02, double omega_eps = 0.051)
    {
        // 间隔时间为负数，或间隔超过0.5s则手动赋值
        if(dtime<0 || dtime>0.5)
        {
            dtime = 0.1;
        }
            // return false;
        Eigen::Vector3f translation = trans.block<3, 1>(0, 3);
        float rot_angle = fabs(acos((std::min(trans.block<3, 3>(0, 0).trace(), 2.999999f) - 1.0f) / 2.0f));
        // 值足够小，可当做车辆未移动
        Eigen::Vector3d velocity(fabs(translation.x() / dtime), fabs(translation.y() / dtime), fabs(rot_angle / dtime));

        if (velocity.x() <= vx_eps && velocity.y() <= vy_eps && velocity.z() <= omega_eps)
        {
            return false;
        }
        // char buf[512] = {0};
        // sprintf(buf, "显著性分析, 速度 %.3f %.3f %.3f", velocity.x(), velocity.y(), velocity.z());
        // LOG(INFO) << buf << ", dtime: " << dtime;
        // // LOG(WARNING) << "???????????????????????????????????显著？？？？？？";
        // if (velocity.x() > vx_eps)
        // {
        //     LOG(WARNING) << "x:" << velocity.x();
        // }
        // if(velocity.y() > vy_eps)
        // {
        //     LOG(WARNING) << "y:" << velocity.y();
        // }
        // if(velocity.z() > omega_eps)
        // {
        //     LOG(WARNING) << "z:" << velocity.z();
        // }
        return true;
    }

    void thread_func()
    {
        while(!mb_exit)
        {
            {
                std::lock_guard<std::mutex> lck(m_mutex);
                for (auto iter = m_measure_results_map.begin(); iter != m_measure_results_map.end(); iter++)
                {
                    std::deque<Region_measure_info_stamped> *tp_data_queue = &(iter->second);
                    for (size_t i = 0; i < tp_data_queue->size(); i++)
                    {
                        // 判断队列头部(最老历史数据)是否超时，超时则丢出
                        double dtime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - tp_data_queue->front().measure_result.measure_time).count();
                        if(dtime > DEFAULT_REGION_STATUS_TIMEOUT_MILLI)
                        {
                            tp_data_queue->pop_front();
                        }
                    }
                    // 维持队列长度
                    while (tp_data_queue->size() > DEFAULT_REGION_STATUS_MAX_QUEUE_SIZE)
                    {
                        tp_data_queue->pop_front();
                    }
                }
            }
            usleep(1000 * 100);
        }
    }

private:
    // 父类的构造函数必须保护，子类的构造函数必须私有。
    Region_status_checker() = default;

    // map访问锁
    std::mutex m_mutex;
    // 各终端测量数据队列
    std::map<int, std::deque<Region_measure_info_stamped> > m_measure_results_map;

    bool mb_exit;
    // 内部管理线程
    std::thread *m_manager_thread;

    // // 配置参数
    // // 合法时间长度，即过去多久时间内数据有效
    // double m_valid_time_milli;
    // // 队列极限长度
    // double m_max_queue_size;

    // icp匹配
    ICPSVDRegistration *mp_icp_svd;
    // 是否已经稳定
    bool mb_is_stable;
    // 最初稳定时刻
    std::chrono::system_clock::time_point m_stable_time;

    // // 点云高度截断，保留底层点云匹配
    // double height_limit;
};

#endif // !REGION_STATUS_CHECKER_HH