diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 5db412ca6d..610808bd65 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -1809,6 +1809,8 @@ void EntityComponentManager::SetState( components::BaseComponent *comp = this->ComponentImplementation(entity, compIter.first); + std::istringstream istr(compMsg.component()); + // Create if new if (nullptr == comp) { @@ -1822,7 +1824,6 @@ void EntityComponentManager::SetState( continue; } - std::istringstream istr(compMsg.component()); newComp->Deserialize(istr); this->CreateComponentImplementation(entity, @@ -1831,7 +1832,6 @@ void EntityComponentManager::SetState( // Update component value else { - std::istringstream istr(compMsg.component()); comp->Deserialize(istr); this->SetChanged(entity, compIter.first, _stateMsg.has_one_time_component_changes() ? diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 800d338c35..ad168d2811 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -329,11 +329,16 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, set(this->dataPtr->stepMsg.mutable_stats(), _info); - // Publish full state if there are change events - if (changeEvent || this->dataPtr->stateServiceRequest) + // Publish full state if it has been explicitly requested + if (this->dataPtr->stateServiceRequest) { _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); } + // Publish the changed state if a change occurred to the ECS + else if (changeEvent) + { + _manager.ChangedState(*this->dataPtr->stepMsg.mutable_state()); + } // Otherwise publish just periodic change components when running else if (!_info.paused) { diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 9775f23864..b62b4d1995 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -620,13 +620,15 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(StateStatic)) } ///////////////////////////////////////////////// -/// Test whether the scene topic is published when a component is removed. -TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) +/// Test whether the scene topic is published when entities and components are +/// removed/added +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(AddRemoveEntitiesComponents)) { // Start server ignition::gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/shapes.sdf"); + "/test/worlds/shapes_scene_broadcaster_only.sdf"); gazebo::Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -638,7 +640,10 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) testSystem.OnUpdate([](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) { - if (_info.iterations > 1) + static bool periodicChangeMade = false; + + // remove a component from an entity + if (_info.iterations == 2) { _ecm.Each(boxEntity, + ignition::gazebo::components::Pose({1, 2, 3, 4, 5, 6})); + EXPECT_TRUE(_ecm.EntityHasComponentType(boxEntity, + ignition::gazebo::components::Pose::typeId)); + } + // remove an entity + else if (_info.iterations == 4) + { + auto boxEntity = _ecm.EntityByComponents( + gazebo::components::Name("box"), gazebo::components::Model()); + ASSERT_NE(gazebo::kNullEntity, boxEntity); + _ecm.RequestRemoveEntity(boxEntity); + } + // create an entity + else if (_info.iterations == 5) + { + EXPECT_EQ(gazebo::kNullEntity, _ecm.EntityByComponents( + gazebo::components::Name("newEntity"), + gazebo::components::Model())); + auto newEntity = _ecm.CreateEntity(); + _ecm.CreateComponent(newEntity, gazebo::components::Name("newEntity")); + _ecm.CreateComponent(newEntity, gazebo::components::Model()); + EXPECT_NE(gazebo::kNullEntity, _ecm.EntityByComponents( + gazebo::components::Name("newEntity"), + gazebo::components::Model())); + } + // modify an existing component via OneTimeChange + else if (_info.iterations == 6) + { + auto entity = _ecm.EntityByComponents( + gazebo::components::Name("newEntity"), + gazebo::components::Model()); + ASSERT_NE(gazebo::kNullEntity, entity); + EXPECT_TRUE(_ecm.SetComponentData(entity, + "newEntity1")); + _ecm.SetChanged(entity, gazebo::components::Name::typeId, + gazebo::ComponentState::OneTimeChange); + } + // modify an existing component via PeriodicChange + else if (_info.iterations > 6 && !periodicChangeMade) + { + auto entity = _ecm.EntityByComponents( + gazebo::components::Name("newEntity1"), + gazebo::components::Model()); + ASSERT_NE(gazebo::kNullEntity, entity); + EXPECT_TRUE(_ecm.SetComponentData(entity, + "newEntity2")); + _ecm.SetChanged(entity, gazebo::components::Name::typeId, + gazebo::ComponentState::PeriodicChange); + periodicChangeMade = true; + } }); server.AddSystem(testSystem.systemPtr); + int receivedStates = 0; bool received = false; bool hasState = false; + ignition::gazebo::EntityComponentManager localEcm; std::function cb = [&](const msgs::SerializedStepMap &_res) { + hasState = _res.has_state(); // Check the received state. if (hasState) { - ignition::gazebo::EntityComponentManager localEcm; + receivedStates++; + localEcm.SetState(_res.state()); bool hasBox = false; + bool hasNewEntity = false; + bool hasModifiedComponent = false; + bool newEntityIteration = _res.stats().iterations() == 5; + bool oneTimeChangeIteration = _res.stats().iterations() == 6; + bool periodicChangeIteration = _res.stats().iterations() > 7; localEcm.Each( [&](const ignition::gazebo::Entity &_entity, @@ -679,22 +752,52 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) if (_name->Data() == "box") { hasBox = true; - if (_res.stats().iterations() > 1) + if (_res.stats().iterations() != 2) { - // The pose component should be gone - EXPECT_FALSE(localEcm.EntityHasComponentType( + // The pose component should exist + EXPECT_TRUE(localEcm.EntityHasComponentType( _entity, ignition::gazebo::components::Pose::typeId)); } else { - // The pose component should exist - EXPECT_TRUE(localEcm.EntityHasComponentType( + // The pose component should be gone + EXPECT_FALSE(localEcm.EntityHasComponentType( _entity, ignition::gazebo::components::Pose::typeId)); } } + + if (newEntityIteration && _name->Data() == "newEntity") + hasNewEntity = true; + + if (oneTimeChangeIteration && _name->Data() == "newEntity1") + hasModifiedComponent = true; + else if (periodicChangeIteration && _name->Data() == "newEntity2") + hasModifiedComponent = true; + return true; }); + + // make sure that the box entity is marked as removed + if (_res.stats().iterations() >= 4) + { + bool markedAsRemoved = false; + localEcm.EachRemoved( + [&](const ignition::gazebo::Entity &, + const ignition::gazebo::components::Model *, + const ignition::gazebo::components::Name *_name)->bool + { + if (_name->Data() == "box") + markedAsRemoved = true; + return true; + }); + EXPECT_TRUE(markedAsRemoved); + } + EXPECT_TRUE(hasBox); + EXPECT_EQ(newEntityIteration, hasNewEntity); + EXPECT_EQ(periodicChangeIteration || oneTimeChangeIteration, + hasModifiedComponent); } received = true; }; @@ -702,43 +805,73 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) transport::Node node; EXPECT_TRUE(node.Subscribe("/world/default/state", cb)); - unsigned int sleep = 0u; - unsigned int maxSleep = 30u; + // Helper method that runs the server one iteration and then checks that + // received data was processed correctly. + // The _shouldHaveState parameter defines whether the published + // msgs::SerializedStepMap should contain state info or not + std::function runServerOnce = + [&](bool _shouldHaveState) + { + unsigned int sleep = 0u; + unsigned int maxSleep = 30u; + 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_EQ(_shouldHaveState, hasState); + }; // 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); + runServerOnce(true); // Run server again. The second time shouldn't have state info. The // message can still arrive due the passage of time (see `itsPubTime` in // SceneBroadcaster::PostUpdate. - 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(hasState); + runServerOnce(false); // Run server again. The third time should send the state message because // the test system removed a component. - sleep = 0u; + runServerOnce(true); + + // Run server again. The fourth time should send the state message because + // the test system added a component. + runServerOnce(true); + + // Run server again. The fifth time should send the state message because + // the test system requested to remove an entity. + runServerOnce(true); + + // Run server again. The sixth time should send the state message because + // the test system created an entity. + runServerOnce(true); + + // Run server again. The seventh time should send the state message because + // the test system modified a component and marked it as a OneTimeChange. + runServerOnce(true); + + // Run server for a few iterations to make sure that the periodic change + // made by the test system is received. received = false; hasState = false; - server.RunOnce(true); + server.Run(true, 10, false); + // (wait for a bit after running the server in case ign-transport is still + // processing messages) + unsigned int sleep = 0u; + unsigned int maxSleep = 50u; // cppcheck-suppress unmatchedSuppression // cppcheck-suppress knownConditionTrueFalse while (!received && sleep++ < maxSleep) IGN_SLEEP_MS(100); EXPECT_TRUE(received); EXPECT_TRUE(hasState); + + // Sanity check: make sure that at least 7 states were received and processed + EXPECT_GE(receivedStates, 7); } // Run multiple times diff --git a/test/worlds/shapes_scene_broadcaster_only.sdf b/test/worlds/shapes_scene_broadcaster_only.sdf new file mode 100644 index 0000000000..dff73115c5 --- /dev/null +++ b/test/worlds/shapes_scene_broadcaster_only.sdf @@ -0,0 +1,261 @@ + + + + + 0.001 + 0 + + + + + 1000 + + + + + + + + 3D View + false + docked + + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 1 2 3 0 0 1 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + 0.1 0.1 0.1 0 0 0 + + 0.11 0.11 0.11 0 0 0 + + + 3 4 5 + + + + + + 1150 + 0.12 0.12 0.12 0 0 0 + + + 1 2 3 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + -1 -2 -3 0 0 1 + + 0.2 0.2 0.2 0 0 0 + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + 0.21 0.21 0.21 0 0 0 + + + 0.2 + 0.1 + + + + + + 1654 + 0.22 0.22 0.22 0 0 0 + + + 2.1 + 10.2 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 0 0 0 0 1 + + 0.3 0.3 0.3 0 0 0 + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + 0.31 0.31 0.31 0 0 0 + + + 23.4 + + + + + + 50 + 0.32 0.32 0.32 0 0 0 + 0.5 + false + + + 1.2 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + -4 -5 -6 0 0 1 + + 0.5 0.5 0.5 0 0 0 + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + 0.51 0.51 0.51 0 0 0 + + + 0.23 + 0.14 + + + + + 6.54 + 0.52 0.52 0.52 0 0 0 + + + 2.12 + 1.23 + + + + 0 0 1 1 + 0 0 1 1 + 0 1 0 1 + + + + + + + 4 5 6 0 0 1 + + 0.8 0.8 0.8 0 0 0 + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + 0.81 0.81 0.81 0 0 0 + + + 0.4 0.6 1.6 + + + + + 3.21 + 0.82 0.82 0.82 0 0 0 + 0.5 + false + + + 0.4 0.6 1.6 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + + +