src/Comms/Client.cpp

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2024  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/core/lock_helper.h>
#include <mrpt/version.h>
#include <mvsim/Comms/Client.h>
#include <mvsim/Comms/common.h>
#include <mvsim/Comms/ports.h>
#include <mvsim/Comms/zmq_monitor.h>
#if MRPT_VERSION >= 0x204
#include <mrpt/system/thread_name.h>
#endif
 
#include <iostream>
#include <mutex>
#include <shared_mutex>
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
 
#include <google/protobuf/text_format.h>
#include <mvsim/mvsim-msgs/AdvertiseServiceRequest.pb.h>
#include <mvsim/mvsim-msgs/AdvertiseTopicRequest.pb.h>
#include <mvsim/mvsim-msgs/CallService.pb.h>
#include <mvsim/mvsim-msgs/GenericAnswer.pb.h>
#include <mvsim/mvsim-msgs/GetServiceInfoAnswer.pb.h>
#include <mvsim/mvsim-msgs/GetServiceInfoRequest.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;
 
#if defined(MVSIM_HAS_ZMQ)
namespace mvsim::internal
{
struct InfoPerAdvertisedTopic
{
        InfoPerAdvertisedTopic(zmq::context_t& c) : context(c) {}
 
        zmq::context_t& context;
 
        std::string topicName;
        zmq::socket_t pubSocket = zmq::socket_t(context, ZMQ_PUB);
        std::string endpoint;
        const google::protobuf::Descriptor* descriptor = nullptr;
};
 
struct InfoPerService
{
        InfoPerService() = default;
 
        std::string serviceName;
        const google::protobuf::Descriptor* descInput = nullptr;
        const google::protobuf::Descriptor* descOutput = nullptr;
        Client::service_callback_t callback;
};
struct InfoPerSubscribedTopic
{
        InfoPerSubscribedTopic(zmq::context_t& c) : context(c) {}
        ~InfoPerSubscribedTopic()
        {
                if (topicThread.joinable()) topicThread.join();
        }
 
        zmq::context_t& context;
 
        std::string topicName;
        zmq::socket_t subSocket = zmq::socket_t(context, ZMQ_SUB);
        const google::protobuf::Descriptor* descriptor = nullptr;
 
        std::vector<Client::topic_callback_t> callbacks;
 
        std::thread topicThread;
};
}  // namespace mvsim::internal
#endif
 
struct Client::ZMQImpl
{
#if defined(MVSIM_HAS_ZMQ)
        zmq::context_t context{2 /* io sockets */, ZMQ_MAX_SOCKETS_DFLT};
        std::optional<zmq::socket_t> mainReqSocket;
        std::recursive_mutex mainReqSocketMtx;
        mvsim::SocketMonitor mainReqSocketMonitor;
 
        std::map<std::string, internal::InfoPerAdvertisedTopic> advertisedTopics;
        std::shared_mutex advertisedTopics_mtx;
 
        std::optional<zmq::socket_t> srvListenSocket;
        std::map<std::string, internal::InfoPerService> offeredServices;
        std::shared_mutex offeredServices_mtx;
 
        std::map<std::string, internal::InfoPerSubscribedTopic> subscribedTopics;
        std::shared_mutex subscribedTopics_mtx;
 
        std::optional<zmq::socket_t> topicNotificationsSocket;
        std::string topicNotificationsEndPoint;
 
#endif
};
 
Client::Client()
        : mrpt::system::COutputLogger("mvsim::Client"), zmq_(std::make_unique<Client::ZMQImpl>())
{
}
Client::Client(const std::string& nodeName) : Client() { setName(nodeName); }
 
Client::~Client() { shutdown(); }
 
void Client::setName(const std::string& nodeName) { nodeName_ = nodeName; }
 
bool Client::connected() const
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        return zmq_->mainReqSocketMonitor.connected();
#else
        return false;
#endif
}
 
