From 025bb3f5ee91189596519fc9358c2861589d257d Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 6 Dec 2021 13:01:42 -0800 Subject: [PATCH 1/3] Send state message when components are removed Signed-off-by: Nate Koenig --- include/ignition/gazebo/EntityComponentManager.hh | 4 ++++ src/EntityComponentManager.cc | 7 +++++++ src/systems/scene_broadcaster/SceneBroadcaster.cc | 2 +- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index a98c2b854b..e699a65fe7 100644 --- 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 index f0d1b4d4d7..776aef93fb 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -557,6 +557,13 @@ void EntityComponentManager::ClearNewlyCreatedEntities() } } +///////////////////////////////////////////////// +bool EntityComponentManager::HasRemovedComponents() const +{ + std::lock_guard lock(this->dataPtr->removedComponentsMutex); + return !this->dataPtr->removedComponents.empty(); +} + ///////////////////////////////////////////////// void EntityComponentManager::ClearRemovedComponents() { diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index c299ca50f0..484188bdae 100644 --- 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); From b2ad027f44d4b81628a3ae633b2aed3224d9f2a6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 7 Dec 2021 10:33:19 -0800 Subject: [PATCH 2/3] Added tests Signed-off-by: Nate Koenig --- 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(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 #include +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" #include #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( + [&](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(_entity); + } + return true; + }); + } + }); + server.AddSystem(testSystem.systemPtr); + + bool received = false; + bool hasState = false; + std::function 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)); From 326a841f77644ea494202a0bb702d2cdfdeec145 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 7 Dec 2021 16:16:39 -0800 Subject: [PATCH 3/3] Expand test Signed-off-by: Nate Koenig --- test/integration/scene_broadcaster_system.cc | 36 ++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index d0b934474d..2dc83ca890 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -648,7 +648,7 @@ TEST_P(SceneBroadcasterTest, RemovedComponent) if (_name->Data() == "box") { _ecm.RemoveComponent(_entity); - } + } return true; }); } @@ -660,8 +660,40 @@ TEST_P(SceneBroadcasterTest, RemovedComponent) std::function cb = [&](const msgs::SerializedStepMap &_res) { - received = true; hasState = _res.has_state(); + // Check the received state. + if (hasState) + { + ignition::gazebo::EntityComponentManager localEcm; + localEcm.SetState(_res.state()); + bool hasBox = false; + localEcm.Each( + [&](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;