Send state message when components are removed (#1235)

* Send state message when components are removed Signed-off-by: Nate Koenig <[email protected]> * Added tests Signed-off-by: Nate Koenig <[email protected]> * Expand test Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
gazebosim · Dec 8, 2021 · 17f63c4 · 17f63c4
1 parent 4152bdf
commit 17f63c4
Show file tree

Hide file tree

Showing 5 changed files with 139 additions and 1 deletion.
diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh
@@ -669,6 +669,10 @@ namespace ignition
       /// \param[in] _offset Offset value.
       public: void SetEntityCreateOffset(uint64_t _offset);
 
+      /// \brief Return true if there are components marked for removal.
+      /// \return True if there are components marked for removal.
+      public: bool HasRemovedComponents() const;
+
       /// \brief Clear the list of newly added entities so that a call to
       /// EachAdded after this will have no entities to iterate. This function
       /// is protected to facilitate testing.

diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc
@@ -557,6 +557,13 @@ void EntityComponentManager::ClearNewlyCreatedEntities()
   }
 }
 
+/////////////////////////////////////////////////
+bool EntityComponentManager::HasRemovedComponents() const
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->removedComponentsMutex);
+  return !this->dataPtr->removedComponents.empty();
+}
+
 /////////////////////////////////////////////////
 void EntityComponentManager::ClearRemovedComponents()
 {

diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc
@@ -125,6 +125,7 @@ TEST_P(EntityComponentManagerFixture, InvalidComponentType)
   EXPECT_EQ(2u, manager.CreateEntity());
   EXPECT_TRUE(manager.HasEntity(2));
   EXPECT_FALSE(manager.RemoveComponent(2, IntComponent::typeId));
+  EXPECT_FALSE(manager.HasRemovedComponents());
 
   // We should get a nullptr if the component type doesn't exist.
   EXPECT_TRUE(manager.HasEntity(1u));
@@ -182,6 +183,7 @@ TEST_P(EntityComponentManagerFixture, RemoveComponent)
   EXPECT_FALSE(manager.EntityHasComponentType(eInt, IntComponent::typeId));
   EXPECT_TRUE(manager.ComponentTypes(eInt).empty());
   EXPECT_EQ(nullptr, manager.Component<IntComponent>(eInt));
+  EXPECT_TRUE(manager.HasRemovedComponents());
 
   EXPECT_TRUE(manager.RemoveComponent(eDouble, DoubleComponent::typeId));
   EXPECT_FALSE(manager.EntityHasComponentType(eDouble,

diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc
@@ -284,7 +284,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info,
   bool jumpBackInTime = _info.dt < std::chrono::steady_clock::duration::zero();
   bool changeEvent = _manager.HasEntitiesMarkedForRemoval() ||
     _manager.HasNewEntities() || _manager.HasOneTimeComponentChanges() ||
-    jumpBackInTime;
+    jumpBackInTime || _manager.HasRemovedComponents();
   auto now = std::chrono::system_clock::now();
   bool itsPubTime = !_info.paused && (now - this->dataPtr->lastStatePubTime >
        this->dataPtr->statePublishPeriod);

diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc
@@ -22,12 +22,16 @@
 
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
+#include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/Pose.hh"
 #include <ignition/transport/Node.hh>
 
 #include "ignition/gazebo/Server.hh"
 #include "ignition/gazebo/test_config.hh"
 
 #include "../helpers/EnvTestFixture.hh"
+#include "../helpers/Relay.hh"
 
 using namespace ignition;
 
@@ -612,6 +616,127 @@ TEST_P(SceneBroadcasterTest, StateStatic)
   EXPECT_TRUE(received);
 }
 
+/////////////////////////////////////////////////
+/// Test whether the scene topic is published when a component is removed.
+TEST_P(SceneBroadcasterTest, RemovedComponent)
+{
+  // Start server
+  ignition::gazebo::ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+                          "/test/worlds/shapes.sdf");
+
+  gazebo::Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Create a system that removes a component
+  ignition::gazebo::test::Relay testSystem;
+
+  testSystem.OnUpdate([](const gazebo::UpdateInfo &_info,
+    gazebo::EntityComponentManager &_ecm)
+    {
+      if (_info.iterations > 1)
+      {
+        _ecm.Each<ignition::gazebo::components::Model,
+                  ignition::gazebo::components::Name,
+                  ignition::gazebo::components::Pose>(
+          [&](const ignition::gazebo::Entity &_entity,
+              const ignition::gazebo::components::Model *,
+              const ignition::gazebo::components::Name *_name,
+              const ignition::gazebo::components::Pose *)->bool
+          {
+            if (_name->Data() == "box")
+            {
+              _ecm.RemoveComponent<ignition::gazebo::components::Pose>(_entity);
+            }
+            return true;
+          });
+      }
+    });
+  server.AddSystem(testSystem.systemPtr);
+
+  bool received = false;
+  bool hasState = false;
+  std::function<void(const msgs::SerializedStepMap &)> cb =
+      [&](const msgs::SerializedStepMap &_res)
+  {
+    hasState = _res.has_state();
+    // Check the received state.
+    if (hasState)
+    {
+      ignition::gazebo::EntityComponentManager localEcm;
+      localEcm.SetState(_res.state());
+      bool hasBox = false;
+      localEcm.Each<ignition::gazebo::components::Model,
+                  ignition::gazebo::components::Name>(
+          [&](const ignition::gazebo::Entity &_entity,
+              const ignition::gazebo::components::Model *,
+              const ignition::gazebo::components::Name *_name)->bool
+          {
+            if (_name->Data() == "box")
+            {
+              hasBox = true;
+              if (_res.stats().iterations() > 1)
+              {
+                // The pose component should be gone
+                EXPECT_FALSE(localEcm.EntityHasComponentType(
+                      _entity, ignition::gazebo::components::Pose::typeId));
+              }
+              else
+              {
+                // The pose component should exist
+                EXPECT_TRUE(localEcm.EntityHasComponentType(
+                      _entity, ignition::gazebo::components::Pose::typeId));
+              }
+            }
+            return true;
+          });
+      EXPECT_TRUE(hasBox);
+    }
+    received = true;
+  };
+
+  transport::Node node;
+  EXPECT_TRUE(node.Subscribe("/world/default/state", cb));
+
+  unsigned int sleep = 0u;
+  unsigned int maxSleep = 30u;
+
+  // Run server once. The first time should send the state message
+  server.RunOnce(true);
+  // cppcheck-suppress unmatchedSuppression
+  // cppcheck-suppress knownConditionTrueFalse
+  while (!received && sleep++ < maxSleep)
+    IGN_SLEEP_MS(100);
+  EXPECT_TRUE(received);
+  EXPECT_TRUE(hasState);
+
+  // Run server again. The second time shouldn't send the state message.
+  sleep = 0u;
+  received = false;
+  hasState = false;
+  server.RunOnce(true);
+  // cppcheck-suppress unmatchedSuppression
+  // cppcheck-suppress knownConditionTrueFalse
+  while (!received && sleep++ < maxSleep)
+    IGN_SLEEP_MS(100);
+  EXPECT_FALSE(received);
+  EXPECT_FALSE(hasState);
+
+  // Run server again. The third time should send the state message because
+  // the test system removed a component.
+  sleep = 0u;
+  received = false;
+  hasState = false;
+  server.RunOnce(true);
+  // cppcheck-suppress unmatchedSuppression
+  // cppcheck-suppress knownConditionTrueFalse
+  while (!received && sleep++ < maxSleep)
+    IGN_SLEEP_MS(100);
+  EXPECT_TRUE(received);
+  EXPECT_TRUE(hasState);
+}
+
 // Run multiple times
 INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest,
     ::testing::Range(1, 2));