Announce subscriptions and report subscriber snapshots in discovery

src/Discovery.hh

@@ -60,12 +60,14 @@
 
 #include <algorithm>
 #include <condition_variable>
+#include <cstdint>
 #include <limits>
 #include <map>
 #include <memory>
 #include <mutex>
 #include <string>
 #include <thread>
+#include <type_traits>
 #include <utility>
 #include <vector>
 
@@ -456,6 +458,16 @@ namespace gz
 
         // Start the thread that receives discovery information.
         this->threadReception = std::thread(&Discovery::RecvMessages, this);
+
+        // Request the list of current subscribers so that the cache is
+        // already complete when the initialization phase finishes. New
+        // subscriptions are tracked through the SUBSCRIBE announcements.
+        if constexpr (std::is_same_v<Pub, MessagePublisher>)
+        {
+          Publisher pub("", "", this->pUuid, "", AdvertiseOptions());
+          this->SendMsg(
+            DestinationType::ALL, msgs::Discovery::SUBSCRIBERS_REQ, pub);
+        }
       }
 
 #ifdef HAVE_ZENOH
@@ -517,9 +529,13 @@ namespace gz
       /// \sa SetConnectionsCb.
       /// \sa SetDisconnectionsCb.
       /// \param[in] _topic Topic name requested.
+      /// \param[in] _nUuid Node UUID of the subscriber requesting the topic,
+      /// announced so that other processes can track this subscription.
+      /// Empty when the request is not tied to a subscription.
       /// \return True if the method succeeded or false otherwise
       /// (e.g. if the discovery has not been started).
-      public: bool Discover(const std::string &_topic) const
+      public: bool Discover(const std::string &_topic,
+                            const std::string &_nUuid = "") const
       {
         DiscoveryCallback<Pub> cb;
         bool found;
@@ -537,6 +553,7 @@ namespace gz
         Pub pub;
         pub.SetTopic(_topic);
         pub.SetPUuid(this->pUuid);
+        pub.SetNUuid(_nUuid);
 
         // Send a discovery request.
         this->SendMsg(DestinationType::ALL, msgs::Discovery::SUBSCRIBE, pub);
@@ -567,12 +584,35 @@ namespace gz
         return true;
       }
 
-      /// \brief Send the response to a SUBSCRIBERS_REQ message.
-      /// \param[in] _pub Information to send.
-      public: void SendSubscribersRep(const MessagePublisher &_pub) const
+      /// \brief Send one response of the burst answering a SUBSCRIBERS_REQ
+      /// message. The burst is a snapshot of all the subscriptions of this
+      /// process: the receiver knows that the snapshot is complete when
+      /// _count messages have been received.
+      /// \param[in] _pub Information to send. An empty topic is used to
+      /// answer when the process has no subscriptions (_count is zero).
+      /// \param[in] _count Number of messages in this burst.
+      /// \param[in] _generation Subscription generation of this process when
+      /// the snapshot was taken.
+      public: void SendSubscribersRep(const MessagePublisher &_pub,
+                                      const uint32_t _count,
+                                      const uint64_t _generation) const
       {
-        this->SendMsg(
-          DestinationType::ALL, msgs::Discovery::SUBSCRIBERS_REP, _pub);
+        gz::msgs::Discovery discoveryMsg;
+        discoveryMsg.set_version(this->Version());
+        discoveryMsg.set_type(msgs::Discovery::SUBSCRIBERS_REP);
+        discoveryMsg.set_process_uuid(this->pUuid);
+        if (!_pub.Topic().empty())
+          _pub.FillDiscovery(discoveryMsg);
+
+        auto *snapshot = discoveryMsg.mutable_subscribers_snapshot();
+        snapshot->set_count(_count);
+        snapshot->set_generation(_generation);
+
+        this->SendMulticast(discoveryMsg);
+
+        // Set the RELAY flag in the header and send to the unicast relays.
+        discoveryMsg.mutable_flags()->set_relay(true);
+        this->SendUnicast(discoveryMsg);
       }
 
       /// \brief Register a node from this process as a remote subscriber.
@@ -813,23 +853,71 @@ namespace gz
       }
 
       /// \brief Get the list of topics currently advertised and subscribed