void Client::connect()
{
        using namespace std::string_literals;
        ASSERTMSG_(!zmq_->mainReqSocket || !zmq_->mainReqSocket, "Client is already running.");
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
 
        mrpt::system::CTimeLoggerEntry tle(profiler_, "connect");
 
        auto lck = mrpt::lockHelper(zmq_->mainReqSocketMtx);
 
        zmq_->mainReqSocket.emplace(zmq_->context, ZMQ_REQ);
 
        // Monitor to listen on ZMQ socket events:
        zmq_->mainReqSocketMonitor.monitor(zmq_->mainReqSocket.value());
 
        zmq_->mainReqSocket->connect(
                "tcp://"s + serverHostAddress_ + ":"s + std::to_string(MVSIM_PORTNO_MAIN_REP));
 
        // Let the server know about this new node:
        doRegisterClient();
 
        // Create listening socket for services:
        zmq_->srvListenSocket.emplace(zmq_->context, ZMQ_REP);
        zmq_->srvListenSocket->bind("tcp://0.0.0.0:*"s);
 
        if (!zmq_->srvListenSocket) THROW_EXCEPTION("Error binding service listening socket.");
 
        ASSERTMSG_(!serviceInvokerThread_.joinable(), "Client service thread is already running!");
 
        serviceInvokerThread_ = std::thread(&Client::internalServiceServingThread, this);
#if MRPT_VERSION >= 0x204
        mrpt::system::thread_name("services_"s + nodeName_, serviceInvokerThread_);
#endif
 
        // Create listening socket for subscription updates:
        zmq_->topicNotificationsSocket.emplace(zmq_->context, ZMQ_PAIR);
        zmq_->topicNotificationsSocket->bind("tcp://0.0.0.0:*"s);
 
        if (!zmq_->topicNotificationsSocket)
                THROW_EXCEPTION("Error binding topic updates listening socket.");
 
        zmq_->topicNotificationsEndPoint = get_zmq_endpoint(*zmq_->topicNotificationsSocket);
 
        ASSERTMSG_(!topicUpdatesThread_.joinable(), "Client topic updates thread is already running!");
 
        topicUpdatesThread_ = std::thread(&Client::internalTopicUpdatesThread, this);
#if MRPT_VERSION >= 0x204
        mrpt::system::thread_name("topicUpdates_"s + nodeName_, topicUpdatesThread_);
#endif
 
#else
        THROW_EXCEPTION(
                "MVSIM needs building with ZMQ and PROTOBUF to enable "
                "client/server");
#endif
}
 
void Client::shutdown() noexcept
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "shutdown");
 
        auto lck = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        if (!zmq_->mainReqSocket) return;
 
        try
        {
                MRPT_LOG_DEBUG_STREAM("Unregistering from server.");
                doUnregisterClient();
        }
        catch (const std::exception& e)
        {
                MRPT_LOG_ERROR_STREAM("shutdown: Exception: " << mrpt::exception_to_str(e));
        }
 
#if CPPZMQ_VERSIONZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 4, 0)
        zmq_->context.shutdown();
#else
        // Missing shutdown() in older versions:
        zmq_ctx_shutdown(zmq_->context.operator void*());
#endif
 
        if (serviceInvokerThread_.joinable()) serviceInvokerThread_.join();
        if (topicUpdatesThread_.joinable()) topicUpdatesThread_.join();
        zmq_->subscribedTopics.clear();
        zmq_->offeredServices.clear();
 
#endif
}
 
void Client::doRegisterClient()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "doRegisterClient");
 
        auto lck = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        auto& s = *zmq_->mainReqSocket;
 
        mvsim_msgs::RegisterNodeRequest rnq;
        rnq.set_nodename(nodeName_);
        mvsim::sendMessage(rnq, s);
 
        //  Get the reply.
        const zmq::message_t reply = mvsim::receiveMessage(s);
 
        mvsim_msgs::RegisterNodeAnswer rna;
        mvsim::parseMessage(reply, rna);
        if (!rna.success())
        {
                THROW_EXCEPTION_FMT(
                        "Server did not allow registering node: %s", rna.errormessage().c_str());
        }
        MRPT_LOG_DEBUG("Successfully registered in the server.");
#else
        THROW_EXCEPTION("MVSIM built without ZMQ");
#endif
}
 
void Client::doUnregisterClient()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "doUnregisterClient");
 
        auto lck = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        if (!zmq_->mainReqSocket) return;
        auto& s = *zmq_->mainReqSocket;
 
        mvsim_msgs::UnregisterNodeRequest rnq;
        rnq.set_nodename(nodeName_);
        mvsim::sendMessage(rnq, s);
 
        //  Get the reply.
        const zmq::message_t reply = mvsim::receiveMessage(s);
 
        mvsim_msgs::GenericAnswer rna;
        mvsim::parseMessage(reply, rna);
        if (!rna.success())
        {
                THROW_EXCEPTION_FMT(
                        "Server answered an error unregistering node: %s", rna.errormessage().c_str());
        }
        MRPT_LOG_DEBUG("Successfully unregistered in the server.");
