Publish remaining breadcrumb deployments. (#308)

* Publish remaining breadcrumb deployments Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
gazebosim · Aug 27, 2020 · f05fb0b · f05fb0b
1 parent 865e77d
commit f05fb0b
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 1 deletion.
diff --git a/src/systems/breadcrumbs/Breadcrumbs.cc b/src/systems/breadcrumbs/Breadcrumbs.cc
@@ -130,6 +130,7 @@ void Breadcrumbs::Configure(const Entity &_entity,
     topic = _sdf->Get<std::string>("topic");
 
   this->node.Subscribe(topic, &Breadcrumbs::OnDeploy, this);
+  this->remainingPub = this->node.Advertise<msgs::Int32>(topic + "/remaining");
 
   ignmsg << "Breadcrumbs subscribing to deploy messages on [" << topic << "]"
          << std::endl;
@@ -242,6 +243,11 @@ void Breadcrumbs::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
                << " but the maximum number of deployments has reached the "
                << "limit of " << this->maxDeployments << std::endl;
       }
+
+      // Publish remaining deployments
+      msgs::Int32 remainingMsg;
+      remainingMsg.set_data(this->maxDeployments - this->numDeployments);
+      this->remainingPub.Publish(remainingMsg);
     }
 
     std::set<Entity> processedEntities;

diff --git a/src/systems/breadcrumbs/Breadcrumbs.hh b/src/systems/breadcrumbs/Breadcrumbs.hh
@@ -63,7 +63,8 @@ namespace systems
   /// have no effect. If a negative number is set, the maximum deployment will
   /// be unbounded. If a value of zero is used, then the breadcrumb system will
   /// be disabled. A zero value is useful for situations where SDF files are
-  /// programmatically created.
+  /// programmatically created. The remaining deployment count is available on
+  /// the `<topic>/remaining` topic.
   /// `<disable_physics_time>`: The time in which the breadcrumb entity's
   /// dynamics remain enabled. After his specified time, the breadcrumb will
   /// be made static. If this value is <= 0 or the param is not specified, the
@@ -158,6 +159,9 @@ namespace systems
 
     /// \brief SDF DOM of a static model with empty link
     private: sdf::Model staticModelToSpawn;
+
+    /// \brief Publishes remaining deployments.
+    public: transport::Node::Publisher remainingPub;
   };
   }
 }

diff --git a/test/integration/breadcrumbs.cc b/test/integration/breadcrumbs.cc
@@ -66,6 +66,43 @@ class BreadcrumbsTest : public ::testing::Test
   public: std::unique_ptr<Server> server;
 };
 
+int kRemaining;
+
+/////////////////////////////////////////////////
+void remainingCb(const msgs::Int32 &_msg)
+{
+  kRemaining = _msg.data();
+}
+
+/////////////////////////////////////////////////
+// This test checks the .../deploy/remaining topic
+TEST_F(BreadcrumbsTest, Remaining)
+{
+  // Start server
+  this->LoadWorld("test/worlds/breadcrumbs.sdf");
+  kRemaining = 0;
+
+  test::Relay testSystem;
+  transport::Node node;
+  auto deployB1 =
+      node.Advertise<msgs::Empty>("/model/vehicle_blue/breadcrumbs/B1/deploy");
+  node.Subscribe("/model/vehicle_blue/breadcrumbs/B1/deploy/remaining",
+      &remainingCb);
+  EXPECT_EQ(0, kRemaining);
+  deployB1.Publish(msgs::Empty());
+  this->server->Run(true, 1, false);
+  EXPECT_EQ(2, kRemaining);
+  deployB1.Publish(msgs::Empty());
+  this->server->Run(true, 1, false);
+  EXPECT_EQ(1, kRemaining);
+  deployB1.Publish(msgs::Empty());
+  this->server->Run(true, 1, false);
+  EXPECT_EQ(0, kRemaining);
+  deployB1.Publish(msgs::Empty());
+  this->server->Run(true, 1, false);
+  EXPECT_EQ(0, kRemaining);
+}
+
 /////////////////////////////////////////////////
 // The test checks breadcrumbs are deployed at the correct pose
 TEST_F(BreadcrumbsTest, DeployAtOffset)
@@ -185,6 +222,7 @@ TEST_F(BreadcrumbsTest, MaxDeployments)
   this->server->Run(true, iterTestStart + 5001, false);
 }
 
+
 /////////////////////////////////////////////////
 // The test checks that including models from fuel works. Also checks custom
 // topic