TopicSubscriber.cpp

#include "TopicSubscriber.h"


ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
        ROSBridge::CasePacker *casePacker,
        RosbridgeWsClient *rbc) :
    _casePacker(casePacker)
  , _rbc(rbc)
{

}

void ROSBridge::ComPrivate::TopicSubscriber::start()
{
    _rbc->addClient(ROSBridge::ComPrivate::_topicSubscriberKey);
}

void ROSBridge::ComPrivate::TopicSubscriber::stop()
{
    _rbc->removeClient(ROSBridge::ComPrivate::_topicSubscriberKey);
}

bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
        const char *topic,
        const std::function<void(JsonDocUPtr)> &callback)
{
    using namespace std::placeholders;
    HashType hash = getHash(topic);
    auto ret = _callbackMap.insert(std::make_pair(hash, callback)); //
    if ( !ret.second )
        return false; // Topic subscription already present.

    auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback, hash, _callbackMap, _1, _2);
    _rbc->subscribe(ROSBridge::ComPrivate::_topicSubscriberKey, topic, f);

    return true;
}


using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
void ROSBridge::ComPrivate::subscriberCallback(
        const HashType &hash,
        const ROSBridge::ComPrivate::TopicSubscriber::CallbackMap &map,
        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.
    auto it = map.find(hash);
    if (it == map.end()) {
        assert(false); // callback not found
        return;
    }

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

}