#else
        THROW_EXCEPTION("MVSIM built without ZMQ");
#endif
}
 
std::vector<Client::InfoPerNode> Client::requestListOfNodes()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "requestListOfNodes");
 
        auto lck = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        auto& s = *zmq_->mainReqSocket;
 
        mvsim_msgs::ListNodesRequest req;
        mvsim::sendMessage(req, s);
 
        //  Get the reply.
        const zmq::message_t reply = mvsim::receiveMessage(s);
 
        mvsim_msgs::ListNodesAnswer lna;
        mvsim::parseMessage(reply, lna);
 
        std::vector<Client::InfoPerNode> nodes;
        nodes.resize(lna.nodes_size());
 
        for (int i = 0; i < lna.nodes_size(); i++)
        {
                nodes[i].name = lna.nodes(i);
        }
        return nodes;
#else
        THROW_EXCEPTION("MVSIM built without ZMQ");
#endif
}
 
std::vector<Client::InfoPerTopic> Client::requestListOfTopics()
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "requestListOfTopics");
        auto lck = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        auto& s = *zmq_->mainReqSocket;
 
        mvsim_msgs::ListTopicsRequest req;
        mvsim::sendMessage(req, s);
 
        //  Get the reply.
        const zmq::message_t reply = mvsim::receiveMessage(s);
 
        mvsim_msgs::ListTopicsAnswer lta;
        mvsim::parseMessage(reply, lta);
 
        std::vector<Client::InfoPerTopic> topics;
        topics.resize(lta.topics_size());
 
        for (int i = 0; i < lta.topics_size(); i++)
        {
                const auto& t = lta.topics(i);
                auto& dst = topics[i];
 
                dst.name = t.topicname();
                dst.type = t.topictype();
 
                ASSERT_EQUAL_(t.publisherendpoint_size(), t.publishername_size());
                dst.endpoints.resize(t.publisherendpoint_size());
                dst.publishers.resize(t.publisherendpoint_size());
 
                for (int k = 0; k < t.publisherendpoint_size(); k++)
                {
                        dst.publishers[k] = t.publishername(k);
                        dst.endpoints[k] = t.publisherendpoint(k);
                }
        }
        return topics;
#else
        THROW_EXCEPTION("MVSIM built without ZMQ");
#endif
}
 
void Client::doAdvertiseTopic(
        const std::string& topicName, const google::protobuf::Descriptor* descriptor)
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "doAdvertiseTopic");
        auto& advTopics = zmq_->advertisedTopics;
 
        std::unique_lock<std::shared_mutex> lck(zmq_->advertisedTopics_mtx);
 
        if (advTopics.find(topicName) != advTopics.end())
                THROW_EXCEPTION_FMT(
                        "Topic `%s` already registered for publication in this same "
                        "client (!)",
                        topicName.c_str());
 
        // the ctor of InfoPerAdvertisedTopic automatically creates a ZMQ_PUB
        // socket in pubSocket
        internal::InfoPerAdvertisedTopic& ipat =
                advTopics.emplace_hint(advTopics.begin(), topicName, zmq_->context)->second;
 
        lck.unlock();
 
        // Bind the PUBLISH socket:
        ipat.pubSocket.bind("tcp://0.0.0.0:*");
 
#if CPPZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 7, 1)
        if (!ipat.pubSocket)
#else
        if (!ipat.pubSocket.connected())
#endif
        {
                THROW_EXCEPTION("Could not bind publisher socket");
        }
 
        // Retrieve assigned TCP port:
        ipat.endpoint = get_zmq_endpoint(ipat.pubSocket);
        ipat.topicName = topicName;      // redundant in container, but handy.
        ipat.descriptor = descriptor;
 
        MRPT_LOG_DEBUG_FMT(
                "Advertising topic `%s` [%s] on endpoint `%s`", topicName.c_str(),
                descriptor->full_name().c_str(), ipat.endpoint.c_str());
 
        // MRPT_LOG_INFO_STREAM("Type: " << descriptor->DebugString());
 
        mvsim_msgs::AdvertiseTopicRequest req;
        req.set_topicname(ipat.topicName);
        req.set_endpoint(ipat.endpoint);
        req.set_topictypename(ipat.descriptor->full_name());
        req.set_nodename(nodeName_);
 
        auto lckMain = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        mvsim::sendMessage(req, *zmq_->mainReqSocket);
 
        //  Get the reply.
        const zmq::message_t reply = mvsim::receiveMessage(*zmq_->mainReqSocket);
        lckMain.unlock();
 
        mvsim_msgs::GenericAnswer ans;
        mvsim::parseMessage(reply, ans);
 
        if (!ans.success())
                THROW_EXCEPTION_FMT(
                        "Error registering topic `%s` in server: `%s`", topicName.c_str(),
                        ans.errormessage().c_str());
 
#else
        THROW_EXCEPTION("MVSIM built without ZMQ & PROTOBUF");
#endif
}
 
void Client::doAdvertiseService(
        const std::string& serviceName, const google::protobuf::Descriptor* descIn,
        const google::protobuf::Descriptor* descOut, service_callback_t callback)
{
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "doAdvertiseService");
        std::unique_lock<std::shared_mutex> lck(zmq_->offeredServices_mtx);
 
        auto& services = zmq_->offeredServices;
 
        if (services.find(serviceName) != services.end())
                THROW_EXCEPTION_FMT(
                        "Service `%s` already registered in this same client!", serviceName.c_str());
 
        internal::InfoPerService& ips = services[serviceName];
 
        lck.unlock();
 
        // Retrieve assigned TCP port:
        const auto assignedPort = mvsim::get_zmq_endpoint(*zmq_->srvListenSocket);
 
        ips.serviceName = serviceName;  // redundant in container, but handy.
        ips.callback = callback;
        ips.descInput = descIn;
        ips.descOutput = descOut;
 
        MRPT_LOG_DEBUG_FMT(
                "Advertising service `%s` [%s->%s] on endpoint `%s`", serviceName.c_str(),
                descIn->full_name().c_str(), descOut->full_name().c_str(), assignedPort.c_str());
 
        mvsim_msgs::AdvertiseServiceRequest req;
        req.set_servicename(ips.serviceName);
        req.set_endpoint(assignedPort);
        req.set_inputtypename(ips.descInput->full_name());
        req.set_outputtypename(ips.descOutput->full_name());
        req.set_nodename(nodeName_);
 
        auto lckMain = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        mvsim::sendMessage(req, *zmq_->mainReqSocket);
 
        //  Get the reply.
        const zmq::message_t reply = mvsim::receiveMessage(*zmq_->mainReqSocket);
        lckMain.unlock();
        mvsim_msgs::GenericAnswer ans;
        mvsim::parseMessage(reply, ans);
 
        if (!ans.success())
                THROW_EXCEPTION_FMT(
                        "Error registering service `%s` in server: `%s`", serviceName.c_str(),
                        ans.errormessage().c_str());
 
#else
        THROW_EXCEPTION("MVSIM built without ZMQ & PROTOBUF");
#endif
}
 
void Client::publishTopic(const std::string& topicName, const google::protobuf::Message& msg)
{
        MRPT_START
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        ASSERTMSG_(
                zmq_ && zmq_->mainReqSocket && zmq_->mainReqSocket, "Client not connected to Server");
        mrpt::system::CTimeLoggerEntry tle(profiler_, "publishTopic");
 
        std::shared_lock<std::shared_mutex> lck(zmq_->advertisedTopics_mtx);
        auto itIpat = zmq_->advertisedTopics.find(topicName);
 
        ASSERTMSG_(
                itIpat != zmq_->advertisedTopics.end(),
                mrpt::format(
                        "Topic `%s` has not been registered. Missing former call to "
                        "advertiseTopic()?",
                        topicName.c_str()));
 
        lck.unlock();
 
        auto& ipat = itIpat->second;
 
        ASSERTMSG_(
                msg.GetDescriptor() == ipat.descriptor,
                mrpt::format(
                        "Topic `%s` has type `%s`, but expected `%s` from former call to "
                        "advertiseTopic()?",
                        topicName.c_str(), msg.GetDescriptor()->name().c_str(),
                        ipat.descriptor->name().c_str()));
 
#if CPPZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 7, 1)
        ASSERT_(ipat.pubSocket);
#else
        ASSERT_(ipat.pubSocket.connected());
#endif
 
        mvsim::sendMessage(msg, ipat.pubSocket);
 
#else
        THROW_EXCEPTION("MVSIM built without ZMQ & PROTOBUF");
#endif
        MRPT_END
}
 
