Publish all periodic change components in Scene Broadcaster (gazebosi…

…m#544) Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
playertr · Jan 25, 2021 · afb228d · afb228d
1 parent 8998740
commit afb228d
Show file tree

Hide file tree

Showing 4 changed files with 46 additions and 2 deletions.
diff --git 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<ComponentTypeId>
+          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
@@ -419,6 +419,18 @@ bool EntityComponentManager::HasOneTimeComponentChanges() const
   return !this->dataPtr->oneTimeChangedComponents.empty();
 }
 
+/////////////////////////////////////////////////
+std::unordered_set<ComponentTypeId>
+    EntityComponentManager::ComponentTypesWithPeriodicChanges() const
+{
+  std::unordered_set<ComponentTypeId> periodicComponents;
+  for (const auto& compPair : this->dataPtr->periodicChangedComponents)
+  {
+    periodicComponents.insert(compPair.first);
+  }
+  return periodicComponents;
+}
+
 /////////////////////////////////////////////////
 bool EntityComponentManager::HasEntity(const Entity _entity) const
 {

diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc
@@ -2082,6 +2082,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged)
   auto c2 = manager.CreateComponent<IntComponent>(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,16 +2094,39 @@ 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,
       manager.ComponentState(e2, c2.first));
 
   // 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/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