gz/transport/Discovery.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
 
#ifndef GZ_TRANSPORT_DISCOVERY_HH_
#define GZ_TRANSPORT_DISCOVERY_HH_
#include <errno.h>
#include <string.h>
 
#ifdef _WIN32
  // For socket(), connect(), send(), and recv().
  #include <Winsock2.h>
  #include <Ws2def.h>
  #include <Ws2ipdef.h>
  #include <Ws2tcpip.h>
  // Type used for raw data on this platform.
  using raw_type = char;
#else
  // For data types
  #include <sys/types.h>
  // For socket(), connect(), send(), and recv()
  #include <sys/socket.h>
  // For gethostbyname()
  #include <netdb.h>
  // For inet_addr()
  #include <arpa/inet.h>
  // For close()
  #include <unistd.h>
  // For sockaddr_in
  #include <netinet/in.h>
  // Type used for raw data on this platform
  using raw_type = void;
#endif
 
#ifdef _WIN32
  #pragma warning(push, 0)
#endif
#ifdef _WIN32
  #pragma warning(pop)
  // Suppress "decorated name length exceed" warning in STL.
  #pragma warning(disable: 4503)
  // Suppress "depreted API warnings" in WINSOCK.
  #pragma warning(disable: 4996)
#endif
 
#include <gz/msgs/discovery.pb.h>
 
#include <algorithm>
#include <condition_variable>
#include <limits>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
 
#include <gz/msgs/Utility.hh>
 
#include "gz/transport/config.hh"
#include "gz/transport/Export.hh"
#include "gz/transport/Helpers.hh"
#include "gz/transport/NetUtils.hh"
#include "gz/transport/Publisher.hh"
#include "gz/transport/TopicStorage.hh"
#include "gz/transport/TransportTypes.hh"
 
