Fix component updates (#1580)

Signed-off-by: Jenn Nguyen <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>

src/EntityComponentManager.cc

@@ -493,7 +493,17 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent,
  auto originalComp = this->ComponentImplementation(_entity, type);
  auto clonedComp = originalComp->Clone();
 
- this->CreateComponentImplementation(clonedEntity, type, clonedComp.get());
+ auto updateData =
+ this->CreateComponentImplementation(clonedEntity, type, clonedComp.get());
+ if (updateData)
+ {
+ // When a cloned entity is removed, it erases all components/data so a new
+ // cloned entity should not have components to be updated
+ // LCOV_EXCL_START
+ ignerr << "Internal error: The component's data needs to be updated but "
+ << "this should not happen." << std::endl;
+ // LCOV_EXCL_STOP
+ }
  }
 
  // keep track of canonical link information (for clones of models, the cloned
@@ -1726,11 +1736,11 @@ void EntityComponentManager::SetState(
  // Get Component
  auto comp = this->ComponentImplementation(entity, type);
 
- std::istringstream istr(compMsg.component());
-
  // Create if new
  if (nullptr == comp)
  {
+ std::istringstream istr(compMsg.component());
+
  auto newComp = components::Factory::Instance()->New(type);
  if (nullptr == newComp)
  {
@@ -1739,11 +1749,20 @@ void EntityComponentManager::SetState(
  continue;
  }
  newComp->Deserialize(istr);
- this->CreateComponentImplementation(entity, type, newComp.get());
+
+ auto updateData =
+ this->CreateComponentImplementation(entity, type, newComp.get());
+ if (updateData)
+ {
+ // Set comp so we deserialize the data below again
+ comp = this->ComponentImplementation(entity, type);
+ }
  }
+
  // Update component value
- else
+ if (comp)
  {
+ std::istringstream istr(compMsg.component());
  comp->Deserialize(istr);
  this->dataPtr->AddModifiedComponent(entity);
  }
@@ -1809,29 +1828,34 @@ void EntityComponentManager::SetState(
  components::BaseComponent *comp =
  this->ComponentImplementation(entity, compIter.first);
 
- std::istringstream istr(compMsg.component());
-
  // Create if new
  if (nullptr == comp)
  {
+ std::istringstream istr(compMsg.component());
+
  // Create component
  auto newComp = components::Factory::Instance()->New(compMsg.type());
-
  if (nullptr == newComp)
  {
  ignerr << "Failed to create component of type [" << compMsg.type()
  << "]" << std::endl;
  continue;
  }
-
  newComp->Deserialize(istr);
 
- this->CreateComponentImplementation(entity,
- newComp->TypeId(), newComp.get());
+ auto updateData = this->CreateComponentImplementation(
+ entity, newComp->TypeId(), newComp.get());
+ if (updateData)
+ {
+ // Set comp so we deserialize the data below again
+ comp = this->ComponentImplementation(entity, compIter.first);
+ }
  }
+
  // Update component value
- else
+ if (comp)
  {
+ std::istringstream istr(compMsg.component());
  comp->Deserialize(istr);
  this->SetChanged(entity, compIter.first,
  _stateMsg.has_one_time_component_changes() ?

src/EntityComponentManager_TEST.cc

@@ -3161,6 +3161,94 @@ TEST_P(EntityComponentManagerFixture,
  EXPECT_EQ(1, foundEntities);
 }
 
+//////////////////////////////////////////////////
+TEST_P(EntityComponentManagerFixture,
+ IGN_UTILS_TEST_DISABLED_ON_WIN32(AddRemoveAddComponentsStateMap))
+{
+ Entity e1 = manager.CreateEntity();
+ EXPECT_EQ(1u, manager.EntityCount());
+ EXPECT_EQ(0, eachCount<IntComponent>(manager));
+
+ // add a component
+ auto comp = manager.CreateComponent<IntComponent>(e1, IntComponent(123));
+ ASSERT_NE(nullptr, comp);
+ EXPECT_EQ(1, eachCount<IntComponent>(manager));
+ EXPECT_EQ(123, comp->Data());
+
+ // Serialize into a message
+ msgs::SerializedStateMap stateMsg;
+ manager.State(stateMsg);
+ ASSERT_EQ(1, stateMsg.entities_size());
+
+ // remove a component
+ EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId));
+ EXPECT_EQ(nullptr, manager.Component<IntComponent>(e1));
+ manager.RunClearNewlyCreatedEntities();
+ auto changedStateMsg = manager.ChangedState();
+ EXPECT_EQ(0, changedStateMsg.entities_size());
+
+ // add same type of component back in using SetState
+ auto iter = stateMsg.mutable_entities()->find(e1);
+ ASSERT_TRUE(iter != stateMsg.mutable_entities()->end());
+ msgs::SerializedEntityMap &e1Msg = iter->second;
+
+ auto compIter = e1Msg.mutable_components()->find(comp->TypeId());
+ ASSERT_TRUE(compIter != e1Msg.mutable_components()->end());
+ msgs::SerializedComponent &e1c1Msg = compIter->second;
+ e1c1Msg.set_component(std::to_string(321));
+ (*e1Msg.mutable_components())[e1c1Msg.type()] = e1c1Msg;
+ (*stateMsg.mutable_entities())[static_cast<int64_t>(e1)] = e1Msg;
+ manager.SetState(stateMsg);
+ changedStateMsg = manager.ChangedState();
+ EXPECT_EQ(1, changedStateMsg.entities_size());
+
+ // check component
+ comp = manager.Component<IntComponent>(e1);
+ ASSERT_NE(nullptr, comp);
+ EXPECT_EQ(321, comp->Data());
+}
+
+//////////////////////////////////////////////////
+TEST_P(EntityComponentManagerFixture,
+ IGN_UTILS_TEST_DISABLED_ON_WIN32(AddRemoveAddComponentsState))
+{
+ Entity e1 = manager.CreateEntity();
+ EXPECT_EQ(1u, manager.EntityCount());
+ EXPECT_EQ(0, eachCount<IntComponent>(manager));
+
+ // add a component
+ auto comp = manager.CreateComponent<IntComponent>(e1, IntComponent(123));
+ ASSERT_NE(nullptr, comp);
+ EXPECT_EQ(1, eachCount<IntComponent>(manager));
+ EXPECT_EQ(123, comp->Data());
+
+ // Serialize into a message
+ msgs::SerializedState stateMsg = manager.State();
+ ASSERT_EQ(1, stateMsg.entities_size());
+
+ // remove a component
+ EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId));
+ EXPECT_EQ(nullptr, manager.Component<IntComponent>(e1));
+ manager.RunClearNewlyCreatedEntities();
+ auto changedStateMsg = manager.ChangedState();
+ EXPECT_EQ(0, changedStateMsg.entities_size());
+
+ // add same type of component back in using SetState
+ auto entityMsg = stateMsg.mutable_entities(0);
+ EXPECT_EQ(1, entityMsg->components().size());
+ auto compMsg = entityMsg->mutable_components(0);
+ compMsg->set_component(std::to_string(321));
+
+ manager.SetState(stateMsg);
+ changedStateMsg = manager.ChangedState();
+ EXPECT_EQ(1, changedStateMsg.entities_size());
+
+ // check component
+ comp = manager.Component<IntComponent>(e1);
+ ASSERT_NE(nullptr, comp);
+ EXPECT_EQ(321, comp->Data());
+}
+
 // Run multiple times. We want to make sure that static globals don't cause
 // problems.
 INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat,

src/systems/user_commands/UserCommands.cc

@@ -1823,13 +1823,34 @@ bool VisualCommand::Execute()
  return false;
  }
 