void Client::internalServiceServingThread()
{
        using namespace std::string_literals;
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        try
        {
                MRPT_LOG_INFO_STREAM("[" << nodeName_ << "] Client service thread started.");
 
                zmq::socket_t& s = *zmq_->srvListenSocket;
 
                for (;;)
                {
                        //  Wait for next request from client:
                        zmq::message_t m = mvsim::receiveMessage(s);
 
                        // parse it:
                        mvsim_msgs::CallService csMsg;
                        mvsim::parseMessage(m, csMsg);
 
                        std::shared_lock<std::shared_mutex> lck(zmq_->offeredServices_mtx);
                        const auto& srvName = csMsg.servicename();
 
                        auto itSrv = zmq_->offeredServices.find(srvName);
                        if (itSrv == zmq_->offeredServices.end())
                        {
                                // Error: unknown service:
                                mvsim_msgs::GenericAnswer ans;
                                ans.set_success(false);
                                ans.set_errormessage(
                                        mrpt::format("Requested unknown service `%s`", srvName.c_str()));
                                MRPT_LOG_ERROR_STREAM(ans.errormessage());
 
                                mvsim::sendMessage(ans, s);
                                continue;
                        }
 
                        internal::InfoPerService& ips = itSrv->second;
 
                        // MRPT_TODO("Check input descriptor?");
 
                        auto outMsgPtr = ips.callback(csMsg.serializedinput());
 
                        // Send response:
                        mvsim::sendMessage(*outMsgPtr, s);
                }
        }
        catch (const zmq::error_t& e)
        {
                if (e.num() == ETERM)
                {
                        // This simply means someone called
                        // requestMainThreadTermination(). Just exit silently.
                        MRPT_LOG_DEBUG_STREAM(
                                "internalServiceServingThread about to exit for ZMQ term "
                                "signal.");
                }
                else
                {
                        MRPT_LOG_ERROR_STREAM("internalServiceServingThread: ZMQ error: " << e.what());
                }
        }
        catch (const std::exception& e)
        {
                MRPT_LOG_ERROR_STREAM(
                        "internalServiceServingThread: Exception: " << mrpt::exception_to_str(e));
        }
        MRPT_LOG_DEBUG_STREAM("internalServiceServingThread quitted.");
 
#endif
}
 
void Client::internalTopicUpdatesThread()
{
        using namespace std::string_literals;
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        try
        {
                MRPT_LOG_DEBUG_STREAM("[" << nodeName_ << "] Client topic updates thread started.");
 
                zmq::socket_t& s = *zmq_->topicNotificationsSocket;
 
                for (;;)
                {
                        //  Wait for next update from server:
                        zmq::message_t m = mvsim::receiveMessage(s);
 
                        // parse it:
                        mvsim_msgs::TopicInfo tiMsg;
                        mvsim::parseMessage(m, tiMsg);
 
                        // Let the server know that we received this:
                        mvsim_msgs::GenericAnswer ans;
                        ans.set_success(true);
                        mvsim::sendMessage(ans, s);
 
                        // We got a message. This means we have to new endpoints to
                        // subscribe, either because we have just subscribed to a new topic,
                        // or a new node has advertised a topic we already subscribed in the
                        // past.
 
                        // Look for the entry in the list of subscribed topics:
                        std::unique_lock<std::shared_mutex> lck(zmq_->subscribedTopics_mtx);
                        const auto& topicName = tiMsg.topicname();
 
                        auto itTopic = zmq_->subscribedTopics.find(topicName);
                        if (itTopic == zmq_->subscribedTopics.end())
                        {
                                // This shouldn't happen (?).
                                MRPT_LOG_WARN_STREAM(
                                        "Received a topic `" << topicName
                                                                                 << "` update message from server, but this node is not "
                                                                                        "subscribed to it (!).");
                                continue;
                        }
 
                        MRPT_LOG_DEBUG_STREAM("[internalTopicUpdatesThread] Received: " << tiMsg.DebugString());
 
                        internal::InfoPerSubscribedTopic& ipt = itTopic->second;
 
                        for (int i = 0; i < tiMsg.publisherendpoint_size(); i++)
                        {
                                ipt.subSocket.connect(tiMsg.publisherendpoint(i));
                        }
 
                        // No need to send response back to server.
                }
        }
        catch (const zmq::error_t& e)
        {
                if (e.num() == ETERM)
                {
                        // This simply means someone called
                        // requestMainThreadTermination(). Just exit silently.
                        MRPT_LOG_DEBUG_STREAM(
                                "internalTopicUpdatesThread about to exit for ZMQ term "
                                "signal.");
                }
                else
                {
                        MRPT_LOG_ERROR_STREAM("internalTopicUpdatesThread: ZMQ error: " << e.what());
                }
        }
        catch (const std::exception& e)
        {
                MRPT_LOG_ERROR_STREAM(
                        "internalTopicUpdatesThread: Exception: " << mrpt::exception_to_str(e));
        }
        MRPT_LOG_DEBUG_STREAM("[" << nodeName_ << "] Client topic updates thread quitted.");
 
#endif
}
 
// Overload for python wrapper
std::string Client::callService(
        const std::string& serviceName, const std::string& inputSerializedMsg)
{
        MRPT_START
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "callService");
 
        std::string outMsgData, outMsgType;
        doCallService(serviceName, inputSerializedMsg, std::nullopt, outMsgData, outMsgType);
        return outMsgData;
#endif
        MRPT_END
}
void Client::subscribeTopic(
        const std::string& topicName,
        const std::function<void(
                const std::string& /*msgType*/, const std::vector<uint8_t>& /*serializedMsg*/)>& callback)
{
        MRPT_START
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
 
        subscribe_topic_raw(
                topicName,
                [callback](const zmq::message_t& m)
                {
                        const auto [sType, sData] = mvsim::internal::parseMessageToParts(m);
                        std::vector<uint8_t> d(sData.size());
                        ::memcpy(d.data(), sData.data(), sData.size());
                        callback(sType, d);
                });
#endif
        MRPT_END
}
 
void Client::doCallService(
        const std::string& serviceName, const std::string& inputSerializedMsg,
        mrpt::optional_ref<google::protobuf::Message> outputMsg,
        mrpt::optional_ref<std::string> outputSerializedMsg,
        mrpt::optional_ref<std::string> outputMsgTypeName)
{
        MRPT_START
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "doCallService");
 
        // 1) Request to the server who is serving this service:
        std::string srvEndpoint;
 
        auto lckCache = mrpt::lockHelper(serviceToEndPointCacheMtx_);
        if (auto it = serviceToEndPointCache_.find(serviceName); it != serviceToEndPointCache_.end())
        {
                srvEndpoint = it->second;
        }
        else
        {
                mrpt::system::CTimeLoggerEntry tle2(profiler_, "doCallService.getinfo");
 
                auto lckMain = mrpt::lockHelper(zmq_->mainReqSocketMtx);
                zmq::socket_t& s = *zmq_->mainReqSocket;
 
                mvsim_msgs::GetServiceInfoRequest gsi;
                gsi.set_servicename(serviceName);
                mvsim::sendMessage(gsi, s);
 
                auto m = mvsim::receiveMessage(s);
                mvsim_msgs::GetServiceInfoAnswer gsia;
                mvsim::parseMessage(m, gsia);
 
                if (!gsia.success())
                        THROW_EXCEPTION_FMT(
                                "Error requesting information about service `%s`: %s", serviceName.c_str(),
                                gsia.errormessage().c_str());
 
                srvEndpoint = gsia.serviceendpoint();
 
                serviceToEndPointCache_[serviceName] = srvEndpoint;
        }
        lckCache.unlock();
 
        // 2) Connect to the service offerrer and request the execution:
        zmq::socket_t srvReqSock(zmq_->context, ZMQ_REQ);
        srvReqSock.connect(srvEndpoint);
 
        mvsim_msgs::CallService csMsg;
        csMsg.set_servicename(serviceName);
        csMsg.set_serializedinput(inputSerializedMsg);
 
        mvsim::sendMessage(csMsg, srvReqSock);
 
        const auto m = mvsim::receiveMessage(srvReqSock);
        if (outputMsg)
        {
                mvsim::parseMessage(m, outputMsg.value().get());
        }
        if (outputSerializedMsg)
        {
                const auto [typeName, serializedData] = internal::parseMessageToParts(m);
 
                outputSerializedMsg.value().get() = serializedData;
                if (outputMsgTypeName) outputMsgTypeName.value().get() = typeName;
        }
