localizer/include/localizer.h

/**
 * \file localizer.h
 * \brief 简要介绍
 * \details 详细说明
 * \author lenotary
 * \version 1.0
 * \date 2021/6/23
 * \pre 先决条件。（有的话才添加。）
 * \bug 存在的bug。（有的话才添加，注明“还未发现”也可以。）
 * \warning 警告。（有的话才添加。）
 * \copyright 版权声明。（通常只写遵循什么协议，版权详细内容应放在LICENSE文件中，并且应该与LICENSE文件的内容统一，不能自相矛盾。）
 * \since 一些历史情况记录。（有的话才添加。）
 */


//
// Created by lenotary on 2021/6/23.
//

#ifndef SRC_LOCALIZER_H
#define SRC_LOCALIZER_H
#include <thread>
#include <mutex>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>

#include <ros/ros.h>
#include <ros/node_handle.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/registration/ndt.h>

#include <diagnostic_msgs/DiagnosticArray.h>
#include <tf2/transform_datatypes.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include "extended_kalman_filter.h"
#include <fast_gicp/gicp/fast_vgicp.hpp>
#include "common.h"
#include <fast_gicp/ndt/ndt_cuda.hpp>
//#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
#define MAX_PATH_SIZE 1000000
class Localizer {
public:
    Localizer(ros::NodeHandle& nh, ros::NodeHandle &privateNh);
    ~Localizer();
    bool GetTransform(const std::string &cFrameTarget, const std::string &cFrameSource, const geometry_msgs::TransformStamped::Ptr &pTransformStamped);
    static void Split(const std::string& s, std::vector<double>& tokens, const std::string& cDelimiters = " ");

private:
    ExtendedKalmanFilter m_iExtendedKalmanFilter_;
    ros::NodeHandle m_iNh_;
    ros::NodeHandle m_iNhPrivate_;
    //订阅的话题
    ros::Subscriber m_iSubInitPose_;
    ros::Subscriber m_iSubMap_;
    ros::Subscriber m_iSubLidarCloud_;
    ros::Subscriber m_iSubOdom_;
    //发布的话题
    ros::Publisher m_iPubAlignedPointCloud_;
    ros::Publisher m_iPubOdom_;
    ros::Publisher m_iPubExeTime_; //计算时间
    ros::Publisher m_iPubAlignedScore_;
    ros::Publisher m_iPubDiagnostics_;
    ros::Publisher m_iPubTrajectory_;
    ros::Publisher m_iPubAlignedPose_;
    ros::Publisher m_iPubLocalizerState_;

    tf2_ros::Buffer m_iTf2Buffer_;
    tf2_ros::TransformListener m_iTf2Listener_;
    tf2_ros::TransformBroadcaster m_iTfBroadcaster_;

    //map
    std::map<std::string,std::string> m_iMapKeyValue_;
    geometry_msgs::PoseWithCovarianceStamped m_iInitialPoseMsg_;



    std::string m_cFrameMap_;
    std::string m_cFrameBase_;
    bool m_bInitPose_;
    bool m_bIsGetMap_;
    //ekf
    Eigen::MatrixXd m_iMatrixXdEkfQ;
    Eigen::MatrixXd m_iMatrixXdEkfR;
    double m_dOdomToLidar_;
    //aligned
    std::string m_cRegName_;
    fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ> m_iAlignedFVGIcpMt_;
//    fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ> m_iAlignedFVGIcpCuda_;
    pcl::NormalDistributionsTransform<pcl::PointXYZ,pcl::PointXYZ> m_iAlignedNdt_;
    Eigen::Matrix4f m_iPreTrans_, m_iDeltaTrans_;
    bool m_bIsAligned_;
    bool m_bIsLocation_;

    geometry_msgs::PoseStamped m_iMsgPoseStampedResult_;
    std::queue<geometry_msgs::PoseStamped> m_qMsgPoseStampedResult_;
    std::mutex m_iMtxMap_;
    std::thread m_tThreadPointCloud_;
    double m_dExeTime_;
    double m_dScore_;

    double m_dCoverScoreThreshold_;
    double m_dMaxAlignedTime_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_pAlignedPoses_;
    nav_msgs::Path m_iPathWithEkf_;

    //lidar
    double m_dTimeLidar_;
    std::queue<sensor_msgs::PointCloud2> m_qPointCloudInput_;
    //地图
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_pPointClodMap_;
    //odom
    std::queue<nav_msgs::Odometry> m_qOdom_;

private:
    void __InitLocalizer();
    void __OnCallbackInitPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
    void __OnCallBackMap(const sensor_msgs::PointCloud2::ConstPtr &msg);
    void __OnCallBackPointCloud(const sensor_msgs::PointCloud2::ConstPtr &msg);
    void __OnCallBackOdom(const nav_msgs::Odometry::ConstPtr &msg);


    void __PublishTF(const std::string &cFrame, const std::string &cFrameChild,const geometry_msgs::PoseStamped &iPoseMsg);

    template <typename Registration>
    double __AlignedPcl(Registration& reg,const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& source, const Eigen::Matrix4f &iMatrix4fInitPose,Eigen::Matrix4f &iMatrix4fTransform);
    template <typename Registration>
    double __Aligned(Registration& reg,const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& source, const Eigen::Matrix4f &iMatrix4fInitPose,Eigen::Matrix4f &iMatrix4fTransform);
    void __InitAligned(pcl::PointCloud<pcl::PointXYZ>::Ptr pPointCloudMap);
    void __ProcessPointCloud(void);
    void __Relocation(sensor_msgs::PointCloud2 iMsgCloudInput);
};


#endif //SRC_LOCALIZER_H