From b2ad027f44d4b81628a3ae633b2aed3224d9f2a6 Mon Sep 17 00:00:00 2001
From: Nate Koenig <nate@openrobotics.org>
Date: Tue, 7 Dec 2021 10:33:19 -0800
Subject: [PATCH] Added tests

Signed-off-by: Nate Koenig <nate@openrobotics.org>
---
 src/EntityComponentManager_TEST.cc           |  2 +
 test/integration/scene_broadcaster_system.cc | 93 ++++++++++++++++++++
 2 files changed, 95 insertions(+)

diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc
index ed45452d66..cd0b8dcaf6 100644
--- 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/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc
index 3c740bf40c..d0b934474d 100644
--- 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,95 @@ 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)
+  {
+    received = true;
+    hasState = _res.has_state();
+  };
+
+  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));