Server.cpp

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2023  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */

#include <mrpt/core/exceptions.h>
#include <mrpt/version.h>
#include <mvsim/Comms/Server.h>
#include <mvsim/Comms/common.h>
#if MRPT_VERSION >= 0x204
#include <mrpt/system/thread_name.h>
#endif

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/mvsim-msgs/GenericAnswer.pb.h>
#include <mvsim/mvsim-msgs/GetServiceInfoAnswer.pb.h>
#include <mvsim/mvsim-msgs/ListNodesAnswer.pb.h>
#include <mvsim/mvsim-msgs/ListNodesRequest.pb.h>
#include <mvsim/mvsim-msgs/ListTopicsAnswer.pb.h>
#include <mvsim/mvsim-msgs/ListTopicsRequest.pb.h>
#include <mvsim/mvsim-msgs/RegisterNodeAnswer.pb.h>
#include <mvsim/mvsim-msgs/RegisterNodeRequest.pb.h>
#include <mvsim/mvsim-msgs/SubscribeAnswer.pb.h>
#include <mvsim/mvsim-msgs/SubscribeRequest.pb.h>
#include <mvsim/mvsim-msgs/UnregisterNodeRequest.pb.h>

#include <zmq.hpp>

#endif

using namespace mvsim;

Server::Server() : mrpt::system::COutputLogger("mvsim::Server") {}

Server::~Server() { shutdown(); }

void Server::start()
{
        ASSERTMSG_(!mainThread_.joinable(), "Server is already running.");

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        requestMainThreadTermination();
        mainThread_ = std::thread(&Server::internalServerThread, this);

#if MRPT_VERSION >= 0x204
        mrpt::system::thread_name("serverMain", mainThread_);
#endif

#else
        THROW_EXCEPTION(
                "MVSIM needs building with ZMQ and PROTOBUF to enable client/server");
#endif
}

void Server::shutdown() noexcept
{
        try
        {
                MRPT_LOG_DEBUG_STREAM("Waiting for the thread to quit.");
                requestMainThreadTermination();

                if (mainThread_.joinable()) mainThread_.join();

                MRPT_LOG_DEBUG_STREAM("Joined thread.");
        }
        catch (const std::exception& e)
        {
                MRPT_LOG_ERROR_STREAM(
                        "shutdown: Exception: " << mrpt::exception_to_str(e));
        }
}

void Server::internalServerThread()
{
        using namespace std::string_literals;

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        try
        {
                MRPT_LOG_INFO_STREAM("Server thread started.");

                zmq::context_t context(3);
                mainThreadZMQcontext_ = &context;

                zmq::socket_t mainRepSocket(context, ZMQ_REP);
                mainRepSocket.bind("tcp://*:"s + std::to_string(serverPortNo_));

                for (;;)
                {
                        zmq::message_t request;

                        //  Wait for next request from client
#if CPPZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 3, 1)
                        std::optional<size_t> reqSize = mainRepSocket.recv(request);
                        ASSERT_(reqSize.has_value());
#else
                        mainRepSocket.recv(&request);
#endif

                        // Variant with all valid client requests:
                        using client_requests_t = std::variant<
                                mvsim_msgs::RegisterNodeRequest,
                                mvsim_msgs::UnregisterNodeRequest, mvsim_msgs::SubscribeRequest,
                                mvsim_msgs::ListNodesRequest, mvsim_msgs::ListTopicsRequest,
                                mvsim_msgs::AdvertiseTopicRequest,
                                mvsim_msgs::AdvertiseServiceRequest,
                                mvsim_msgs::GetServiceInfoRequest>;

                        // Parse and dispatch:
                        try
                        {
                                client_requests_t req =
                                        mvsim::parseMessageVariant<client_requests_t>(request);

                                std::visit(
                                        overloaded{
                                                [&](const auto& m) { this->handle(m, mainRepSocket); },
                                        },
                                        req);
                        }
                        catch (const UnexpectedMessageException& e)
                        {
                                MRPT_LOG_ERROR_STREAM(e.what());
                        }
                }
        }
        catch (const zmq::error_t& e)
        {
                if (e.num() == ETERM)
                {
                        // This simply means someone called requestMainThreadTermination().
                        // Just exit silently.
                        MRPT_LOG_DEBUG_STREAM(
                                "Server thread about to exit for ZMQ term signal.");
                }
                else
                {
                        MRPT_LOG_ERROR_STREAM(
                                "internalServerThread: ZMQ error: " << e.what());
                }
        }
        catch (const std::exception& e)
        {
                MRPT_LOG_ERROR_STREAM(
                        "internalServerThread: Exception: " << mrpt::exception_to_str(e));
        }
        MRPT_LOG_DEBUG_STREAM("Server thread quitted.");

        mainThreadZMQcontext_ = nullptr;
#endif
}