namespace ignition
{
  namespace transport
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_TRANSPORT_VERSION_NAMESPACE {
    enum class DestinationType
    {
      UNICAST,
      MULTICAST,
      ALL
    };
 
    //
    bool IGNITION_TRANSPORT_VISIBLE pollSockets(
      const std::vector<int> &_sockets,
      const int _timeout);
 
    template<typename Pub>
    class Discovery
    {
      public: Discovery(const std::string &_pUuid,
                        const int _port,
                        const bool _verbose = false)
        : port(_port),
          hostAddr(determineHost()),
          pUuid(_pUuid),
          silenceInterval(kDefSilenceInterval),
          activityInterval(kDefActivityInterval),
          heartbeatInterval(kDefHeartbeatInterval),
          connectionCb(nullptr),
          disconnectionCb(nullptr),
          verbose(_verbose),
          initialized(false),
          numHeartbeatsUninitialized(0),
          exit(false),
          enabled(false)
      {
        std::string ignIp;
        if (env("IGN_IP", ignIp) && !ignIp.empty())
          this->hostInterfaces = {ignIp};
        else
        {
          // Get the list of network interfaces in this host.
          this->hostInterfaces = determineInterfaces();
        }
 
#ifdef _WIN32
        WORD wVersionRequested;
        WSADATA wsaData;
 
        // Request WinSock v2.2.
        wVersionRequested = MAKEWORD(2, 2);
        // Load WinSock DLL.
        if (WSAStartup(wVersionRequested, &wsaData) != 0)
        {
          std::cerr << "Unable to load WinSock DLL" << std::endl;
          return;
        }
#endif
        for (const auto &netIface : this->hostInterfaces)
        {
          auto succeed = this->RegisterNetIface(netIface);
 
          // If the IP address that we're selecting as the main IP address of
          // the host is invalid, we change it to 127.0.0.1 .
          // This is probably because IGN_IP is set to a wrong value.
          if (netIface == this->hostAddr && !succeed)
          {
            this->RegisterNetIface("127.0.0.1");
            std::cerr << "Did you set the environment variable IGN_IP with a "
                      << "correct IP address? " << std::endl
                      << "  [" << netIface << "] seems an invalid local IP "
                      << "address." << std::endl
                      << "  Using 127.0.0.1 as hostname." << std::endl;
            this->hostAddr = "127.0.0.1";
          }
        }
 
        // Socket option: SO_REUSEADDR. This options is used only for receiving
        // data. We can reuse the same socket for receiving multicast data from
        // multiple interfaces. We will use the socket at position 0 for
        // receiving data.
        int reuseAddr = 1;
        if (setsockopt(this->sockets.at(0), SOL_SOCKET, SO_REUSEADDR,
            reinterpret_cast<const char *>(&reuseAddr), sizeof(reuseAddr)) != 0)
        {
          std::cerr << "Error setting socket option (SO_REUSEADDR)."
                    << std::endl;
          return;
        }
 
#ifdef SO_REUSEPORT
        // Socket option: SO_REUSEPORT. This options is used only for receiving
        // data. We can reuse the same socket for receiving multicast data from
        // multiple interfaces. We will use the socket at position 0 for
        // receiving data.
        int reusePort = 1;
        // cppcheck-suppress ConfigurationNotChecked
        if (setsockopt(this->sockets.at(0), SOL_SOCKET, SO_REUSEPORT,
            reinterpret_cast<const char *>(&reusePort), sizeof(reusePort)) != 0)
        {
          std::cerr << "Error setting socket option (SO_REUSEPORT)."
                    << std::endl;
          return;
        }
#endif
        // Bind the first socket to the discovery port.
        sockaddr_in localAddr;
        memset(&localAddr, 0, sizeof(localAddr));
        localAddr.sin_family = AF_INET;
        localAddr.sin_addr.s_addr = htonl(INADDR_ANY);
        localAddr.sin_port = htons(static_cast<u_short>(this->port));
 
        if (bind(this->sockets.at(0),
          reinterpret_cast<sockaddr *>(&localAddr), sizeof(sockaddr_in)) < 0)
        {
          std::cerr << "Binding to a local port failed." << std::endl;
          return;
        }
 
        // Set 'mcastAddr' to the multicast discovery group.
        memset(&this->mcastAddr, 0, sizeof(this->mcastAddr));
        this->mcastAddr.sin_family = AF_INET;
        this->mcastAddr.sin_addr.s_addr =
          inet_addr(this->kMulticastGroup.c_str());
        this->mcastAddr.sin_port = htons(static_cast<u_short>(this->port));
 
        std::vector<std::string> relays;
        std::string ignRelay = "";
        if (env("IGN_RELAY", ignRelay) && !ignRelay.empty())
        {
          relays = transport::split(ignRelay, ':');
        }
 
        // Register all unicast relays.
        for (auto const &relayAddr : relays)
          this->AddRelayAddress(relayAddr);
 
        if (this->verbose)
          this->PrintCurrentState();
      }
 
      public: virtual ~Discovery()
      {
        // Tell the service thread to terminate.
        this->exitMutex.lock();
        this->exit = true;
        this->exitMutex.unlock();
 
        // Wait for the service threads to finish before exit.
        if (this->threadReception.joinable())
          this->threadReception.join();
 
        // Broadcast a BYE message to trigger the remote cancellation of
        // all our advertised topics.
        this->SendMsg(DestinationType::ALL, msgs::Discovery::BYE,
          Publisher("", "", this->pUuid, "", AdvertiseOptions()));
 
        // Close sockets.
        for (const auto &sock : this->sockets)
        {
#ifdef _WIN32
          closesocket(sock);
          WSACleanup();
#else
          close(sock);
#endif
        }
      }
 
      public: void Start()
      {
        {
          std::lock_guard<std::mutex> lock(this->mutex);
 
          // The service is already running.
          if (this->enabled)
            return;
 
          this->enabled = true;
        }
 
        auto now = std::chrono::steady_clock::now();
        this->timeNextHeartbeat = now;
        this->timeNextActivity = now;
 
        // Start the thread that receives discovery information.
        this->threadReception = std::thread(&Discovery::RecvMessages, this);
      }
 
      public: bool Advertise(const Pub &_publisher)
      {
        {
          std::lock_guard<std::mutex> lock(this->mutex);
 
          if (!this->enabled)
            return false;
 
          // Add the addressing information (local publisher).
          if (!this->info.AddPublisher(_publisher))
            return false;
        }
 
        // Only advertise a message outside this process if the scope
        // is not 'Process'
        if (_publisher.Options().Scope() != Scope_t::PROCESS)
          this->SendMsg(DestinationType::ALL, msgs::Discovery::ADVERTISE,
              _publisher);
 
        return true;
      }
 
      public: bool Discover(const std::string &_topic) const
      {
        DiscoveryCallback<Pub> cb;
        bool found;
        Addresses_M<Pub> addresses;
 
        {
          std::lock_guard<std::mutex> lock(this->mutex);
 
          if (!this->enabled)
            return false;
 
          cb = this->connectionCb;
        }
 
        Pub pub;
        pub.SetTopic(_topic);
        pub.SetPUuid(this->pUuid);
 
        // Send a discovery request.
        this->SendMsg(DestinationType::ALL, msgs::Discovery::SUBSCRIBE, pub);
 
        {
          std::lock_guard<std::mutex> lock(this->mutex);
          found = this->info.Publishers(_topic, addresses);
        }
 
        if (found)
        {
          // I already have information about this topic.
          for (const auto &proc : addresses)
          {
            for (const auto &node : proc.second)
            {
              if (cb)
              {
                // Execute the user's callback for a service request. Notice
                // that we only execute one callback for preventing receive
                //  multiple service responses for a single request.
                cb(node);
              }
            }
          }
        }
 
        return true;
      }
 
      public: void Register(const MessagePublisher &_pub) const
      {
        this->SendMsg(
          DestinationType::ALL, msgs::Discovery::NEW_CONNECTION, _pub);
      }
 
      public: void Unregister(const MessagePublisher &_pub) const
      {
        this->SendMsg(
          DestinationType::ALL, msgs::Discovery::END_CONNECTION, _pub);
      }
 
      public: const TopicStorage<Pub> &Info() const
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        return this->info;
      }
 
      public: bool Publishers(const std::string &_topic,
                              Addresses_M<Pub> &_publishers) const
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        return this->info.Publishers(_topic, _publishers);
      }
 
