server.cpp 3.57 KB
Newer Older
1
#include "server.h"
2

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

5
ros_bridge::com_private::Server::Server(CasePacker &casePacker, RosbridgeWsClient &rbc) :
6 7
    _rbc(rbc)
  , _casePacker(casePacker)
8
  , _stopped(std::make_shared<std::atomic_bool>(true))
9 10 11 12
{

}

13
void ros_bridge::com_private::Server::advertiseService(const std::string &service,
14 15 16 17
                                                     const std::string &type,
                                                     const Server::CallbackType &userCallback)
{
    std::string clientName = _serviceAdvertiserKey + service;
18 19
    auto it = _serviceMap.find(clientName);
    if (it != _serviceMap.end())
20
        return; // Service allready advertised.
21
    _serviceMap.insert(std::make_pair(clientName, service));
22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
    _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;
        }

        if ( !document.HasMember("args") || !document["args"].IsObject()){
44 45 46 47 48 49 50 51 52 53 54 55 56
            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: "
57 58 59 60 61 62 63 64 65 66 67 68
                      << 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(),
69
                    pDocResponse->MemberCount() > 0 ? true : false,
70 71 72 73 74 75 76
                    *pDocResponse);

        return;

    });
}

77
ros_bridge::com_private::Server::~Server()
78
{
79 80
    this->reset();
}
81

82
void ros_bridge::com_private::Server::start()
83
{
84
    _stopped->store(false);
85 86
}

87
void ros_bridge::com_private::Server::reset()
88
{
89
    if ( _stopped->load() )
90
        return;
91 92
    std::cout << "Server: _stopped->store(true)" << std::endl;
    _stopped->store(true);
93
    for (const auto& item : _serviceMap){
94
        std::cout << "Server: unadvertiseService " << item.second << std::endl;
95
        _rbc.unadvertiseService(item.second);
96
        std::cout << "Server: removeClient " << item.first << std::endl;
97 98
        _rbc.removeClient(item.first);
    }
99
    std::cout << "Server: _serviceMap cleared " << std::endl;
100
    _serviceMap.clear();
101
}