Measure/Modules/DataToCloud/rslidar/rslidar_driver.h

#pragma once

#include <iostream>
#include <thread>
#include <mutex>
#include <chrono>
#include <functional>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <deque>

#include <glog/logging.h>

#include "rs_driver/api/lidar_driver.hpp"
#include "rs_driver/driver/decoder/decoder_factory.hpp"
#include "pcl/common/transforms.h"

#ifdef ENABLE_PCL_POINTCLOUD
#include <rs_driver/msg/pcl_point_cloud_msg.hpp>
#else

#include "rs_driver/msg/point_cloud_msg.hpp"

#endif

#include "error_code/error_code.hpp"
#include "rslidar_mqtt_async.h"
#include <opencv2/opencv.hpp>

using namespace robosense::lidar;

typedef PointXYZIRT PointT;
typedef PointCloudT<PointT> PointCloudMsg;

typedef int (*ArrivedCloudPoint)(std::shared_ptr<PointCloudMsg> msg);

class RslidarDevice {
public:
    // rslidar status
    enum RSLIDAR_STATUS {
        E_UNKNOWN = 0,    //未知
        E_READY = 1,    //正常待机
        E_DISCONNECT = 2,    //断连
        E_BUSY = 3,    //工作正忙
        E_FAULT = 10,    //故障
    };

public:
    // 无参构造函数
    RslidarDevice();

    // 析构函数
    ~RslidarDevice();

    // 初始化
    Error_manager init();

    Error_manager init(RSDriverParam &param);

    Error_manager uninit();

    Error_manager get_last_cloud(std::vector<uint8_t> &data);

    bool updateRSTransformParam(RSTransformParam &transform_param);

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

    RSLIDAR_STATUS status();

    int id();

private:
    void update_status();

    std::shared_ptr<PointCloudMsg> driverGetPointCloudFromCallerCallback(void);

    void driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg);

    void exceptionCallback(const Error &code);

    void packetCallback(const Packet &pkt);

    static void decoderexceptionCallback(const Error &code) {};

    static void decoderCallback(uint16_t v_c, double v_b) {};

protected:
    // 测试变量
    std::thread *mp_cloud_handle_thread;
    double m_timestamp;

    //状态
    RSLIDAR_STATUS m_rs_lidar_device_status;    //设备状态
    RSLIDAR_STATUS m_rs_protocol_status;

    // 雷达配置
    RSDriverParam m_lidar_params;
    LidarDriver<PointCloudMsg> m_lidar_driver;               ///< Declare the driver object

    // // 空点云缓存
    SyncQueue<std::shared_ptr<PointCloudMsg>> free_cloud_queue;
    SyncQueue<std::shared_ptr<PointCloudMsg>> stuffed_cloud_queue;

    std::mutex m_cloud_mutex;
    // 完整可获取点云
    std::vector<PointT> m_ring_cloud;
    // 获取点云时间
    std::chrono::steady_clock::time_point m_capture_time;

    // 点云内参齐次矩阵
    Eigen::Matrix4d m_calib_matrix;

    std::deque<std::vector<unsigned char>> m_lidar_data;
    std::vector<unsigned char> m_tmp_data;
    std::string m_topic;
    std::shared_ptr<robosense::lidar::Decoder<PointCloudMsg>> m_decoder_ptr_;
    std::shared_ptr<PointCloudMsg> m_point_cloud_ = std::make_shared<PointCloudMsg>();
};