From 17f63c497e7fb81dca5f87708ba8b2128db67b31 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 7 Dec 2021 20:42:09 -0800 Subject: [PATCH] Send state message when components are removed (#1235) * Send state message when components are removed Signed-off-by: Nate Koenig * Added tests Signed-off-by: Nate Koenig * Expand test Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- .../ignition/gazebo/EntityComponentManager.hh | 4 + src/EntityComponentManager.cc | 7 + src/EntityComponentManager_TEST.cc | 2 + .../scene_broadcaster/SceneBroadcaster.cc | 2 +- test/integration/scene_broadcaster_system.cc | 125 ++++++++++++++++++ 5 files changed, 139 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/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/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); diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 3c740bf40c..2dc83ca890 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,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( + [&](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) + { + 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; + 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));