TopicSubscriber.cpp

#include "TopicSubscriber.h"


ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
        RosbridgeWsClient &rbc) :
    _casePacker(casePacker)
  , _rbc(rbc)
  , _running(false)
{

}

ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber()
{
    this->reset();
}

void ROSBridge::ComPrivate::TopicSubscriber::start()
{
    _running = true;
}

void ROSBridge::ComPrivate::TopicSubscriber::reset()
{
    if ( _running ){
        _running = false;
        {
            for (std::string clientName : _clientList){
                _rbc.removeClient(clientName);
            }
        }
        {
            std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
            _callbackMapStruct.map.clear();
        }
        _clientList.clear();
    }
}

bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
        const char *topic,
        const std::function<void(JsonDocUPtr)> &callback)
{
    if ( !_running )
        return false;

    std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey
                           + std::string(topic);
    _clientList.push_back(clientName);

    HashType hash = getHash(clientName);
    {
        std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
        auto ret = _callbackMapStruct.map.insert(std::make_pair(hash, callback)); //
        if ( !ret.second )
            return false; // Topic subscription already present.

    }

    using namespace std::placeholders;
    auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
                      hash,
                      std::ref(_callbackMapStruct),
                      _1, _2);

//    std::cout << std::endl;
//    std::cout << "topic subscription" << std::endl;
//    std::cout << "client name: " << clientName << std::endl;
//    std::cout << "topic: " << topic << std::endl;
    {
        auto start = std::chrono::high_resolution_clock::now();
        _rbc.addClient(clientName);
        auto end = std::chrono::high_resolution_clock::now();
        std::cout  << "add client time: "
                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                   << " ms" << std::endl;
        start = std::chrono::high_resolution_clock::now();
        _rbc.subscribe(clientName,
                        topic,
                        f );
        end = std::chrono::high_resolution_clock::now();
        std::cout  << "subscribe time: "
                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                   << " ms" << std::endl;
    }

    return true;
}


using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
void ROSBridge::ComPrivate::subscriberCallback(
        const HashType &hash,
        ROSBridge::ComPrivate::MapStruct &mapWrapper,
        std::shared_ptr<WsClient::Connection>,
        std::shared_ptr<WsClient::InMessage> in_message)
{
    // Parse document.
    JsonDoc docFull;
    docFull.Parse(in_message->string().c_str());
    if ( docFull.HasParseError() ) {
        std::cout << "Json document has parse error: "
                  << in_message->string()
                  << std::endl;
        return;
    } else if (!docFull.HasMember("msg")) {
        std::cout << "Json document does not contain a message (\"msg\"): "
                  << in_message->string()
                  << std::endl;
        return;
    }


//    std::cout << "Json document: "
//              << in_message->string()
//              << std::endl;

    // Search callback.
    CallbackType callback;
    {
        std::lock_guard<std::mutex> lk(mapWrapper.mutex);
        auto it = mapWrapper.map.find(hash);
        if (it == mapWrapper.map.end()) {
            //assert(false); // callback not found
            return;
        }
        callback = it->second;
    }

    // Extract message and call callback.
    JsonDocUPtr pDoc(new JsonDoc());
    pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
    callback(std::move(pDoc));
    return;

}