- if (visualMsg->id() == kNullEntity)
+ Entity visualEntity = kNullEntity;
+ if (visualMsg->id() != kNullEntity)
+ {
+ visualEntity = visualMsg->id();
+ }
+ else if (!visualMsg->name().empty() && !visualMsg->parent_name().empty())
+ {
+ Entity parentEntity =
+ this->iface->ecm->EntityByComponents(
+ components::Name(visualMsg->parent_name()));
+
+ auto entities =
+ this->iface->ecm->ChildrenByComponents(parentEntity,
+ components::Name(visualMsg->name()));
+
+ // When size > 1, we don't know which entity to modify
+ if (entities.size() == 1)
+ {
+ visualEntity = entities[0];
+ }
+ }
+
+ if (visualEntity == kNullEntity)
  {
  ignerr << "Failed to find visual entity" << std::endl;
  return false;
  }
 
- Entity visualEntity = visualMsg->id();
  auto visualCmdComp =
  this->iface->ecm->Component<components::VisualCmd>(visualEntity);
  if (!visualCmdComp)

test/integration/user_commands.cc

@@ -20,6 +20,7 @@
 #include <ignition/msgs/entity_factory.pb.h>
 #include <ignition/msgs/light.pb.h>
 #include <ignition/msgs/physics.pb.h>
