server.cpp

#include "server.h"

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

ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) :
    _rbc(rbc)
  , _stopped(std::make_shared<std::atomic_bool>(true))
{

}

void ros_bridge::com_private::Server::advertiseService(const std::string &service,
                                                     const std::string &type,
                                                     const Server::CallbackType &userCallback)
{
    std::string clientName = _serviceAdvertiserKey + service;
    auto it = _serviceMap.find(clientName);
    if (it != _serviceMap.end())
        return; // Service allready advertised.
    _serviceMap.insert(std::make_pair(clientName, service));
    _rbc.addClient(clientName);

    auto rbc = &_rbc;
    _rbc.advertiseService(clientName, service, type,
                          [userCallback, rbc](
                          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;
        }

        if ( !document.HasMember("args") || !document["args"].IsObject()){
            std::cerr << "advertiseServiceCallback(): message has no args member: "
                      << messagebuf << std::endl;
            return;
        }

        if ( !document.HasMember("service") || !document["service"].IsString()){
            std::cerr << "advertiseServiceCallback(): message has no service member: "
                      << messagebuf << std::endl;
            return;
        }

        if ( !document.HasMember("id") || !document["id"].IsString()){
            std::cerr << "advertiseServiceCallback(): message has no id member: "
                      << 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));

        rbc->serviceResponse(
                    document["service"].GetString(),
                    document["id"].GetString(),
                    pDocResponse->MemberCount() > 0 ? true : false,
                    *pDocResponse);

        return;

    });
}

ros_bridge::com_private::Server::~Server()
{
    this->reset();
}

void ros_bridge::com_private::Server::start()
{
    _stopped->store(false);
}

void ros_bridge::com_private::Server::reset()
{
    if ( _stopped->load() )
        return;
    std::cout << "Server: _stopped->store(true)" << std::endl;
    _stopped->store(true);
    for (const auto& item : _serviceMap){
        std::cout << "Server: unadvertiseService " << item.second << std::endl;
        _rbc.unadvertiseService(item.second);
        std::cout << "Server: removeClient " << item.first << std::endl;
        _rbc.removeClient(item.first);
    }
    std::cout << "Server: _serviceMap cleared " << std::endl;
    _serviceMap.clear();
}