void Server::requestMainThreadTermination()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        zmq::context_t* ctx = mainThreadZMQcontext_;
        if (ctx)
        {
#if CPPZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 4, 0)
                ctx->shutdown();
#else
                // Missing shutdown() in older versions:
                zmq_ctx_shutdown(ctx->operator void*());
#endif
        }
#endif
}

void Server::db_remove_node(const std::string& nodeName)
{
        std::unique_lock lck(dbMutex);

        auto itNode = connectedNodes_.find(nodeName);
        if (itNode == connectedNodes_.end()) return;  // Nothing to do

        for (const std::string& topic : itNode->second.advertisedTopics)
        {
                auto itTopic = knownTopics_.find(topic);
                if (itTopic == knownTopics_.end()) continue;
                knownTopics_.erase(itTopic);
        }

        // for (const std::string& topic : itNode->second.subscribedTopics) { }

        connectedNodes_.erase(itNode);
}

void Server::db_register_node(const std::string& nodeName)
{
        std::unique_lock lck(dbMutex);

        connectedNodes_.emplace_hint(connectedNodes_.end(), nodeName, nodeName);
}

void Server::db_advertise_topic(
        const std::string& topicName, const std::string& topicTypeName,
        const std::string& publisherEndpoint, const std::string& nodeName)
{
        std::unique_lock lck(dbMutex);

        // 1) Add as a source of this topic:
        auto& dbTopic = knownTopics_[topicName];

        if (!dbTopic.topicTypeName.empty() &&
                dbTopic.topicTypeName != topicTypeName)
        {
                throw std::runtime_error(mrpt::format(
                        "Trying to register topic `%s` [%s] but already known with type "
                        "[%s]",
                        topicName.c_str(), topicTypeName.c_str(),
                        dbTopic.topicTypeName.c_str()));
        }
        dbTopic.topicName = topicName;
        dbTopic.topicTypeName = topicTypeName;

        dbTopic.publishers.try_emplace(
                nodeName, topicName, nodeName, publisherEndpoint);

        // 2) If clients are already waiting for this topic, inform them so they
        // can subscribe to this new source of data:
        send_topic_publishers_to_subscribed_clients(topicName);
}

void Server::db_add_topic_subscriber(
        const std::string& topicName, const std::string& updatesEndPoint)
{
        std::unique_lock lck(dbMutex);

        auto& dbTopic = knownTopics_[topicName];

        dbTopic.subscribers.try_emplace(
                updatesEndPoint, topicName, updatesEndPoint);

        // Send all currently-existing publishers:
        send_topic_publishers_to_subscribed_clients(
                topicName, updatesEndPoint /*only this one*/);
}

