Measure/Modules/MeasureNode/velodyne_lidar/velodyne_manager.h

/*
 * @Description: velodyne多线雷达管理类
 * 非分布式模式下，将采集所有雷达点云，并下发给所有区域
 * 分布式模式下，程序各自独立，分别只实例化一个设定的区域，以及相应雷达
 * @Author: yct
 * @Date: 2021-07-23 16:37:33
 * @LastEditTime: 2021-09-03 16:20:26
 * @LastEditors: yct
 */

#ifndef VELODYNE_MANAGER_HH
#define VELODYNE_MANAGER_HH

#include <thread>
#include <map>
#include <memory>

#include "tool/singleton.h"
#include "tool/point_tool.h"
#include "error_code/error_code.hpp"

#include "velodyne_driver/velodyne_lidar_device.h"
#include "ground_region.h"
#include "velodyne_config.pb.h"
#include "velodyne_manager_task.h"
#include "rslidar_mqtt_async.h"

struct Timestamped_cloud {
public:
    Timestamped_cloud(velodyne::CalibParameter calib) {
        m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        m_time_collected = std::chrono::system_clock::now();
        m_extrinsic.CopyFrom(calib);
        updateMatrix();
    }

    Timestamped_cloud(velodyne::CalibParameter calib, pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud) {
        m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        m_cloud->operator+=(*input_cloud);
        m_time_collected = std::chrono::system_clock::now();
        m_extrinsic.CopyFrom(calib);
        updateMatrix();
    }

    ~Timestamped_cloud() {}

    // 设置点云，覆盖
    void setCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud) {
        m_cloud->clear();
        m_cloud->operator+=(*input_cloud);
        m_time_collected = std::chrono::system_clock::now();
    }

    // 添加点云至现有点云中
    void addCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud) {
        m_cloud->operator+=(*input_cloud);
        m_time_collected = std::chrono::system_clock::now();
    }

    void clearCloud() {
        m_cloud->clear();
        m_time_collected = std::chrono::system_clock::now();
    }

    // 更新外参矩阵
    void updateMatrix() {
        Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(m_extrinsic.r() * M_PI / 180.0, Eigen::Vector3d::UnitX());
        Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(m_extrinsic.p() * M_PI / 180.0, Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(m_extrinsic.y() * M_PI / 180.0, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d trans(m_extrinsic.cx(), m_extrinsic.cy(), m_extrinsic.cz());
        m_extrin_matrix << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() *
                           rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
    }

    // 使用外参返回变换后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr transCloud() {
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        if (m_cloud->size() > 0) {
            for (size_t i = 0; i < m_cloud->size(); i++) {
                Eigen::Vector4d t_point(m_cloud->points[i].x, m_cloud->points[i].y, m_cloud->points[i].z, 1.0);
                t_point.x() = m_extrin_matrix(0, 0) * t_point.x() + m_extrin_matrix(0, 1) * t_point.y() +
                              m_extrin_matrix(0, 2) * t_point.z() + m_extrin_matrix(0, 3) * t_point.w();
                t_point.y() = m_extrin_matrix(1, 0) * t_point.x() + m_extrin_matrix(1, 1) * t_point.y() +
                              m_extrin_matrix(1, 2) * t_point.z() + m_extrin_matrix(1, 3) * t_point.w();
                t_point.z() = m_extrin_matrix(2, 0) * t_point.x() + m_extrin_matrix(2, 1) * t_point.y() +
                              m_extrin_matrix(2, 2) * t_point.z() + m_extrin_matrix(2, 3) * t_point.w();
                t_point.w() = m_extrin_matrix(3, 0) * t_point.x() + m_extrin_matrix(3, 1) * t_point.y() +
                              m_extrin_matrix(3, 2) * t_point.z() + m_extrin_matrix(3, 3) * t_point.w();
                pcl::PointXYZ t_pcl_point;
                t_pcl_point.x = t_point.x() / t_point.w();
                t_pcl_point.y = t_point.y() / t_point.w();
                t_pcl_point.z = t_point.z() / t_point.w();
                t_cloud->push_back(t_pcl_point);
            }
        }
        // t_cloud->operator+=(*m_cloud);

        return t_cloud;
    }

    // 时间戳
    std::chrono::system_clock::time_point m_time_collected;
    // 点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
    // 外参数据
    velodyne::CalibParameter m_extrinsic;
    // 外参齐次矩阵
    Eigen::Matrix4d m_extrin_matrix;
};

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

public:
    //velodyne管理任务超时时间1000ms,