+#include <ignition/msgs/visual.pb.h>
 
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
@@ -29,10 +30,12 @@
 
 #include "ignition/gazebo/components/Light.hh"
 #include "ignition/gazebo/components/Link.hh"
+#include "ignition/gazebo/components/Material.hh"
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/Physics.hh"
 #include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/VisualCmd.hh"
 #include "ignition/gazebo/components/WheelSlipCmd.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/Model.hh"
@@ -1205,4 +1208,91 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WheelSlip))
  // Run two iterations, in the first one the WheelSlipCmd component is created
  // and processed.
  // The second one is just to check everything went fine.
- server.Run(true, 3, false);}
+ server.Run(true, 3, false);
+}
+
+/////////////////////////////////////////////////
+TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Visual))
+{
+ // Start server
+ ServerConfig serverConfig;
+ const auto sdfFile = common::joinPaths(
+ std::string(PROJECT_SOURCE_PATH), "test", "worlds", "shapes.sdf");
+ serverConfig.SetSdfFile(sdfFile);
+
+ Server server(serverConfig);
+ EXPECT_FALSE(server.Running());
+ EXPECT_FALSE(*server.Running(0));
+
+ // Create a system just to get the ECM
+ EntityComponentManager *ecm{nullptr};
+ test::Relay testSystem;
+ testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &,
+ gazebo::EntityComponentManager &_ecm)
+ {
+ ecm = &_ecm;
+ });
+
+ server.AddSystem(testSystem.systemPtr);
+
+ // Run server and check we have the ECM
+ EXPECT_EQ(nullptr, ecm);
+ server.Run(true, 1, false);
+ ASSERT_NE(nullptr, ecm);
+
+ msgs::Visual req;
+ msgs::Boolean res;
+ transport::Node node;
+ bool result;
+ unsigned int timeout = 100;
+ std::string service{"/world/default/visual_config"};
+
+ auto boxVisualEntity =
+ ecm->EntityByComponents(components::Name("box_visual"));
+ ASSERT_NE(kNullEntity, boxVisualEntity);
+
+ // check box visual's initial values
+ auto boxVisualComp = ecm->Component<components::Material>(boxVisualEntity);
+ ASSERT_NE(nullptr, boxVisualComp);
+ EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1.0f),
+ boxVisualComp->Data().Diffuse());
+
+ msgs::Set(req.mutable_material()->mutable_diffuse(),
+ math::Color(0.0f, 1.0f, 0.0f, 1.0f));
+
+ // This will fail to find the entity in VisualCommand::Execute()
+ // since no id was provided
+ EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+ server.Run(true, 1, false);
+ // check that the VisualCmd component was not created
+ auto boxVisCmdComp = ecm->Component<components::VisualCmd>(boxVisualEntity);
+ EXPECT_EQ(nullptr, boxVisCmdComp);
+
+ // add id to msg and resend request
+ req.set_id(boxVisualEntity);
+ EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+ server.Run(true, 1, false);
+ // check the VisualCmd was created and check the values
+ boxVisCmdComp = ecm->Component<components::VisualCmd>(boxVisualEntity);
+ ASSERT_NE(nullptr, boxVisualComp);
+ EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().r());
+ EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().g());
+ EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().b());
+ EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().a());
+
+ // update component using visual name and parent name
+ req.Clear();
+ req.set_name("box_visual");
+ req.set_parent_name("box_link");
+ msgs::Set(req.mutable_material()->mutable_diffuse(),
+ math::Color(0.0f, 0.0f, 1.0f, 1.0f));
+ EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+ server.Run(true, 1, false);
+ // check the values
+ boxVisCmdComp = ecm->Component<components::VisualCmd>(boxVisualEntity);
+ ASSERT_NE(nullptr, boxVisualComp);
+ EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().r());
+ EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().g());
+ EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().b());
+ EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().a());
+}