Add support for topic statistics on breadcrumb deployments (#532)

* Add support for topic statistics on breadcrumb deployments

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Require version 9.1 of ignition transport

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Move to after mutex

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Carlos Agüero <caguero@osrfoundation.org>

CMakeLists.txt

@@ -48,7 +48,7 @@ set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-transport
-ign_find_package(ignition-transport9 REQUIRED COMPONENTS log)
+ign_find_package(ignition-transport9 REQUIRED COMPONENTS log VERSION 9.1)
 set(IGN_TRANSPORT_VER ${ignition-transport9_VERSION_MAJOR})
 
 #--------------------------------------

src/systems/breadcrumbs/Breadcrumbs.cc

@@ -141,13 +141,28 @@ void Breadcrumbs::Configure(const Entity &_entity,
   topics.push_back("/model/" +
       this->model.Name(_ecm) + "/breadcrumbs/" +
       this->modelRoot.ModelByIndex(0)->Name() + "/deploy");
-  auto topic = validTopic(topics);
+  this->topic = validTopic(topics);
 
-  this->node.Subscribe(topic, &Breadcrumbs::OnDeploy, this);
-  this->remainingPub = this->node.Advertise<msgs::Int32>(topic + "/remaining");
+  this->topicStatistics = _sdf->Get<bool>("topic_statistics",
+      this->topicStatistics).first;
 
-  ignmsg << "Breadcrumbs subscribing to deploy messages on [" << topic << "]"
-         << std::endl;
+  // Enable topic statistics when requested.
+  if (this->topicStatistics)
+  {
+    if (!node.EnableStats(this->topic, true))
+    {
+      ignerr << "Unable to enable topic statistics on topic["
+        << this->topic << "]." << std::endl;
+      this->topicStatistics = false;
+    }
+  }
+
+  this->node.Subscribe(this->topic, &Breadcrumbs::OnDeploy, this);
+  this->remainingPub = this->node.Advertise<msgs::Int32>(
+      this->topic + "/remaining");
+
+  ignmsg << "Breadcrumbs subscribing to deploy messages on ["
+    << this->topic << "]" << std::endl;
 
   this->creator = std::make_unique<SdfEntityCreator>(_ecm, _eventMgr);
 
@@ -390,8 +405,32 @@ void Breadcrumbs::OnDeploy(const msgs::Empty &)
   IGN_PROFILE("Breadcrumbs::PreUpdate");
   {
     std::lock_guard<std::mutex> lock(this->pendingCmdsMutex);
+
     this->pendingCmds.push_back(true);
   }
+
+  // Check topic statistics for dropped messages
+  if (this->topicStatistics)
+  {
+    ignmsg << "Received breadcrumb deployment for " <<
+      this->modelRoot.ModelByIndex(0)->Name() << std::endl;
+    std::optional<transport::TopicStatistics> stats =
+      this->node.TopicStats(this->topic);
+    if (stats)
+    {
+      if (stats->DroppedMsgCount() > 0)
+      {
+        ignwarn << "Dropped message count of " << stats->DroppedMsgCount()
+          << " for breadcrumbs on model "
+          << this->modelRoot.ModelByIndex(0)->Name() << std::endl;
+      }
+    }
+    else
+    {
+      ignerr << "Unable to get topic statistics for topic["
+        << this->topic << "]." << std::endl;
+    }
+  }
 }
 
 IGNITION_ADD_PLUGIN(Breadcrumbs,

src/systems/breadcrumbs/Breadcrumbs.hh

@@ -19,6 +19,7 @@
 
 #include <memory>
 #include <set>
+#include <string>
 #include <unordered_map>
 #include <vector>
 
@@ -79,6 +80,9 @@ namespace systems
   /// Defaults to false.
   /// `<breadcrumb>`: This is the model used as a template for deploying
   /// breadcrumbs.
+  /// `<topic_statistics>`: If true, then topic statistics are enabled on
+  /// `<topic>` and error messages will be generated when messages are
+  /// dropped. Default to false.
   class IGNITION_GAZEBO_VISIBLE Breadcrumbs
       : public System,
         public ISystemConfigure,
@@ -163,6 +167,12 @@ namespace systems
 
     /// \brief Publishes remaining deployments.
     public: transport::Node::Publisher remainingPub;
+
+    /// \brief True when topic statistics are enabled.
+    public: bool topicStatistics{false};
+
+    /// \brief Name of the deploy topic.
+    public: std::string topic;
   };
   }
 }