TopicSubscriber.cpp

#include "TopicSubscriber.h"


ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
        ROSBridge::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 ){
        for (std::string clientName : _clientList)
            _rbc->removeClient(clientName);
        _running = false;
        _callbackMap.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);
    auto ret = _callbackMap.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::cref(_callbackMap),
                      _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;
    _rbc->addClient(clientName);
    _rbc->subscribe(clientName,
                    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;

}

void ROSBridge::ComPrivate::test(
        std::shared_ptr<WsClient::Connection>,
        std::shared_ptr<WsClient::InMessage> in_message)
{

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