#endif
        MRPT_END
}
 
void Client::subscribe_topic_raw(const std::string& topicName, const topic_callback_t& callback)
{
        doSubscribeTopic(topicName, nullptr, callback);
}
 
void Client::doSubscribeTopic(
        const std::string& topicName, [[maybe_unused]] const google::protobuf::Descriptor* descriptor,
        const topic_callback_t& callback)
{
        MRPT_START
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mrpt::system::CTimeLoggerEntry tle(profiler_, "doSubscribeTopic");
        // Register in my internal DB:
        std::unique_lock<std::shared_mutex> lck(zmq_->subscribedTopics_mtx);
 
        auto& topics = zmq_->subscribedTopics;
 
        // It's ok to subscribe more than once:
        internal::InfoPerSubscribedTopic& ipt =
                topics.emplace_hint(topics.begin(), topicName, zmq_->context)->second;
 
        // subscribe to .recv() any message:
#if CPPZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 7, 1)
        ipt.subSocket.set(zmq::sockopt::subscribe, "");
#else
        ipt.subSocket.setsockopt(ZMQ_SUBSCRIBE, "", 0);
#endif
 
        ipt.callbacks.push_back(callback);
 
        ipt.topicName = topicName;
 
        lck.unlock();
 
        ipt.topicThread = std::thread([&]() { this->internalTopicSubscribeThread(ipt); });
 
        // Let the server know about our interest in the topic:
        mvsim_msgs::SubscribeRequest subReq;
        subReq.set_topic(topicName);
        subReq.set_updatesendpoint(zmq_->topicNotificationsEndPoint);
 
        auto lckMain = mrpt::lockHelper(zmq_->mainReqSocketMtx);
        mvsim::sendMessage(subReq, *zmq_->mainReqSocket);
 
        const auto m = mvsim::receiveMessage(*zmq_->mainReqSocket);
        lckMain.unlock();
        mvsim_msgs::SubscribeAnswer subAns;
        mvsim::parseMessage(m, subAns);
 
        ASSERT_EQUAL_(subAns.topic(), topicName);
        ASSERT_(subAns.success());
 
        // That is... the rest will be done upon reception of messages from the
        // server on potential clients we should subscribe to.
 
#endif
        MRPT_END
}
 
void Client::internalTopicSubscribeThread(internal::InfoPerSubscribedTopic& ipt)
{
        using namespace std::string_literals;
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        try
        {
                MRPT_LOG_DEBUG_STREAM(
                        "[" << nodeName_ << "] Client topic subscribe thread for `" << ipt.topicName
                                << "` started.");
 
                zmq::socket_t& s = ipt.subSocket;
 
                for (;;)
                {
                        //  Wait for next update from server:
                        const zmq::message_t m = mvsim::receiveMessage(s);
 
                        // Send to subscriber callbacks:
                        try
                        {
                                for (const auto& callback : ipt.callbacks) callback(m);
                        }
                        catch (const std::exception& e)
                        {
                                MRPT_LOG_ERROR_STREAM(
                                        "Exception in topic `"
                                        << ipt.topicName << "` subscription callback:" << mrpt::exception_to_str(e));
                        }
                }
        }
        catch (const zmq::error_t& e)
        {
                if (e.num() == ETERM)
                {
                        // This simply means someone called
                        // requestMainThreadTermination(). Just exit silently.
                        MRPT_LOG_DEBUG_STREAM(
                                "[" << nodeName_
                                        << "] Client topic subscribe thread about to exit for ZMQ "
                                           "term signal.");
                }
                else
                {
                        MRPT_LOG_ERROR_STREAM("internalTopicSubscribeThread: ZMQ error: " << e.what());
                }
        }
        catch (const std::exception& e)
        {
                MRPT_LOG_ERROR_STREAM(
                        "internalTopicSubscribeThread: Exception: " << mrpt::exception_to_str(e));
        }
        MRPT_LOG_DEBUG_STREAM("[" << nodeName_ << "] Client topic subscribe thread quitted.");
 
#endif
}