-      /// in the network.
+      /// in the network. The call blocks until every known process has
+      /// reported a complete snapshot of its subscribers or a short timeout
+      /// expires. The wait normally finishes in a few milliseconds, when
+      /// the last snapshot arrives.
       /// \param[out] _topics List of advertised topics.
       public: void TopicList(std::vector<std::string> &_topics)
       {
-        if (!this->useZenoh)
+        [[maybe_unused]] Timestamp requestTime =
+          std::chrono::steady_clock::now();
+        [[maybe_unused]] std::vector<std::string> knownProcs;
+
+        // Request the list of subscribers. This request is only meaningful
+        // for message discovery over UDP: the Zenoh backend keeps
+        // remoteSubscribers updated via liveliness tokens and nothing
+        // answers this request on the service discovery channel.
+        if constexpr (std::is_same_v<Pub, MessagePublisher>)
         {
-          std::lock_guard<std::mutex> lock(this->mutex);
-          this->remoteSubscribers.Clear();
-        }
+          if (!this->useZenoh)
+          {
+            {
+              std::lock_guard<std::mutex> lock(this->mutex);
+              for (const auto &proc : this->activity)
+                knownProcs.push_back(proc.first);
+            }
 
-        // Request the list of subscribers.
-        Publisher pub("", "", this->pUuid, "", AdvertiseOptions());
-        this->SendMsg(
-          DestinationType::ALL, msgs::Discovery::SUBSCRIBERS_REQ, pub);
+            Publisher pub("", "", this->pUuid, "", AdvertiseOptions());
+            this->SendMsg(
+              DestinationType::ALL, msgs::Discovery::SUBSCRIBERS_REQ, pub);
+          }
+        }
 
         this->WaitForInit();
-        std::lock_guard<std::mutex> lock(this->mutex);
+        std::unique_lock<std::mutex> lock(this->mutex);
+
+        if constexpr (std::is_same_v<Pub, MessagePublisher>)
+        {
+          if (!this->useZenoh)
+          {
+            // Wait until every process known at request time has reported a
+            // snapshot after the request or the timeout expires. A lost
+            // reply or a process running an older version is covered by the
+            // timeout: the cached information is used instead. A process
+            // that expired while waiting is not expected to reply.
+            this->subscribersRepCv.wait_until(lock,
+              requestTime +
+              std::chrono::milliseconds(kDefSubscribersRepTimeout),
+              [&]
+              {
+                for (const auto &proc : knownProcs)
+                {
+                  if (this->activity.find(proc) == this->activity.end())
+                    continue;
+
+                  auto it = this->subscribersSnapshots.find(proc);
+                  if (it == this->subscribersSnapshots.end() ||
+                      it->second.completed < requestTime)
+                  {
+                    return false;
+                  }
+                }
+                return true;
+              });
+          }
+        }
+
         this->info.TopicList(_topics);
 
         std::vector<std::string> remoteSubs;
@@ -889,6 +977,8 @@ namespace gz
             {
               // Remove all the info entries for this process UUID.
               this->info.DelPublishersByProc(it->first);
+              this->remoteSubscribers.DelPublishersByProc(it->first);
+              this->subscribersSnapshots.erase(it->first);
 
               uuids.push_back(it->first);
 
@@ -1239,6 +1329,21 @@ namespace gz
               break;
             }
 
+            // Register the remote subscriber. Subscribers running an older
+            // version do not announce their node UUID and are only tracked
+            // through the SUBSCRIBERS_REQ mechanism.
+            if constexpr (std::is_same_v<Pub, MessagePublisher>)
+            {
+              if (!msg.sub().n_uuid().empty())
+              {
+                Pub subscriber(recvTopic, "", "", recvPUuid,
+                  msg.sub().n_uuid(), kGenericMessageType,
+                  AdvertiseMessageOptions());
+                std::lock_guard<std::mutex> lock(this->mutex);
+                this->remoteSubscribers.AddPublisher(subscriber);
+              }
+            }
+
             // Check if at least one of my nodes advertises the topic requested.
             Addresses_M<Pub> addresses;
             {
@@ -1284,7 +1389,43 @@ namespace gz
 
             {
               std::lock_guard<std::mutex> lock(this->mutex);
-              this->remoteSubscribers.AddPublisher(publisher);
+
+              if (msg.has_subscribers_snapshot())
+              {
+                const auto &snapshot = msg.subscribers_snapshot();
+                auto &progress = this->subscribersSnapshots[recvPUuid];
+
+                // A new generation starts an authoritative snapshot:
+                // replace all the entries known for this process.
+                if (!progress.started ||
+                    progress.generation != snapshot.generation())
+                {
+                  progress.started = true;
+                  progress.generation = snapshot.generation();
+                  progress.expected = snapshot.count();
+                  progress.received = 0;
+                  this->remoteSubscribers.DelPublishersByProc(recvPUuid);
+                }
+
+                if (!publisher.Topic().empty())
+                {
+                  this->remoteSubscribers.AddPublisher(publisher);
+                  ++progress.received;
+                }
+
+                if (progress.received >= progress.expected)
+                {
+                  progress.completed = std::chrono::steady_clock::now();
+                  this->subscribersRepCv.notify_all();
+                }
+              }
+              else
+              {
+                // A process running an older version does not attach the
+                // snapshot metadata. Its information is merged and its
+                // completion is covered by the TopicList() timeout.
+                this->remoteSubscribers.AddPublisher(publisher);
+              }
             }
             break;
           }
