SceneBroadcaster: only send changed state information for change even…

…ts (#1392) Signed-off-by: Ashton Larkin <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
gazebosim · Mar 28, 2022 · 0996fe0 · 0996fe0
1 parent 9b07b24
commit 0996fe0
Show file tree

Hide file tree

Showing 4 changed files with 433 additions and 34 deletions.
diff --git 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
@@ -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
@@ -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<ignition::gazebo::components::Model,
                   ignition::gazebo::components::Name,
@@ -655,21 +660,89 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent))
             return true;
           });
       }
+      // add a component to an entity
+      else if (_info.iterations == 3)
+      {
+        auto boxEntity = _ecm.EntityByComponents(
+            gazebo::components::Name("box"), gazebo::components::Model());
+        ASSERT_NE(gazebo::kNullEntity, boxEntity);
+        EXPECT_FALSE(_ecm.EntityHasComponentType(boxEntity,
+              ignition::gazebo::components::Pose::typeId));
+        _ecm.CreateComponent<ignition::gazebo::components::Pose>(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<gazebo::components::Name>(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<gazebo::components::Name>(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<void(const msgs::SerializedStepMap &)> 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<ignition::gazebo::components::Model,
                   ignition::gazebo::components::Name>(
           [&](const ignition::gazebo::Entity &_entity,
@@ -679,66 +752,126 @@ 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<ignition::gazebo::components::Model,
+                    ignition::gazebo::components::Name>(
+            [&](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;
   };
 
   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<void(bool)> 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