Support recreating model entities

include/ignition/gazebo/components/Recreate.hh

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_RECREATE_HH_
+#define IGNITION_GAZEBO_COMPONENTS_RECREATE_HH_
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief A component that identifies an entity needs to be recreated
+  using Recreate = Component<NoData, class RecreateTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Recreate", Recreate)
+}
+}
+}
+}
+
+#endif

include/ignition/gazebo/detail/View.hh

@@ -280,6 +280,10 @@ template<typename ...ComponentTypeTs>
 bool View<ComponentTypeTs...>::NotifyComponentRemoval(const Entity _entity,
     const ComponentTypeId _typeId)
 {
+  // if entity is still marked as to add, remove from the view
+  if (this->RequiresComponent(_typeId))
+    this->toAddEntities.erase(_entity);
+
   // make sure that _typeId is a type required by the view and that _entity is
   // already a part of the view
   if (!this->RequiresComponent(_typeId) ||

src/EntityComponentManager.cc

@@ -39,6 +39,7 @@
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/ParentLinkName.hh"
+#include "ignition/gazebo/components/Recreate.hh"
 
 using namespace ignition;
 using namespace gazebo;
@@ -420,7 +421,30 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent,
   }
   else if (!_name.empty() && !_allowRename)
   {
-    if (kNullEntity != this->EntityByComponents(components::Name(_name)))
+    // Get the entity's original parent. This is used to make sure we get
+    // the correct entity. For example, two different models may have a
+    // child with the name "link".
+    auto origParentComp =
+        this->Component<components::ParentEntity>(_entity);
+
+    // If there is an entity with the same name and user indicated renaming is
+    // not allowed then return null entity.
+    // If the entity or one of its ancestor has a Recreate component then carry
+    // on since the ECM is supposed to create a new entity with the same name.
+    Entity ent = this->EntityByComponents(components::Name(_name),
+        components::ParentEntity(origParentComp->Data()));
+
+    bool hasRecreateComp = false;
+    Entity recreateEnt = ent;
+    while (recreateEnt != kNullEntity && !hasRecreateComp)
+    {
+      hasRecreateComp = this->Component<components::Recreate>(recreateEnt) !=
+        nullptr;
+      auto parentComp = this->Component<components::ParentEntity>(recreateEnt);
+      recreateEnt = parentComp ? parentComp->Data() : kNullEntity;
+    }
+
+    if (kNullEntity != ent && !hasRecreateComp)
     {
       ignerr << "Requested to clone entity [" << _entity
         << "] with a name of [" << _name << "], but another entity already "
@@ -533,7 +557,14 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent,
   for (const auto &childEntity :
       this->EntitiesByComponents(components::ParentEntity(_entity)))
   {
-    auto clonedChild = this->CloneImpl(childEntity, clonedEntity, "", true);
+    std::string name;
+    if (!_allowRename)
+    {
+      auto nameComp = this->Component<components::Name>(childEntity);
+      name = nameComp->Data();
+    }
+    auto clonedChild = this->CloneImpl(childEntity, clonedEntity, name,
+        _allowRename);
     if (kNullEntity == clonedChild)
     {
       ignerr << "Cloning child entity [" << childEntity << "] failed.\n";
@@ -1038,6 +1069,14 @@ bool EntityComponentManager::CreateComponentImplementation(
 
   this->dataPtr->createdCompTypes.insert(_componentTypeId);
 
+  // If the component is a components::ParentEntity, then make sure to
+  // update the entities graph.
+  if (_componentTypeId == components::ParentEntity::typeId)
+  {
+    auto parentComp = this->Component<components::ParentEntity>(_entity);
+    this->SetParentEntity(_entity, parentComp->Data());
+  }
+
   return updateData;
 }
 

src/SimulationRunner.cc

@@ -27,8 +27,10 @@
 #include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/World.hh"
+#include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Physics.hh"
 #include "ignition/gazebo/components/PhysicsCmd.hh"
+#include "ignition/gazebo/components/Recreate.hh"
 #include "ignition/gazebo/Events.hh"
 #include "ignition/gazebo/SdfEntityCreator.hh"
 #include "ignition/gazebo/Util.hh"
@@ -820,6 +822,10 @@ void SimulationRunner::Step(const UpdateInfo &_info)
   IGN_PROFILE("SimulationRunner::Step");
   this->currentInfo = _info;
 
+  // Process new ECM state information, typically sent from the GUI after
+  // a change was made to the GUI's ECM.
+  this->ProcessNewWorldControlState();
+
   // Publish info
   this->PublishStats();
 
@@ -831,6 +837,12 @@ void SimulationRunner::Step(const UpdateInfo &_info)
   // Handle pending systems
   this->ProcessSystemQueue();
 
+  // Handle entities that need to be recreated.
+  // Put in a request to mark them as removed so that in the UpdateSystem call
+  // the systems can remove them first before new ones are created. This is
+  // so that we can recreate entities with the same name.
+  this->ProcessRecreateEntitiesRemove();
+
   // Update all the systems.
   this->UpdateSystems();
 
@@ -861,6 +873,12 @@ void SimulationRunner::Step(const UpdateInfo &_info)
   // Clear all new entities
   this->entityCompMgr.ClearNewlyCreatedEntities();
 
+  // Recreate any entities that have the Recreate component
+  // The entities will have different Entity ids but keep the same name
+  // Make sure this happens after ClearNewlyCreatedEntities, otherwise the
+  // cloned entities will loose their "New" state.
+  this->ProcessRecreateEntitiesCreate();
+
   // Process entity removals.
   this->entityCompMgr.ProcessRemoveEntityRequests();
 
@@ -1136,11 +1154,13 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req,
 {
   std::lock_guard<std::mutex> lock(this->msgBufferMutex);
 
-  // update the server ECM if the request contains SerializedState information
+  // Copy the state information if it exists
   if (_req.has_state())
-    this->entityCompMgr.SetState(_req.state());
-  // TODO(anyone) notify server systems of changes made to the ECM, if there
-  // were any?
+  {
+    if (this->newWorldControlState == nullptr)
+      this->newWorldControlState = _req.New();
+    this->newWorldControlState->CopyFrom(_req);
+  }
 
   WorldControl control;
   control.pause = _req.world_control().pause();
@@ -1170,7 +1190,7 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req,
   {
     control.runToSimTime = std::chrono::seconds(
         _req.world_control().run_to_sim_time().sec()) +
-        std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec());
+      std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec());
   }
 
   this->worldControls.push_back(control);
@@ -1179,6 +1199,22 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req,
   return true;
 }
 
+/////////////////////////////////////////////////
+void SimulationRunner::ProcessNewWorldControlState()
+{
+  std::lock_guard<std::mutex> lock(this->msgBufferMutex);
+  // update the server ECM if the request contains SerializedState information
+  if (this->newWorldControlState && this->newWorldControlState->has_state())
+  {
+    this->entityCompMgr.SetState(this->newWorldControlState->state());
+
+    delete this->newWorldControlState;
+    this->newWorldControlState = nullptr;
+  }
+  // TODO(anyone) notify server systems of changes made to the ECM, if there
+  // were any?
+}
+
 /////////////////////////////////////////////////
 bool SimulationRunner::OnPlaybackControl(const msgs::LogPlaybackControl &_req,
     msgs::Boolean &_res)
@@ -1252,6 +1288,57 @@ void SimulationRunner::ProcessWorldControl()
   this->worldControls.clear();
 }
 
+/////////////////////////////////////////////////
+void SimulationRunner::ProcessRecreateEntitiesRemove()
+{
+  IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesRemove");
+
+  // store the original entities to recreate and put in request to remove them
+  this->entityCompMgr.Each<components::Model,
+                           components::Recreate>(
+      [&](const Entity &_entity,
+          const components::Model *,
+          const components::Recreate *)->bool
+      {
+        this->entitiesToRecreate.insert(_entity);
+        this->entityCompMgr.RequestRemoveEntity(_entity, true);
+        return true;
+      });
+}
+
+/////////////////////////////////////////////////
+void SimulationRunner::ProcessRecreateEntitiesCreate()
+{
+  IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesCreate");
+
+  // clone the original entities
+  std::set<Entity> entitiesToRemoveRecreateComp;
+  for (auto & ent : this->entitiesToRecreate)
+  {
+    auto nameComp = this->entityCompMgr.Component<components::Name>(ent);
+    auto parentComp =
+        this->entityCompMgr.Component<components::ParentEntity>(ent);
+    // set allowRenaming to false so the entities keep their original name
+    Entity clonedEntity = this->entityCompMgr.Clone(ent,
+       parentComp->Data(), nameComp->Data(), false);
+    entitiesToRemoveRecreateComp.insert(clonedEntity);
+    entitiesToRemoveRecreateComp.insert(ent);
+  }
+
+  // remove the Recreate component so they do not get recreated again in the
+  // next iteration
+  for (auto &ent : entitiesToRemoveRecreateComp)
+  {
+    if (!this->entityCompMgr.RemoveComponent<components::Recreate>(ent))
+    {
+      ignerr << "Failed to remove Recreate component from entity["
+        << ent << "]" << std::endl;
+    }
+  }
+
+  this->entitiesToRecreate.clear();
+}
+
 /////////////////////////////////////////////////
 bool SimulationRunner::Paused() const
 {

src/SimulationRunner.hh

@@ -27,6 +27,7 @@
 #include <list>
 #include <memory>
 #include <optional>
+#include <set>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
@@ -466,6 +467,18 @@ namespace ignition
         std::optional<Entity> _entity = std::nullopt,
         std::optional<std::shared_ptr<const sdf::Element>> _sdf = std::nullopt);
 
+      /// \brief Process entities with the components::Recreate component.
+      /// Put in a request to make them as removed
+      private: void ProcessRecreateEntitiesRemove();
+
+      /// \brief Process entities with the components::Recreate component.
+      /// Reccreate the entities by cloning from the original ones.
+      private: void ProcessRecreateEntitiesCreate();
+
+      /// \brief Process the new world state message, if it is present.
+      /// See the newWorldControlState variable below.
+      private: void ProcessNewWorldControlState();
+
       /// \brief This is used to indicate that a stop event has been received.
       private: std::atomic<bool> stopReceived{false};
 
@@ -627,6 +640,13 @@ namespace ignition
       /// WorldControl info (true) or not (false)
       private: bool stepping{false};
 
+      /// \brief A set of entities that need to be recreated
+      private: std::set<Entity> entitiesToRecreate;
+
+      /// \brief Holds new world state information so that it can processed
+      /// at the appropriate time.
+      private: msgs::WorldControlState *newWorldControlState{nullptr};
+
       friend class LevelManager;
     };
     }

