[backport #1131] fix updating a component's data via SerializedState …

…msg (#1149)

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>

src/EntityComponentManager.cc

@@ -17,6 +17,8 @@
 
 #include <map>
 #include <set>
+#include <sstream>
+#include <string>
 #include <unordered_map>
 #include <unordered_set>
 #include <vector>
@@ -1106,49 +1108,51 @@ void EntityComponentManager::SetState(
         continue;
       }
 
-      // Create component
-      auto newComp = components::Factory::Instance()->New(compMsg.type());
-
-      if (nullptr == newComp)
-      {
-        ignerr << "Failed to deserialize component of type [" << compMsg.type()
-               << "]" << std::endl;
-        continue;
-      }
-
-      std::istringstream istr(compMsg.component());
-      newComp->Deserialize(istr);
-
-      // Get type id
-      auto typeId = newComp->TypeId();
-
-      // TODO(louise) Move into if, see TODO below
-      this->RemoveComponent(entity, typeId);
-
       // Remove component
       if (compMsg.remove())
       {
+        this->RemoveComponent(entity, type);
         continue;
       }
 
       // Get Component
-      auto comp = this->ComponentImplementation(entity, typeId);
+      auto comp = this->ComponentImplementation(entity, type);
 
       // Create if new
       if (nullptr == comp)
       {
-        this->CreateComponentImplementation(entity, typeId, newComp.get());
+        auto newComp = components::Factory::Instance()->New(type);
+        if (nullptr == newComp)
+        {
+          ignerr << "Failed to create component type ["
+            << compMsg.type() << "]" << std::endl;
+          continue;
+        }
+        std::istringstream istr(compMsg.component());
+        newComp->Deserialize(istr);
+        this->CreateComponentImplementation(entity, type, newComp.get());
       }
       // Update component value
       else
       {
-        ignerr << "Internal error" << std::endl;
-        // TODO(louise) We're shortcutting above and always  removing the
-        // component so that we don't get here, gotta figure out why this
-        // doesn't update the component. The following line prints the correct
-        // values.
-        // igndbg << *comp << "  " << *newComp.get() << std::endl;
-        // *comp = *newComp.get();
+        std::istringstream istr(compMsg.component());
+        comp->Deserialize(istr);
+        // note on merging forward:
+        // `AddModifiedComponent` method is available/used in ign-gazebo4, so we
+        // don't need this header patch when merging forward
+        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, type, flag);
       }
     }
   }

src/EntityComponentManager_TEST.cc

@@ -2198,6 +2198,102 @@ TEST_P(EntityComponentManagerFixture, SetEntityCreateOffset)
   EXPECT_EQ(501u, entity3);
 }
 
+//////////////////////////////////////////////////
+/// \brief Test using msgs::SerializedStateMap and msgs::SerializedState
+/// to update existing component data between multiple ECMs
+TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent)
+{
+  // create 2 ECMs: one will be modified directly, and the other should be
+  // updated to match the first via msgs::SerializedStateMap
+  EntityComponentManager originalECMStateMap;
+  EntityComponentManager otherECMStateMap;
+
+  // create an entity and component
+  auto entity = originalECMStateMap.CreateEntity();
+  originalECMStateMap.CreateComponent(entity, components::IntComponent(1));
+
+  int foundEntities = 0;
+  otherECMStateMap.Each<components::IntComponent>(
+      [&](const Entity &, const components::IntComponent *)
+      {
+        foundEntities++;
+        return true;
+      });
+  EXPECT_EQ(0, foundEntities);
+
+  // update the other ECM to have the new entity and component
+  msgs::SerializedStateMap stateMapMsg;
+  originalECMStateMap.State(stateMapMsg);
+  otherECMStateMap.SetState(stateMapMsg);
+  foundEntities = 0;
+  otherECMStateMap.Each<components::IntComponent>(
+      [&](const Entity &, const components::IntComponent *_intComp)
+      {
+        foundEntities++;
+        EXPECT_EQ(1, _intComp->Data());
+        return true;
+      });
+  EXPECT_EQ(1, foundEntities);
+
+  // modify a component and then share the update with the other ECM
+  stateMapMsg.Clear();
+  originalECMStateMap.SetComponentData<components::IntComponent>(entity, 2);
+  originalECMStateMap.State(stateMapMsg);
+  otherECMStateMap.SetState(stateMapMsg);
+  foundEntities = 0;
+  otherECMStateMap.Each<components::IntComponent>(
+      [&](const Entity &, const components::IntComponent *_intComp)
+      {
+        foundEntities++;
+        EXPECT_EQ(2, _intComp->Data());
+        return true;
+      });
+  EXPECT_EQ(1, foundEntities);
+
+  // Run the same test as above, but this time, use a msgs::SerializedState
+  // instead of a msgs::SerializedStateMap
+  EntityComponentManager originalECMState;
+  EntityComponentManager otherECMState;
+
+  foundEntities = 0;
+  otherECMState.Each<components::IntComponent>(
+      [&](const Entity &, const components::IntComponent *)
+      {
+        foundEntities++;
+        return true;
+      });
+  EXPECT_EQ(0, foundEntities);
+
+  entity = originalECMState.CreateEntity();
+  originalECMState.CreateComponent(entity, components::IntComponent(1));
+
+  auto stateMsg = originalECMState.State();
+  otherECMState.SetState(stateMsg);
+  foundEntities = 0;
+  otherECMState.Each<components::IntComponent>(
+      [&](const Entity &, const components::IntComponent *_intComp)
+      {
+        foundEntities++;
+        EXPECT_EQ(1, _intComp->Data());
+        return true;
+      });
+  EXPECT_EQ(1, foundEntities);
+
+  stateMsg.Clear();
+  originalECMState.SetComponentData<components::IntComponent>(entity, 2);
+  stateMsg = originalECMState.State();
+  otherECMState.SetState(stateMsg);
+  foundEntities = 0;
+  otherECMState.Each<components::IntComponent>(
+      [&](const Entity &, const components::IntComponent *_intComp)
+      {
+        foundEntities++;
+        EXPECT_EQ(2, _intComp->Data());
+        return true;
+      });
+  EXPECT_EQ(1, foundEntities);
+}
+
 // Run multiple times. We want to make sure that static globals don't cause
 // problems.
 INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat,