Measure/Modules/MeasureNode/velodyne_lidar/rs_device_bridge.hpp

#ifndef RS_DEVICE_BRIDGE_HH
#define RS_DEVICE_BRIDGE_HH
/*
 * @Author: yct 18202736439@163.com
 * @Date: 2023-01-12 14:31:29
 * @LastEditors: yct 18202736439@163.com
 * @LastEditTime: 2023-01-12 15:14:54
 * @FilePath: /puai_wj_2021/velodyne_lidar/velodyne_driver/rs_device_bridge.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#include "rslidar_mqtt_async.h"
#include "velodyne_driver/velodyne_lidar_device.h"

class RS_device_bridge : public Velodyne_lidar_device {
public:

    // 无参构造函数
    RS_device_bridge() {}

    // 析构函数
    ~RS_device_bridge() {}

    // 初始化
    Error_manager init(velodyne::velodyneLidarParams params) {
        m_lidar_id = params.lidar_id();
        return Error_code::SUCCESS;
    }

    // 反初始化
    Error_manager uninit() {
        delete m_rs_lidar;
        m_rs_lidar = nullptr;
        return Error_code::SUCCESS;
    }

    // 检查雷达状态,是否正常运行
    Error_manager check_status() {
        if (m_rs_lidar->isConnected()) {
            return Error_code::SUCCESS;
        }
        return Error_code::FAILED;
    }

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

    int get_lidar_id() {
        return m_lidar_id;
    }

    //外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
    Error_manager get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                         std::chrono::steady_clock::time_point command_time, int timeout_ms = 1500) {
        m_rs_lidar->getCloudData("rslidar/" + std::to_string(m_lidar_id), p_cloud);
        if (p_cloud->empty()) {
            return Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD;
        }
        return Error_code::SUCCESS;
    }

    //外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
    // 注意！！！当前最新点云未成环，或许缺失数据
    Error_manager get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                    std::chrono::steady_clock::time_point command_time) {
        m_rs_lidar->getCloudData("rslidar/" + std::to_string(m_lidar_id), p_cloud);
        if (p_cloud->empty()) {
            return Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD;
        }
        return Error_code::SUCCESS;
    }

    //外部调用获取最新的点云, 如果没有就会报错, 不会等待
    Error_manager get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                 std::chrono::steady_clock::time_point command_time) {
        m_rs_lidar->getCloudData("rslidar/" + std::to_string(m_lidar_id), p_cloud);
        if (p_cloud->empty()) {
            return Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD;
        }
        return Error_code::SUCCESS;
    }

    //外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
    //注:函数内部不加锁, 由外部统一加锁.
    Error_manager get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                 std::chrono::steady_clock::time_point command_time) {
        m_rs_lidar->getCloudData("rslidar/" + std::to_string(m_lidar_id), p_cloud);
        if (p_cloud->empty()) {
            return Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD;
        }
        return Error_code::SUCCESS;
    }

public:
//    TODO
    Velodyne_lidar_device_status get_status() {
        if (m_rs_lidar->isConnected() && m_rs_lidar->isTopicLoss("rslidar/" + std::to_string(m_lidar_id))) {
            return Velodyne_lidar_device_status(E_READY);
        }
        return Velodyne_lidar_device_status(E_DISCONNECT);
    }

    void updata_status() {}

protected:


protected:
    CloudDataMqttAsync *m_rs_lidar = CloudDataMqttAsync::instance();
    int m_lidar_id;
};

#endif