@@ -1305,6 +1446,12 @@ namespace gz
             Pub publisher;
             publisher.SetFromDiscovery(msg);
 
+            {
+              std::lock_guard<std::mutex> lock(this->mutex);
+              this->remoteSubscribers.DelPublisherByNode(
+                publisher.Topic(), publisher.PUuid(), publisher.NUuid());
+            }
+
             if (unregisterCb)
               unregisterCb(publisher);
 
@@ -1335,6 +1482,8 @@ namespace gz
             {
               std::lock_guard<std::mutex> lock(this->mutex);
               this->info.DelPublishersByProc(recvPUuid);
+              this->remoteSubscribers.DelPublishersByProc(recvPUuid);
+              this->subscribersSnapshots.erase(recvPUuid);
             }
 
             break;
@@ -1406,6 +1555,7 @@ namespace gz
           case msgs::Discovery::SUBSCRIBE:
           {
             discoveryMsg.mutable_sub()->set_topic(_pub.Topic());
+            discoveryMsg.mutable_sub()->set_n_uuid(_pub.NUuid());
             break;
           }
           case msgs::Discovery::HEARTBEAT:
@@ -1637,6 +1787,11 @@ namespace gz
       /// \sa SetHeartbeatInterval.
       private: static const unsigned int kDefHeartbeatInterval = 1000;
 
+      /// \brief Default maximum time waiting for the subscriber snapshots
+      /// in TopicList() (ms.). The wait normally finishes much earlier,
+      /// when every known process has answered.
+      private: static constexpr unsigned int kDefSubscribersRepTimeout = 100;
+
       /// \brief Default silence interval value (ms.).
       /// \sa MaxSilenceInterval.
       /// \sa SetMaxSilenceInterval.
@@ -1704,6 +1859,35 @@ namespace gz
       /// \brief Remote subscribers.
       private: TopicStorage<Pub> remoteSubscribers;
 
+      /// \brief Progress of the snapshot that a remote process reports in
+      /// a SUBSCRIBERS_REP burst.
+      private: struct SubscribersSnapshotProgress
+      {
+        /// \brief True when at least one burst has been received.
+        bool started = false;
+
+        /// \brief Subscription generation of the last burst.
+        uint64_t generation = 0;
+
+        /// \brief Number of messages expected in the burst.
+        uint32_t expected = 0;
+
+        /// \brief Number of messages received from the burst.
+        uint32_t received = 0;
+
+        /// \brief Last time a complete snapshot was received.
+        Timestamp completed = Timestamp::min();
+      };
+
+      /// \brief Snapshot progress of each remote process, keyed by its
+      /// process UUID.
+      private: std::map<std::string, SubscribersSnapshotProgress>
+        subscribersSnapshots;
+
+      /// \brief Condition variable notified every time a remote process
+      /// completes a subscribers snapshot.
+      private: mutable std::condition_variable subscribersRepCv;
+
       /// \brief Activity information. Every time there is a message from a
       /// remote node, its activity information is updated. If we do not hear
       /// from a node in a while, its entries in 'info' will be invalided. The

src/Node.cc

@@ -824,6 +824,23 @@ void Node::TopicList(std::vector<std::string> &_topics) const
 
   this->dataPtr->shared->dataPtr->msgDiscovery->TopicList(allTopics);
 
+  // Add the topics subscribed within this process. They are not part of the
+  // discovery information because a process discards its own discovery
+  // messages.
+  {
+    std::lock_guard<std::recursive_mutex> lock(this->dataPtr->shared->mutex);
+    for (const auto &pub : this->dataPtr->shared->localSubscribers.Convert(
+        this->dataPtr->shared->dataPtr->myAddress,
+        this->dataPtr->shared->pUuid))
+    {
+      if (std::find(allTopics.begin(), allTopics.end(), pub.Topic()) ==
+          allTopics.end())
+      {
+        allTopics.push_back(pub.Topic());
+      }
+    }
+  }
+
   for (const auto &fullyQualifiedTopic : allTopics)
   {
     std::string partition;
@@ -1062,6 +1079,21 @@ bool Node::TopicInfo(const std::string &_topic,
     convert(subs, _subscribers);
   }
 
+  // Add the subscribers within this process. They are not part of the
+  // discovery information because a process discards its own discovery
+  // messages.
+  for (const auto &pub : this->dataPtr->shared->localSubscribers.Convert(
+      this->dataPtr->shared->dataPtr->myAddress,
+      this->dataPtr->shared->pUuid))
+  {
+    if (pub.Topic() == fullyQualifiedTopic &&
+        std::find(_subscribers.begin(), _subscribers.end(), pub) ==
+        _subscribers.end())
+    {
+      _subscribers.push_back(pub);
+    }
+  }
+
   return true;
 }