Together

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

@@ -421,11 +421,19 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent,
   }
   else if (!_name.empty() && !_allowRename)
   {
-    // if there is an entity with the same name and user indicated renaming is
+    // 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));
+    Entity ent = this->EntityByComponents(components::Name(_name),
+        components::ParentEntity(origParentComp->Data()));
+
     bool hasRecreateComp = false;
     Entity recreateEnt = ent;
     while (recreateEnt != kNullEntity && !hasRecreateComp)
@@ -1061,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

@@ -822,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();
 
@@ -842,10 +846,6 @@ void SimulationRunner::Step(const UpdateInfo &_info)
   // Update all the systems.
   this->UpdateSystems();
 
-  // Recreate any entities that have the Recreate component
-  // The entities will have different Entity ids but keep the same name
-  this->ProcessRecreateEntitiesCreate();
-
   if (!this->Paused() &&
        this->requestedRunToSimTime >
        std::chrono::steady_clock::duration::zero() &&
@@ -873,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();
 
@@ -1148,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();
@@ -1182,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);
@@ -1191,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)
@@ -1288,7 +1312,7 @@ void SimulationRunner::ProcessRecreateEntitiesCreate()
   IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesCreate");
 
   // clone the original entities
-  std::set<Entity> clonedEntities;
+  std::set<Entity> entitiesToRemoveRecreateComp;
   for (auto & ent : this->entitiesToRecreate)
   {
     auto nameComp = this->entityCompMgr.Component<components::Name>(ent);
@@ -1297,14 +1321,19 @@ void SimulationRunner::ProcessRecreateEntitiesCreate()
     // set allowRenaming to false so the entities keep their original name
     Entity clonedEntity = this->entityCompMgr.Clone(ent,
        parentComp->Data(), nameComp->Data(), false);
-    clonedEntities.insert(clonedEntity);
-
+    entitiesToRemoveRecreateComp.insert(clonedEntity);
+    entitiesToRemoveRecreateComp.insert(ent);
   }
+
   // remove the Recreate component so they do not get recreated again in the
   // next iteration
-  for (auto &ent : clonedEntities)
+  for (auto &ent : entitiesToRemoveRecreateComp)
   {
-    this->entityCompMgr.RemoveComponent<components::Recreate>(ent);
+    if (!this->entityCompMgr.RemoveComponent<components::Recreate>(ent))
+    {
+      ignerr << "Failed to remove Recreate component from entity["
+        << ent << "]" << std::endl;
+    }
   }
 
   this->entitiesToRecreate.clear();

src/SimulationRunner.hh

@@ -475,6 +475,10 @@ namespace ignition
       /// 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};
 
@@ -639,6 +643,10 @@ namespace ignition
       /// \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/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

src/systems/physics/Physics.cc

@@ -26,6 +26,7 @@
 #include <iostream>
 #include <deque>
 #include <map>
+#include <set>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
@@ -122,6 +123,7 @@
 #include "ignition/gazebo/components/PhysicsEnginePlugin.hh"
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/PoseCmd.hh"
+#include "ignition/gazebo/components/Recreate.hh"
 #include "ignition/gazebo/components/SelfCollide.hh"
 #include "ignition/gazebo/components/SlipComplianceCmd.hh"
 #include "ignition/gazebo/components/Static.hh"
@@ -612,6 +614,13 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// \brief A map between collision entity ids in the ECM to FreeGroup Entities
   /// in ign-physics.
   public: EntityFreeGroupMap entityFreeGroupMap;
+
+  /// \brief Set of links that were added to an existing model. This set
+  /// is used to track links that were added to an existing model, such as
+  /// through the GUI model editor, so that we can avoid premature creation
+  /// of links and collision elements. This also lets us suppress some
+  /// invalid error messages.
+  public: std::set<Entity> linkAddedToModel;
 };
 
 //////////////////////////////////////////////////