src/SimulationRunner_TEST.cc

@@ -56,6 +56,7 @@
 #include "ignition/gazebo/Events.hh"
 #include "ignition/gazebo/Util.hh"
 #include "ignition/gazebo/config.hh"
+
 #include "../test/helpers/EnvTestFixture.hh"
 #include "SimulationRunner.hh"
 

src/gui/plugins/component_inspector/ComponentInspector.cc

@@ -18,6 +18,7 @@
 #include <iostream>
 #include <list>
 #include <regex>
+
 #include <ignition/common/Console.hh>
 #include <ignition/common/MeshManager.hh>
 #include <ignition/common/Profiler.hh>

src/gui/plugins/component_inspector/ComponentInspector.hh

@@ -61,7 +61,6 @@ namespace gazebo
               << _item->text().toStdString() << "]" << std::endl;
     }
   }
-
   /// \brief Specialized to set string data.
   /// \param[in] _item Item whose data will be set.
   /// \param[in] _data Data to set.

src/gui/plugins/component_inspector/ModelEditor.cc

@@ -32,6 +32,7 @@
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
+#include "ignition/gazebo/components/Recreate.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/SdfEntityCreator.hh"
 
@@ -181,6 +182,9 @@ void ModelEditor::Update(const UpdateInfo &,
       linkSdf.SetName(linkName);
       auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf);
       this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity);
+      // Make sure to mark the parent as needing recreation. This will
+      // tell the server to rebuild the model with the new link.
+      _ecm.CreateComponent(eta.parentEntity, components::Recreate());
 
       // traverse the tree and add all new entities created by the entity
       // creator to the set