#define VELODYNE_MANAGER_EXECUTE_TIMEOUT_MS 1000
    //velodyne配置参数的默认路径
    const char *VELODYNE_MANAGER_PARAMETER_PATH = ETC_PATH"MeasureNode/velodyne_manager.prototxt";
    const char *LEFT_WHEEL_MODEL = ETC_PATH"MeasureNode/left_model.txt";
    const char *RIGHT_WHEEL_MODEL = ETC_PATH"MeasureNode/right_model.txt";
    //雷达管理的状态
    enum Velodyne_manager_status {
        E_UNKNOWN = 0,    //未知
        E_READY = 1,    //准备，待机
        E_BUSY = 2,    //工作正忙

        E_ISSUED_SCAN = 3,    //下发任务, 获取点云
        E_WAIT_SCAN = 4,    //等待任务, 扫描点云

        E_ISSUED_DETECT = 5,    //下发任务, 算法预测
        E_WAIT_DETECT = 6,    //等待任务, 算法预测

        E_FAULT = 10,    //故障
    };

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

    Velodyne_manager &operator=(const Velodyne_manager &) = delete;

    ~Velodyne_manager() = default;

    //初始化 雷达 管理模块。如下三选一
    Error_manager velodyne_manager_init(int terminal = 0);

    //初始化 雷达 管理模块。从文件读取
    Error_manager velodyne_manager_init_from_protobuf(std::string prototxt_path, int terminal = 0);

    //初始化 雷达 管理模块。从protobuf读取
    Error_manager
    velodyne_manager_init_from_protobuf(velodyne::velodyneManagerParams &velodyne_parameters, int terminal = 0);

    // 反初始化
    Error_manager Velodyne_manager_uninit();

    //对外的接口函数，负责接受并处理任务单，
    Error_manager execute_task(Task_Base *p_velodyne_manager_task);

    //结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
    Error_manager end_task();

    //取消任务单，由发送方提前取消任务单
    Error_manager cancel_task(Task_Base *p_velodyne_manager_task);

    //检查雷达状态,是否正常运行
    Error_manager check_status();

    //判断是否为待机，如果已经准备好，则可以执行任务。
    bool is_ready() { return m_velodyne_manager_status == E_READY; }

public://get or set member variable
    Velodyne_manager_status get_status() { return m_velodyne_manager_status; }

    std::map<int, Velodyne_lidar_device *> &get_velodyne_lidar_device_map() { return m_velodyne_lidar_device_map; }

    std::map<int, Ground_region *> &get_ground_region_map() { return m_ground_region_map; }

protected://member functions
    //自动收集点云的线程函数
    void collect_cloud_thread_fun();

    //更新点云，自行计算
    Error_manager update_region_cloud();

    //执行外界任务的执行函数
    void execute_thread_fun();

    // 根据区域与激光id生成唯一string作为key
    std::string generate_region_lidar_key(int region_id, int lidar_id) {
        return std::to_string(region_id) + std::string("-") + std::to_string(lidar_id);
    }

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

    int m_terminal_id;
    Velodyne_manager_status m_velodyne_manager_status; //velodyne管理状态

    std::map<int, Velodyne_lidar_device *> m_velodyne_lidar_device_map;        // velodyne雷达实例指针数组, 内存由本类管理
    CloudDataMqttAsync *m_mqtt_cloud_data;
    std::map<int, Ground_region *> m_ground_region_map;            // 区域功能实例指针数组, 内存由本类管理

    //velodyne雷达的自动功能, 定时收集所有点云, 然后通知detect去计算车轮信息.
    std::mutex m_cloud_collection_mutex;       // 点云更新互斥锁
    std::map<std::string, std::shared_ptr<Timestamped_cloud>> m_region_laser_to_cloud_collection_map;            //扫描点的点云集合, 内存由本类管理
    // std::chrono::system_clock::time_point		m_cloud_update_time;			//扫描点的更新时间. 已被时间戳点云替换

    std::thread *mp_collect_cloud_thread;        //收集点云的线程指针，内存由本类管理
    Thread_condition m_collect_cloud_condition;        //收集点云的条件变量

    //任务执行线程
    std::thread *mp_execute_thread;            //执行的线程指针，内存由本类管理
    Thread_condition m_execute_condition;            //执行的条件变量

    Velodyne_manager_task *mp_velodyne_manager_task;            //velodyne管理模块的任务单的指针，内存由发送方管理。
};

#endif // !VELODYNE_MANAGER_HH