@@ -778,6 +787,9 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm)
 //////////////////////////////////////////////////
 void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm)
 {
+  // Clear the set of links that were added to a model.
+  this->linkAddedToModel.clear();
+
   this->CreateWorldEntities(_ecm);
   this->CreateModelEntities(_ecm);
   this->CreateLinkEntities(_ecm);
@@ -879,6 +891,9 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm)
           const components::Pose *_pose,
           const components::ParentEntity *_parent)->bool
       {
+        if (_ecm.EntityHasComponentType(_entity, components::Recreate::typeId))
+          return true;
+
         // Check if model already exists
         if (this->entityModelMap.HasEntity(_entity))
         {
@@ -1011,6 +1026,18 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm)
         const components::Pose *_pose,
         const components::ParentEntity *_parent)->bool
       {
+        // If the parent model is scheduled for recreation, then do not
+        // try to create a new link. This situation can occur when a link
+        // is added to a model from the GUI model editor.
+        if (_ecm.EntityHasComponentType(_parent->Data(),
+              components::Recreate::typeId))
+        {
+          // Add this entity to the set of newly added links to existing
+          // models.
+          this->linkAddedToModel.insert(_entity);
+          return true;
+        }
+
         // Check if link already exists
         if (this->entityLinkMap.HasEntity(_entity))
         {
@@ -1072,6 +1099,14 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm)
           const components::CollisionElement *_collElement,
           const components::ParentEntity *_parent) -> bool
       {
+        // Check to see if this collision's parent is a link that was
+        // not created because the parent model is marked for recreation.
+        if (this->linkAddedToModel.find(_parent->Data()) !=
+            this->linkAddedToModel.end())
+        {
+          return true;
+        }
+
         if (this->entityCollisionMap.HasEntity(_entity))
         {
            ignwarn << "Collision entity [" << _entity
@@ -2182,14 +2217,23 @@ std::map<Entity, physics::FrameData3d> PhysicsPrivate::ChangedLinks(
     _ecm.Each<components::Link>(
       [&](const Entity &_entity, components::Link *) -> bool
       {
-        if (this->staticEntities.find(_entity) != this->staticEntities.end())
+        if (this->staticEntities.find(_entity) != this->staticEntities.end() ||
+            _ecm.EntityHasComponentType(_entity, components::Recreate::typeId))
+        {
           return true;
+        }
 
+        // This `once` variable is here to aid in debugging, make sure to
+        // remove it.
         auto linkPhys = this->entityLinkMap.Get(_entity);
         if (nullptr == linkPhys)
         {
-          ignerr << "Internal error: link [" << _entity
-                 << "] not in entity map" << std::endl;
+          if (this->linkAddedToModel.find(_entity) ==
+              this->linkAddedToModel.end())
+          {
+            ignerr << "Internal error: link [" << _entity
+              << "] not in entity map" << std::endl;
+          }
           return true;
         }
 
@@ -2339,8 +2383,12 @@ bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity,
   auto entityPhys = this->entityLinkMap.Get(_entity);
   if (nullptr == entityPhys)
   {
-    ignerr << "Internal error: entity [" << _entity
-           << "] not in entity map" << std::endl;
+    // Suppress error message if the link has just been added to the model.
+    if (this->linkAddedToModel.find(_entity) == this->linkAddedToModel.end())
+    {
+      ignerr << "Internal error: entity [" << _entity
+        << "] not in entity map" << std::endl;
+    }
     return false;
   }
 
@@ -2437,7 +2485,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
       if (parentModelPoseIt == this->modelWorldPoses.end())
       {
         ignerr << "Internal error: parent model [" << parentEntity
-              << "] does not have a world pose available" << std::endl;
+              << "] does not have a world pose available for child entity["
+              << entity << "]" << std::endl;
         continue;
       }
       const math::Pose3d &parentWorldPose = parentModelPoseIt->second;

test/integration/recreate_entities.cc

@@ -90,6 +90,7 @@ TEST_F(RecreateEntitiesFixture, RecreateEntities)
 
         // recreate entities only once so set it back to false
         recreateEntities = false;
+        return;
       }
     });
   server.AddSystem(testSystem.systemPtr);
@@ -306,4 +307,12 @@ TEST_F(RecreateEntitiesFixture, RecreateEntities)
   EXPECT_NE(originalsphLinkEntity, sphLinkEntity);
   EXPECT_NE(originalcapLinkEntity, capLinkEntity);
   EXPECT_NE(originalellipLinkEntity, ellipLinkEntity);
+
+  // Run again to make sure the recreate components are removed and no
+  // entities to to be recreated
+  server.Run(true, 1, false);
+
+  auto entities = ecm->EntitiesByComponents(components::Model(),
+      components::Recreate());
+  EXPECT_TRUE(entities.empty());
 }