diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index bc99cf7eb1..326cdb1b8d 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -487,6 +487,12 @@ namespace ignition /// \return True if there are any components with one-time changes. public: bool HasOneTimeComponentChanges() const; + /// \brief Get the components types that are marked as periodic changes. + /// \return All the components that at least one entity marked as + /// periodic changes. + public: std::unordered_set + ComponentTypesWithPeriodicChanges() const; + /// \brief Set the absolute state of the ECM from a serialized message. /// Entities / components that are in the new state but not in the old /// one will be created. diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 3c4992b49d..b7aa6e0a6a 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -419,6 +419,18 @@ bool EntityComponentManager::HasOneTimeComponentChanges() const return !this->dataPtr->oneTimeChangedComponents.empty(); } +///////////////////////////////////////////////// +std::unordered_set + EntityComponentManager::ComponentTypesWithPeriodicChanges() const +{ + std::unordered_set periodicComponents; + for (const auto& compPair : this->dataPtr->periodicChangedComponents) + { + periodicComponents.insert(compPair.first); + } + return periodicComponents; +} + ///////////////////////////////////////////////// bool EntityComponentManager::HasEntity(const Entity _entity) const { @@ -1224,8 +1236,22 @@ void EntityComponentManager::SetState( { std::istringstream istr(compMsg.component()); comp->Deserialize(istr); - this->SetChanged(entity, compIter.first, - ComponentState::OneTimeChange); + // Note on merging forward: + // `has_one_time_component_changes` field is available in Edifice so + // this workaround can be removed + auto flag = ComponentState::PeriodicChange; + for (int i = 0; i < _stateMsg.header().data_size(); ++i) + { + if (_stateMsg.header().data(i).key() == + "has_one_time_component_changes") + { + int v = stoi(_stateMsg.header().data(i).value(0)); + if (v) + flag = ComponentState::OneTimeChange; + break; + } + } + this->SetChanged(entity, compIter.first, flag); } } } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index a58a056e63..0ef3587247 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2082,6 +2082,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) auto c2 = manager.CreateComponent(e2, IntComponent(456)); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::OneTimeChange, manager.ComponentState(e1, c1.first)); EXPECT_EQ(ComponentState::OneTimeChange, @@ -2093,6 +2094,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) // updated manager.RunSetAllComponentsUnchanged(); EXPECT_FALSE(manager.HasOneTimeComponentChanges()); + EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, c1.first)); EXPECT_EQ(ComponentState::NoChange, @@ -2100,9 +2102,31 @@ TEST_P(EntityComponentManagerFixture, SetChanged) // Mark as changed manager.SetChanged(e1, c1.first, ComponentState::PeriodicChange); + + // check that only e1 c1 is serialized into a message + msgs::SerializedStateMap stateMsg; + manager.State(stateMsg); + { + ASSERT_EQ(1, stateMsg.entities_size()); + + auto iter = stateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + EXPECT_EQ(e1, e1Msg.id()); + ASSERT_EQ(1, e1Msg.components_size()); + + auto compIter = e1Msg.components().begin(); + const auto &e1c1Msg = compIter->second; + EXPECT_EQ(IntComponent::typeId, e1c1Msg.type()); + EXPECT_EQ(123, std::stoi(e1c1Msg.component())); + } + manager.SetChanged(e2, c2.first, ComponentState::OneTimeChange); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + // Expect a single component type to be marked as PeriodicChange + ASSERT_EQ(1u, manager.ComponentTypesWithPeriodicChanges().size()); + EXPECT_EQ(IntComponent().TypeId(), + *manager.ComponentTypesWithPeriodicChanges().begin()); EXPECT_EQ(ComponentState::PeriodicChange, manager.ComponentState(e1, c1.first)); EXPECT_EQ(ComponentState::OneTimeChange, @@ -2112,6 +2136,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_TRUE(manager.RemoveComponent(e1, c1.first)); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, c1.first)); diff --git a/src/network/NetworkManagerSecondary.cc b/src/network/NetworkManagerSecondary.cc index eaf4f0c28b..46b979fa70 100644 --- a/src/network/NetworkManagerSecondary.cc +++ b/src/network/NetworkManagerSecondary.cc @@ -167,6 +167,12 @@ void NetworkManagerSecondary::OnStep( msgs::SerializedStateMap stateMsg; if (!entities.empty()) this->dataPtr->ecm->State(stateMsg, entities); + // Note on merging forward: + // `has_one_time_component_changes` field is available in Edifice so this + // workaround can be removed + auto data = stateMsg.mutable_header()->add_data(); + data->set_key("has_one_time_component_changes"); + data->add_value(this->dataPtr->ecm->HasOneTimeComponentChanges() ? "1" : "0"); this->stepAckPub.Publish(stateMsg); diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 9dde20341b..722d8f58ac 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -293,12 +293,13 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, { _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); } - // Otherwise publish just selected components + // Otherwise publish just periodic change components else { IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState"); + auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges(); _manager.State(*this->dataPtr->stepMsg.mutable_state(), - {}, {components::Pose::typeId}); + {}, periodicComponents); } // Full state on demand