void Server::send_topic_publishers_to_subscribed_clients(
        const std::string& topicName,
        const std::optional<std::string>& updatesEndPoint)
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        auto& dbTopic = knownTopics_[topicName];

        mvsim_msgs::TopicInfo tiMsg;
        tiMsg.set_topicname(topicName);
        tiMsg.set_topictype(dbTopic.topicTypeName);

        for (const auto& pub : dbTopic.publishers)
        {
                tiMsg.add_publishername(pub.second.publisherNodeName);
                tiMsg.add_publisherendpoint(pub.second.publisherEndpoint);
        }

        ASSERT_(mainThreadZMQcontext_);

        auto lambdaSendToSub = [&](const std::string& subUpdtEndPoint) {
                try
                {
                        MRPT_LOG_DEBUG_STREAM(
                                "[send_topic_publishers_to_subscribed_clients] Letting "
                                << subUpdtEndPoint << " know about "
                                << dbTopic.publishers.size() << " publishers for topic '"
                                << topicName << "'");

                        zmq::socket_t s(*mainThreadZMQcontext_, ZMQ_PAIR);
                        s.connect(subUpdtEndPoint);
#if CPPZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 7, 1)
                        ASSERT_(s);
#else
                        ASSERT_(s.connected());
#endif
                        sendMessage(tiMsg, s);

                        mvsim_msgs::GenericAnswer ans;
                        const auto m = receiveMessage(s);
                        mvsim::parseMessage(m, ans);
                        ASSERT_(ans.success());
                }
                catch (const std::exception& e)
                {
                        MRPT_LOG_ERROR_STREAM(
                                "Error sending topic updates to endpoint " << subUpdtEndPoint
                                                                                                                   << ":\n"
                                                                                                                   << e.what());
                }
        };

        if (updatesEndPoint.has_value())
        {
                // send to this one only:
                lambdaSendToSub(*updatesEndPoint);
        }
        else
        {
                // send to all:
                for (const auto& ips : dbTopic.subscribers)
                {
                        lambdaSendToSub(ips.second.subscriberUpdatesEndpoint);
                }
        }
#endif
}

void Server::db_advertise_service(
        const std::string& serviceName, const std::string& inputTypeName,
        const std::string& outputTypeName, const std::string& publisherEndpoint,
        const std::string& nodeName)
{
        std::unique_lock lck(dbMutex);

        // 1) Add as a source of this topic:
        auto& dbSrv = knownServices_[serviceName];

        if (!dbSrv.inputTypeName.empty() &&
                (dbSrv.inputTypeName != inputTypeName ||
                 dbSrv.outputTypeName != outputTypeName))
        {
                throw std::runtime_error(mrpt::format(
                        "Trying to register service `%s` [%s->%s] but already known "
                        "with "
                        "types "
                        "[%s->%s]",
                        serviceName.c_str(), inputTypeName.c_str(), outputTypeName.c_str(),
                        dbSrv.inputTypeName.c_str(), dbSrv.outputTypeName.c_str()));
        }
        dbSrv.serviceName = serviceName;
        dbSrv.inputTypeName = inputTypeName;
        dbSrv.outputTypeName = outputTypeName;
        dbSrv.endpoint = publisherEndpoint;
        dbSrv.nodeName = nodeName;
}

bool Server::db_get_service_info(
        const std::string& serviceName, std::string& publisherEndpoint,
        std::string& nodeName) const
{
        std::shared_lock lck(dbMutex);

        // 1) Add as a source of this topic:
        auto itSrv = knownServices_.find(serviceName);
        if (itSrv == knownServices_.end()) return false;

        auto& dbSrv = itSrv->second;

        publisherEndpoint = dbSrv.endpoint;
        nodeName = dbSrv.nodeName;

        return true;
}

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)

// mvsim_msgs::RegisterNodeRequest
void Server::handle(const mvsim_msgs::RegisterNodeRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG_STREAM(
                "Registering new node named '" << m.nodename() << "'");

        // Make sure we don't have already a node named like this:
        // Don't raise an error if the name was already registered, since it
        // might be that the same node disconnected and connected again:
        db_remove_node(m.nodename());

        db_register_node(m.nodename());

        mvsim_msgs::RegisterNodeAnswer rna;
        rna.set_success(true);
        mvsim::sendMessage(rna, s);
}

// mvsim_msgs::UnregisterNodeRequest
void Server::handle(
        const mvsim_msgs::UnregisterNodeRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG_STREAM("Unregistering node named '" << m.nodename() << "'");

        db_remove_node(m.nodename());

        mvsim_msgs::GenericAnswer rna;
        rna.set_success(true);
        mvsim::sendMessage(rna, s);
}

// mvsim_msgs::SubscribeRequest
void Server::handle(const mvsim_msgs::SubscribeRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG_STREAM(
                "Subscription request for topic " << m.topic() << "'");

        // Include in our DB of subscriptions:
        // This also sends the subcriber the list of existing endpoints it must
        // subscribe to:
        db_add_topic_subscriber(m.topic(), m.updatesendpoint());

        mvsim_msgs::SubscribeAnswer ans;
        ans.set_topic(m.topic());
        ans.set_success(true);
        mvsim::sendMessage(ans, s);
}

// mvsim_msgs::GetServiceInfoRequest
void Server::handle(
        const mvsim_msgs::GetServiceInfoRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG_STREAM(
                "GetServiceInfo request for service '" << m.servicename() << "'");

        mvsim_msgs::GetServiceInfoAnswer ans;
        std::string node, endpoint;

        if (db_get_service_info(m.servicename(), endpoint, node))
        {
                ans.set_success(true);
                ans.set_serviceendpoint(endpoint);
                ans.set_servicenodename(node);
        }
        else
        {
                ans.set_success(false);
                ans.set_errormessage(mrpt::format(
                        "Could not find service `%s`", m.servicename().c_str()));
        }

        mvsim::sendMessage(ans, s);
}

// mvsim_msgs::ListTopicsRequest
void Server::handle(const mvsim_msgs::ListTopicsRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG("Listing topics request");

        mvsim_msgs::ListTopicsAnswer ans;

        // Optional name filter:
        const auto& queryPrefix = m.topicstartswith();

        std::shared_lock lck(dbMutex);

        for (const auto& kv : knownTopics_)
        {
                const auto& t = kv.second;
                const auto& name = t.topicName;

                if (!queryPrefix.empty() ||
                        name.substr(0, queryPrefix.size()) == queryPrefix)
                {
                        auto tInfo = ans.add_topics();
                        tInfo->set_topicname(name);
                        tInfo->set_topictype(t.topicTypeName);

                        for (const auto& pubs : t.publishers)
                        {
                                tInfo->add_publishername(pubs.second.publisherNodeName);
                                tInfo->add_publisherendpoint(pubs.second.publisherEndpoint);
                        }
                }
        }
        mvsim::sendMessage(ans, s);
}

// mvsim_msgs::ListNodesRequest
void Server::handle(const mvsim_msgs::ListNodesRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG("Listing nodes request");

        // Optional name filter:
        const auto& queryPrefix = m.nodestartswith();

        mvsim_msgs::ListNodesAnswer ans;
        for (const auto& n : connectedNodes_)
        {
                const auto& name = n.second.nodeName;

                if (!queryPrefix.empty() ||
                        name.substr(0, queryPrefix.size()) == queryPrefix)
                {
                        ans.add_nodes(name);
                }
        }
        mvsim::sendMessage(ans, s);
}

// mvsim_msgs::AdvertiseTopicRequest
void Server::handle(
        const mvsim_msgs::AdvertiseTopicRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG_FMT(
                "Received new topic advertiser: `%s` [%s] @ %s (%s)",
                m.topicname().c_str(), m.topictypename().c_str(), m.endpoint().c_str(),
                m.nodename().c_str());

        mvsim_msgs::GenericAnswer ans;
        try
        {
                db_advertise_topic(
                        m.topicname(), m.topictypename(), m.endpoint(), m.nodename());
                ans.set_success(true);
        }
        catch (const std::exception& e)
        {
                ans.set_success(false);
                ans.set_errormessage(mrpt::exception_to_str(e));
        }
        mvsim::sendMessage(ans, s);
}

// mvsim_msgs::AdvertiseServiceRequest
void Server::handle(
        const mvsim_msgs::AdvertiseServiceRequest& m, zmq::socket_t& s)
{
        //  Send reply back to client
        MRPT_LOG_DEBUG_FMT(
                "Received new service offering: `%s` [%s->%s] @ %s (%s)",
                m.servicename().c_str(), m.inputtypename().c_str(),
                m.outputtypename().c_str(), m.endpoint().c_str(), m.nodename().c_str());

        mvsim_msgs::GenericAnswer ans;
        try
        {
                db_advertise_service(
                        m.servicename(), m.inputtypename(), m.outputtypename(),
                        m.endpoint(), m.nodename());
                ans.set_success(true);
        }
        catch (const std::exception& e)
        {
                ans.set_success(false);
                ans.set_errormessage(mrpt::exception_to_str(e));
        }
        mvsim::sendMessage(ans, s);
}

#endif