Server.cpp

#include "Server.h"

#include "rapidjson/include/rapidjson/ostreamwrapper.h"

ROSBridge::ComPrivate::Server::Server(CasePacker &casePacker, RosbridgeWsClient &rbc) :
    _rbc(rbc)
  , _casePacker(casePacker)
{

}

void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
                                                     const std::string &type,
                                                     const Server::CallbackType &userCallback)
{
    std::string clientName = _serviceAdvertiserKey + service;
    auto it = std::find(_clientList.begin(), _clientList.end(), clientName);
    if (it != _clientList.end())
        return; // Service allready advertised.
    _clientList.push_back(clientName);
    _rbc.addClient(clientName);

    auto rbc = &_rbc;
    auto casePacker = &_casePacker;
    _rbc.advertiseService(clientName, service, type,
                          [userCallback, rbc, casePacker](
                          std::shared_ptr<WsClient::Connection>,
                          std::shared_ptr<WsClient::InMessage> in_message){
        // message->string() is destructive, so we have to buffer it first
        std::string messagebuf = in_message->string();
        //std::cout << "advertiseServiceCallback(): Message Received: "
                 // << messagebuf << std::endl;

        rapidjson::Document document;
        if (document.Parse(messagebuf.c_str()).HasParseError())
        {
          std::cerr << "advertiseServiceCallback(): Error in parsing service request message: "
                    << messagebuf << std::endl;
          return;
        }

        // Extract message and call callback.
        if ( !document.HasMember("args") || !document["args"].IsObject()){
            std::cerr << "advertiseServiceCallback(): message has no args: "
                      << messagebuf << std::endl;
            return;
        }
        JsonDocUPtr pDoc(new JsonDoc());
        std::cout << "args member count: " << document["args"].MemberCount();
        if ( document["args"].MemberCount() > 0)
            pDoc->CopyFrom(document["args"].Move(), document.GetAllocator());
        JsonDocUPtr pDocResponse = userCallback(std::move(pDoc));

        rapidjson::OStreamWrapper out(std::cout);
        // Write document...
        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
        std::cout << "Ros Server: ";
        (*pDocResponse).Accept(writer);
        std::cout << std::endl;

        rbc->serviceResponse(
                    document["service"].GetString(),
                    document["id"].GetString(),
                    true,
                    *pDocResponse);

        return;

    });
}

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

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

void ROSBridge::ComPrivate::Server::reset()
{
    if (!_running)
        return;
    for (const auto& str : _clientList)
        _rbc.removeClient(str);
    _clientList.clear();
}