server.cpp

#include "server.h"

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

static const char *serviceAdvertiserKey = "service_advertiser";

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();
}