Measure/Modules/DataToCloud/rslidar/rslidar_mqtt_async.cpp

#include "rslidar_mqtt_async.h"

void RslidarMqttAsyncClient::init() {
    m_message_arrived_callback_ = CloudDataArrived;

    LOG(INFO) << "load mqtt etc file from: " << ETC_PATH"/etc/rslidar_mqtt_async.json";
    MqttAsyncClient::init_from_proto(ETC_PATH"/etc/rslidar_mqtt_async.json");
}

bool RslidarMqttAsyncClient::setCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
    if (cloud->empty()) {
        printf("---Debug cloud size %ld\n", cloud->size());
        return false;
    }

    auto iter_full_cloud = m_full_cloud_.find(topic);
    if (iter_full_cloud == m_full_cloud_.end()) {
        m_full_cloud_.insert(std::pair<std::string, pcl::PointCloud<pcl::PointALL>::Ptr>(topic,new pcl::PointCloud<pcl::PointALL>));
        iter_full_cloud = m_full_cloud_.find(topic);
    }

    {
        std::lock_guard<std::mutex> lck(m_cloud_mutex);
        iter_full_cloud->second->swap(*cloud);
    }

    return true;
}

bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
    std::lock_guard<std::mutex> lck(m_cloud_mutex);
    auto iter = m_full_cloud_.find(topic);
    if (iter == m_full_cloud_.end()) {
        LOG(WARNING) << topic << " can't find data.";
        return false;
    }

    cloud->clear();
    pcl::PointXYZI cloud_p;
    for (auto &point:*iter->second) {
        cloud_p.x = point.x;
        cloud_p.y = point.y;
        cloud_p.x = point.z;
        cloud_p.intensity = point.intensity;
        cloud->points.push_back(cloud_p);
    }
    return true;
}

bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    std::lock_guard<std::mutex> lck(m_cloud_mutex);
    auto iter = m_full_cloud_.find(topic);
    if (iter == m_full_cloud_.end()) {
        return false;
    }
    cloud->clear();
    for (auto &point: *iter->second) {
        cloud->points.emplace_back(point.x, point.y, point.z);
    }
    return true;
}

bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, const uint8_t *color) {
    std::lock_guard<std::mutex> lck(m_cloud_mutex);
    auto iter = m_full_cloud_.find(topic);
    if (iter == m_full_cloud_.end()) {
        return false;
    }
    cloud->clear();
    pcl::PointXYZRGB rgb;
    for (auto &point: *iter->second) {
        rgb.x = point.x;
        rgb.y = point.y;
        rgb.z = point.z;
        rgb.r = color[0];
        rgb.g = color[1];
        rgb.b = color[2];
        cloud->points.emplace_back(rgb);
    }
    return true;
}

int RslidarMqttAsyncClient::CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen,
                                             MQTTAsync_message *message) {
    pcl::PointCloud<pcl::PointALL>::Ptr cloud(new pcl::PointCloud<pcl::PointALL>);
    CloudPointUnpacket unpacket;
    unpacket.unpacket((unsigned char *) message->payload, message->payloadlen, cloud);
    RslidarMqttAsyncClient *t_client = (RslidarMqttAsyncClient *) client;
    t_client->setCloudData(topicName, cloud);
    return 1;
}