      public: bool Unadvertise(const std::string &_topic,
                               const std::string &_nUuid)
      {
        Pub inf;
        {
          std::lock_guard<std::mutex> lock(this->mutex);
 
          if (!this->enabled)
            return false;
 
          // Don't do anything if the topic is not advertised by any of my nodes
          if (!this->info.Publisher(_topic, this->pUuid, _nUuid, inf))
            return true;
 
          // Remove the topic information.
          this->info.DelPublisherByNode(_topic, this->pUuid, _nUuid);
        }
 
        // Only unadvertise a message outside this process if the scope
        // is not 'Process'.
        if (inf.Options().Scope() != Scope_t::PROCESS)
        {
          this->SendMsg(DestinationType::ALL,
              msgs::Discovery::UNADVERTISE, inf);
        }
 
        return true;
      }
 
      public: std::string HostAddr() const
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        return this->hostAddr;
      }
 
      public: unsigned int ActivityInterval() const
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        return this->activityInterval;
      }
 
      public: unsigned int HeartbeatInterval() const
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        return this->heartbeatInterval;
      }
 
      public: unsigned int SilenceInterval() const
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        return this->silenceInterval;
      }
 
      public: void SetActivityInterval(const unsigned int _ms)
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        this->activityInterval = _ms;
      }
 
      public: void SetHeartbeatInterval(const unsigned int _ms)
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        this->heartbeatInterval = _ms;
      }
 
      public: void SetSilenceInterval(const unsigned int _ms)
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        this->silenceInterval = _ms;
      }
 
      public: void ConnectionsCb(const DiscoveryCallback<Pub> &_cb)
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        this->connectionCb = _cb;
      }
 
      public: void DisconnectionsCb(const DiscoveryCallback<Pub> &_cb)
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        this->disconnectionCb = _cb;
      }
 
      public: void RegistrationsCb(const DiscoveryCallback<Pub> &_cb)
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        this->registrationCb = _cb;
      }
 
      public: void UnregistrationsCb(const DiscoveryCallback<Pub> &_cb)
      {
        std::lock_guard<std::mutex> lock(this->mutex);
        this->unregistrationCb = _cb;
      }
 
      public: void PrintCurrentState() const
      {
        std::lock_guard<std::mutex> lock(this->mutex);
 
        std::cout << "---------------" << std::endl;
        std::cout << std::boolalpha << "Enabled: "
                  << this->enabled << std::endl;
        std::cout << "Discovery state" << std::endl;
        std::cout << "\tUUID: " << this->pUuid << std::endl;
        std::cout << "Settings" << std::endl;
        std::cout << "\tActivity: " << this->activityInterval
                  << " ms." << std::endl;
        std::cout << "\tHeartbeat: " << this->heartbeatInterval
                  << "ms." << std::endl;
        std::cout << "\tSilence: " << this->silenceInterval
                  << " ms." << std::endl;
        std::cout << "Known information:" << std::endl;
        this->info.Print();
 
        // Used to calculate the elapsed time.
        Timestamp now = std::chrono::steady_clock::now();
 
        std::cout << "Activity" << std::endl;
        if (this->activity.empty())
          std::cout << "\t<empty>" << std::endl;
        else
        {
          for (auto &proc : this->activity)
          {
            // Elapsed time since the last update from this publisher.
            std::chrono::duration<double> elapsed = now - proc.second;
 
            std::cout << "\t" << proc.first << std::endl;
            std::cout << "\t\t" << "Since: " << std::chrono::duration_cast<
              std::chrono::milliseconds>(elapsed).count() << " ms. ago. "
              << std::endl;
          }
        }
        std::cout << "---------------" << std::endl;
      }
 
      public: void TopicList(std::vector<std::string> &_topics) const
      {
        this->WaitForInit();
        std::lock_guard<std::mutex> lock(this->mutex);
        this->info.TopicList(_topics);
      }
 
      public: void WaitForInit() const
      {
        std::unique_lock<std::mutex> lk(this->mutex);
 
        if (!this->initialized)
        {
          this->initializedCv.wait(lk, [this]{return this->initialized;});
        }
      }
 
      private: void UpdateActivity()
      {
        // The UUIDs of the processes that have expired.
        std::vector<std::string> uuids;
 
        // A copy of the disconnection callback.
        DiscoveryCallback<Pub> disconnectCb;
 
        Timestamp now = std::chrono::steady_clock::now();
 
        {
          std::lock_guard<std::mutex> lock(this->mutex);
 
          if (now < this->timeNextActivity)
            return;
 
          disconnectCb = this->disconnectionCb;
 
          for (auto it = this->activity.cbegin(); it != this->activity.cend();)
          {
            // Elapsed time since the last update from this publisher.
            auto elapsed = now - it->second;
 
            // This publisher has expired.
            if (std::chrono::duration_cast<std::chrono::milliseconds>
                 (elapsed).count() > this->silenceInterval)
            {
              // Remove all the info entries for this process UUID.
              this->info.DelPublishersByProc(it->first);
 
              uuids.push_back(it->first);
 
              // Remove the activity entry.
              this->activity.erase(it++);
            }
            else
              ++it;
          }
 
          this->timeNextActivity = std::chrono::steady_clock::now() +
            std::chrono::milliseconds(this->activityInterval);
        }
 
        if (!disconnectCb)
          return;
 
        // Notify without topic information. This is useful to inform the
        // client that a remote node is gone, even if we were not
        // interested in its topics.
        for (auto const &uuid : uuids)
        {
          Pub publisher;
          publisher.SetPUuid(uuid);
          disconnectCb(publisher);
        }
      }
 
      private: void UpdateHeartbeat()
      {
        Timestamp now = std::chrono::steady_clock::now();
 
        {
          std::lock_guard<std::mutex> lock(this->mutex);
 
          if (now < this->timeNextHeartbeat)
            return;
        }
 
        Publisher pub("", "", this->pUuid, "", AdvertiseOptions());
        this->SendMsg(DestinationType::ALL, msgs::Discovery::HEARTBEAT, pub);
 
        std::map<std::string, std::vector<Pub>> nodes;
        {
          std::lock_guard<std::mutex> lock(this->mutex);
 
          // Re-advertise topics that are advertised inside this process.
          this->info.PublishersByProc(this->pUuid, nodes);
        }
 
        for (const auto &topic : nodes)
        {
          for (const auto &node : topic.second)
          {
            this->SendMsg(DestinationType::ALL,
                msgs::Discovery::ADVERTISE, node);
          }
        }
 
        {
          std::lock_guard<std::mutex> lock(this->mutex);
          if (!this->initialized)
          {
            if (this->numHeartbeatsUninitialized == 2u)
            {
              // We consider discovery initialized after two heartbeat cycles.
              this->initialized = true;
 
              // Notify anyone waiting for the initialization phase to finish.
              this->initializedCv.notify_all();
            }
            ++this->numHeartbeatsUninitialized;
          }
 
          this->timeNextHeartbeat = std::chrono::steady_clock::now() +
            std::chrono::milliseconds(this->heartbeatInterval);
        }
      }
 
      private: int NextTimeout() const
      {
        auto now = std::chrono::steady_clock::now();
        auto timeUntilNextHeartbeat = this->timeNextHeartbeat - now;
        auto timeUntilNextActivity = this->timeNextActivity - now;
 
        int t = static_cast<int>(
          std::chrono::duration_cast<std::chrono::milliseconds>
            (std::min(timeUntilNextHeartbeat, timeUntilNextActivity)).count());
        int t2 = std::min(t, this->kTimeout);
        return std::max(t2, 0);
      }
 
      private: void RecvMessages()
      {
        bool timeToExit = false;
        while (!timeToExit)
        {
          // Calculate the timeout.
          int timeout = this->NextTimeout();
 
          if (pollSockets(this->sockets, timeout))
          {
            this->RecvDiscoveryUpdate();
 
            if (this->verbose)
              this->PrintCurrentState();
          }
 
          this->UpdateHeartbeat();
          this->UpdateActivity();
 
          // Is it time to exit?
          {
            std::lock_guard<std::mutex> lock(this->exitMutex);
            if (this->exit)
              timeToExit = true;
          }
        }
      }
 
      private: void RecvDiscoveryUpdate()
      {
        char rcvStr[Discovery::kMaxRcvStr];
        sockaddr_in clntAddr;
        socklen_t addrLen = sizeof(clntAddr);
 
        int64_t received = recvfrom(this->sockets.at(0),
              reinterpret_cast<raw_type *>(rcvStr),
              this->kMaxRcvStr, 0,
              reinterpret_cast<sockaddr *>(&clntAddr),
              reinterpret_cast<socklen_t *>(&addrLen));
        if (received > 0)
        {
          uint16_t len = 0;
          memcpy(&len, &rcvStr[0], sizeof(len));
 
          // Ignition Transport delimits each discovery message with a
          // frame_delimiter that contains byte size information.
          // A discovery message has the form:
          //
          // <frame_delimiter><frame_body>
          //
          // Ignition Transport version < 8 sends a frame delimiter that
          // contains the value of sizeof(frame_delimiter)
          // + sizeof(frame_body). In other words, the frame_delimiter
          // contains a value that represents the total size of the
          // frame_body and frame_delimiter in bytes.
          //
          // Ignition Transport version >= 8 sends a frame_delimiter
          // that contains the value of sizeof(frame_body). In other
          // words, the frame_delimiter contains a value that represents
          // the total size of only the frame_body.
          //
          // It is possible that two incompatible versions of Ignition
          // Transport exist on the same network. If we receive an
          // unexpected size, then we ignore the message.
 
          // If-condition for version 8+
          if (len + sizeof(len) == static_cast<uint16_t>(received))
          {
            std::string srcAddr = inet_ntoa(clntAddr.sin_addr);
            uint16_t srcPort = ntohs(clntAddr.sin_port);
 
            if (this->verbose)
            {
              std::cout << "\nReceived discovery update from "
                << srcAddr << ": " << srcPort << std::endl;
            }
 
            this->DispatchDiscoveryMsg(srcAddr, rcvStr + sizeof(len), len);
          }
        }
        else if (received < 0)
        {
          std::cerr << "Discovery::RecvDiscoveryUpdate() recvfrom error"
            << std::endl;
        }
      }
 
      private: void DispatchDiscoveryMsg(const std::string &_fromIp,
                                         char *_msg, uint16_t _len)
      {
        gz::msgs::Discovery msg;
 
        // Parse the message, and return if parsing failed. Parsing could
        // fail when another discovery node is publishing messages using an
        // older (or newer) format.
        if (!msg.ParseFromArray(_msg, _len))
          return;
 
        // Discard the message if the wire protocol is different than mine.
        if (this->Version() != msg.version())
          return;
 
        std::string recvPUuid = msg.process_uuid();
 
        // Discard our own discovery messages.
        if (recvPUuid == this->pUuid)
          return;
 
        // Forwarding summary:
        //   - From a unicast peer  -> to multicast group (with NO_RELAY flag).
        //   - From multicast group -> to unicast peers (with RELAY flag).
 
        // If the RELAY flag is set, this discovery message is coming via a
        // unicast transmission. In this case, we don't process it, we just
        // forward it to the multicast group, and it will be dispatched once
        // received there. Note that we also unset the RELAY flag and set the
        // NO_RELAY flag, to avoid forwarding the message anymore.
        if (msg.has_flags() && msg.flags().relay())
        {
          // Unset the RELAY flag in the header and set the NO_RELAY.
          msg.mutable_flags()->set_relay(false);
          msg.mutable_flags()->set_no_relay(true);
          this->SendMulticast(msg);
 
          // A unicast peer contacted me. I need to save its address for
          // sending future messages in the future.
          this->AddRelayAddress(_fromIp);
          return;
        }
        // If we are receiving this discovery message via the multicast channel
        // and the NO_RELAY flag is not set, we forward this message via unicast
        // to all our relays. Note that this is the most common case, where we
        // receive a regular multicast message and we forward it to any remote
        // relays.
        else if (!msg.has_flags() || !msg.flags().no_relay())
        {
          msg.mutable_flags()->set_relay(true);
          this->SendUnicast(msg);
        }
 
        // Update timestamp and cache the callbacks.
        DiscoveryCallback<Pub> connectCb;
        DiscoveryCallback<Pub> disconnectCb;
        DiscoveryCallback<Pub> registerCb;
        DiscoveryCallback<Pub> unregisterCb;
        {
          std::lock_guard<std::mutex> lock(this->mutex);
          this->activity[recvPUuid] = std::chrono::steady_clock::now();
          connectCb = this->connectionCb;
          disconnectCb = this->disconnectionCb;
          registerCb = this->registrationCb;
          unregisterCb = this->unregistrationCb;
        }
 
        switch (msg.type())
        {
          case msgs::Discovery::ADVERTISE:
          {
            // Read the rest of the fields.
            Pub publisher;
            publisher.SetFromDiscovery(msg);
 
            // Check scope of the topic.
            if ((publisher.Options().Scope() == Scope_t::PROCESS) ||
                (publisher.Options().Scope() == Scope_t::HOST &&
                 _fromIp != this->hostAddr))
            {
              return;
            }
 
            // Register an advertised address for the topic.
            bool added;
            {
              std::lock_guard<std::mutex> lock(this->mutex);
              added = this->info.AddPublisher(publisher);
            }
 
            if (added && connectCb)
            {
              // Execute the client's callback.
              connectCb(publisher);
            }
 
            break;
          }
          case msgs::Discovery::SUBSCRIBE:
          {
            std::string recvTopic;
            // Read the topic information.
            if (msg.has_sub())
            {
              recvTopic = msg.sub().topic();
            }
            else
            {
              std::cerr << "Subscription discovery message is missing "
                << "Subscriber information.\n";
              break;
            }
 
            // Check if at least one of my nodes advertises the topic requested.
            Addresses_M<Pub> addresses;
            {
              std::lock_guard<std::mutex> lock(this->mutex);
              if (!this->info.HasAnyPublishers(recvTopic, this->pUuid))
              {
                break;
              }
 
              if (!this->info.Publishers(recvTopic, addresses))
                break;
            }
 
            for (const auto &nodeInfo : addresses[this->pUuid])
            {
              // Check scope of the topic.
              if ((nodeInfo.Options().Scope() == Scope_t::PROCESS) ||
                  (nodeInfo.Options().Scope() == Scope_t::HOST &&
                   _fromIp != this->hostAddr))
              {
                continue;
              }
 
              // Answer an ADVERTISE message.
              this->SendMsg(DestinationType::ALL,
                  msgs::Discovery::ADVERTISE, nodeInfo);
            }
 
            break;
          }
          case msgs::Discovery::NEW_CONNECTION:
          {
            // Read the rest of the fields.
            Pub publisher;
            publisher.SetFromDiscovery(msg);
 
            if (registerCb)
              registerCb(publisher);
 
            break;
          }
          case msgs::Discovery::END_CONNECTION:
          {
            // Read the rest of the fields.
            Pub publisher;
            publisher.SetFromDiscovery(msg);
 
            if (unregisterCb)
              unregisterCb(publisher);
 
            break;
          }
          case msgs::Discovery::HEARTBEAT:
          {
            // The timestamp has already been updated.
            break;
          }
          case msgs::Discovery::BYE:
          {
            // Remove the activity entry for this publisher.
            {
              std::lock_guard<std::mutex> lock(this->mutex);
              this->activity.erase(recvPUuid);
            }
 
            if (disconnectCb)
            {
              Pub pub;
              pub.SetPUuid(recvPUuid);
              // Notify the new disconnection.
              disconnectCb(pub);
            }
 
            // Remove the address entry for this topic.
            {
              std::lock_guard<std::mutex> lock(this->mutex);
              this->info.DelPublishersByProc(recvPUuid);
            }
 
            break;
          }
          case msgs::Discovery::UNADVERTISE:
          {
            // Read the address.
            Pub publisher;
            publisher.SetFromDiscovery(msg);
 
            // Check scope of the topic.
            if ((publisher.Options().Scope() == Scope_t::PROCESS) ||
                (publisher.Options().Scope() == Scope_t::HOST &&
                 _fromIp != this->hostAddr))
            {
              return;
            }
 
            if (disconnectCb)
            {
              // Notify the new disconnection.
              disconnectCb(publisher);
            }
 
            // Remove the address entry for this topic.
            {
              std::lock_guard<std::mutex> lock(this->mutex);
              this->info.DelPublisherByNode(publisher.Topic(),
                publisher.PUuid(), publisher.NUuid());
            }
 
            break;
          }
          default:
          {
            std::cerr << "Unknown message type [" << msg.type() << "].\n";
            break;
          }
        }
      }
 
      private: template<typename T>
      void SendMsg(const DestinationType &_destType,
                   const msgs::Discovery::Type _type,
                   const T &_pub) const
      {
        gz::msgs::Discovery discoveryMsg;
        discoveryMsg.set_version(this->Version());
        discoveryMsg.set_type(_type);
        discoveryMsg.set_process_uuid(this->pUuid);
 
        switch (_type)
        {
          case msgs::Discovery::ADVERTISE:
          case msgs::Discovery::UNADVERTISE:
          case msgs::Discovery::NEW_CONNECTION:
          case msgs::Discovery::END_CONNECTION:
          {
            _pub.FillDiscovery(discoveryMsg);
            break;
          }
          case msgs::Discovery::SUBSCRIBE:
          {
            discoveryMsg.mutable_sub()->set_topic(_pub.Topic());
            break;
          }
          case msgs::Discovery::HEARTBEAT:
          case msgs::Discovery::BYE:
            break;
          default:
            std::cerr << "Discovery::SendMsg() error: Unrecognized message"
                      << " type [" << _type << "]" << std::endl;
            return;
        }
 
        if (_destType == DestinationType::MULTICAST ||
            _destType == DestinationType::ALL)
        {
          this->SendMulticast(discoveryMsg);
        }
 
        // Send the discovery message to the unicast relays.
        if (_destType == DestinationType::UNICAST ||
            _destType == DestinationType::ALL)
        {
          // Set the RELAY flag in the header.
          discoveryMsg.mutable_flags()->set_relay(true);
          this->SendUnicast(discoveryMsg);
        }
 
        if (this->verbose)
        {
          std::cout << "\t* Sending " << msgs::ToString(_type)
                    << " msg [" << _pub.Topic() << "]" << std::endl;
        }
      }
 
      private: void SendUnicast(const msgs::Discovery &_msg) const
      {
        uint16_t msgSize;
 
#if GOOGLE_PROTOBUF_VERSION >= 3004000
        size_t msgSizeFull = _msg.ByteSizeLong();
#else
        int msgSizeFull = _msg.ByteSize();
#endif
        if (msgSizeFull + sizeof(msgSize) > this->kMaxRcvStr)
        {
          std::cerr << "Discovery message too large to send. Discovery won't "
            << "work. This shouldn't happen.\n";
          return;
        }
        msgSize = msgSizeFull;
 
        uint16_t totalSize = sizeof(msgSize) + msgSize;
        char *buffer = static_cast<char *>(new char[totalSize]);
        memcpy(&buffer[0], &msgSize, sizeof(msgSize));
 
        if (_msg.SerializeToArray(buffer + sizeof(msgSize), msgSize))
        {
          // Send the discovery message to the unicast relays.
          for (const auto &sockAddr : this->relayAddrs)
          {
            auto sent = sendto(this->sockets.at(0),
              reinterpret_cast<const raw_type *>(
                reinterpret_cast<const unsigned char*>(buffer)),
              totalSize, 0,
              reinterpret_cast<const sockaddr *>(&sockAddr),
              sizeof(sockAddr));
 
            if (sent != totalSize)
            {
              std::cerr << "Exception sending a unicast message" << std::endl;
              break;
            }
          }
        }
        else
        {
          std::cerr << "Discovery::SendUnicast: Error serializing data."
            << std::endl;
        }
 
        delete [] buffer;
      }
 
      private: void SendMulticast(const msgs::Discovery &_msg) const
      {
        uint16_t msgSize;
 
#if GOOGLE_PROTOBUF_VERSION >= 3004000
        size_t msgSizeFull = _msg.ByteSizeLong();
#else
        int msgSizeFull = _msg.ByteSize();
#endif
        if (msgSizeFull + sizeof(msgSize) > this->kMaxRcvStr)
        {
          std::cerr << "Discovery message too large to send. Discovery won't "
            << "work. This shouldn't happen.\n";
          return;
        }
 
        msgSize = msgSizeFull;
        uint16_t totalSize = sizeof(msgSize) + msgSize;
        char *buffer = static_cast<char *>(new char[totalSize]);
        memcpy(&buffer[0], &msgSize, sizeof(msgSize));
 
        if (_msg.SerializeToArray(buffer + sizeof(msgSize), msgSize))
        {
          // Send the discovery message to the multicast group through all the
          // sockets.
          for (const auto &sock : this->Sockets())
          {
            errno = 0;
            if (sendto(sock, reinterpret_cast<const raw_type *>(
              reinterpret_cast<const unsigned char*>(buffer)),
              totalSize, 0,
              reinterpret_cast<const sockaddr *>(this->MulticastAddr()),
              sizeof(*(this->MulticastAddr()))) != totalSize)
            {
              // Ignore EPERM and ENOBUFS errors.
              //
              // See issue #106
              //
              // Rationale drawn from:
              //
              // * https://groups.google.com/forum/#!topic/comp.protocols.tcp-ip/Qou9Sfgr77E
              // * https://stackoverflow.com/questions/16555101/sendto-dgrams-do-not-block-for-enobufs-on-osx
              if (errno != EPERM && errno != ENOBUFS)
              {
                std::cerr << "Exception sending a multicast message:"
                  << strerror(errno) << std::endl;
              }
              break;
            }
          }
        }
        else
        {
          std::cerr << "Discovery::SendMulticast: Error serializing data."
            << std::endl;
        }
 
        delete [] buffer;
      }
 
      private: const std::vector<int> &Sockets() const
      {
        return this->sockets;
      }
 
      private: const sockaddr_in *MulticastAddr() const
      {
        return &this->mcastAddr;
      }
 
      private: uint8_t Version() const
      {
        static std::string ignStats;
        static int topicStats =
          (env("IGN_TRANSPORT_TOPIC_STATISTICS", ignStats) && ignStats == "1");
        return this->kWireVersion + (topicStats * 100);
      }
 
      private: bool RegisterNetIface(const std::string &_ip)
      {
        // Make a new socket for sending discovery information.
        int sock = static_cast<int>(socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP));
        if (sock < 0)
        {
          std::cerr << "Socket creation failed." << std::endl;
          return false;
        }
 
        // Socket option: IP_MULTICAST_IF.
        // This socket option needs to be applied to each socket used to send
        // data. This option selects the source interface for outgoing messages.
        struct in_addr ifAddr;
        ifAddr.s_addr = inet_addr(_ip.c_str());
        if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
          reinterpret_cast<const char*>(&ifAddr), sizeof(ifAddr)) != 0)
        {
          std::cerr << "Error setting socket option (IP_MULTICAST_IF)."
                    << std::endl;
          return false;
        }
 
        this->sockets.push_back(sock);
 
        // Join the multicast group. We have to do it for each network interface
        // but we can do it on the same socket. We will use the socket at
        // position 0 for receiving multicast information.
        struct ip_mreq group;
        group.imr_multiaddr.s_addr =
          inet_addr(this->kMulticastGroup.c_str());
        group.imr_interface.s_addr = inet_addr(_ip.c_str());
        if (setsockopt(this->sockets.at(0), IPPROTO_IP, IP_ADD_MEMBERSHIP,
          reinterpret_cast<const char*>(&group), sizeof(group)) != 0)
        {
          std::cerr << "Error setting socket option (IP_ADD_MEMBERSHIP)."
                    << std::endl;
          return false;
        }
 
        return true;
      }
 
      private: void AddRelayAddress(const std::string &_ip)
      {
        // Sanity check: Make sure that this IP address is not already saved.
        for (auto const &addr : this->relayAddrs)
        {
          if (addr.sin_addr.s_addr == inet_addr(_ip.c_str()))
            return;
        }
 
        sockaddr_in addr;
        memset(&addr, 0, sizeof(addr));
        addr.sin_family = AF_INET;
        addr.sin_addr.s_addr = inet_addr(_ip.c_str());
        addr.sin_port = htons(static_cast<u_short>(this->port));
 
        this->relayAddrs.push_back(addr);
      }
 
      private: static const unsigned int kDefActivityInterval = 100;
 
      private: static const unsigned int kDefHeartbeatInterval = 1000;
 
      private: static const unsigned int kDefSilenceInterval = 3000;
 
      private: const std::string kMulticastGroup = "224.0.0.7";
 
      private: const int kTimeout = 250;
 
      private: static const uint16_t kMaxRcvStr =
               std::numeric_limits<uint16_t>::max();
 
      private: static const uint8_t kWireVersion = 10;
 
      private: int port;
 
      private: std::string hostAddr;
 
      private: std::vector<std::string> hostInterfaces;
 
      private: std::string pUuid;
 
      private: unsigned int silenceInterval;
 
      private: unsigned int activityInterval;
 
      private: unsigned int heartbeatInterval;
 
      private: DiscoveryCallback<Pub> connectionCb;
 
      private: DiscoveryCallback<Pub> disconnectionCb;
 
      private: DiscoveryCallback<Pub> registrationCb;
 
      private: DiscoveryCallback<Pub> unregistrationCb;
 
      private: TopicStorage<Pub> info;
 
      protected: std::map<std::string, Timestamp> activity;
 
      private: bool verbose;
 
      private: std::vector<int> sockets;
 
      private: sockaddr_in mcastAddr;
 
      private: std::vector<sockaddr_in> relayAddrs;
 
      private: mutable std::mutex mutex;
 
      private: std::thread threadReception;
 
      private: Timestamp timeNextHeartbeat;
 
      private: Timestamp timeNextActivity;
 
      private: std::mutex exitMutex;
 
      private: bool initialized;
 
      private: unsigned int numHeartbeatsUninitialized;
 
      private: mutable std::condition_variable initializedCv;
 
      private: bool exit;
 
      private: bool enabled;
    };
 
    using MsgDiscovery = Discovery<MessagePublisher>;
 
    using SrvDiscovery = Discovery<ServicePublisher>;
    }
  }
}
 
#endif