Support recreating model entities (#1170)

* add recreate component and implement recreate entities functionality, add test Signed-off-by: Ian Chen <[email protected]> * update test and make recreate entities with same name work Signed-off-by: Ian Chen <[email protected]> * revert add include Signed-off-by: Ian Chen <[email protected]> * style Signed-off-by: Ian Chen <[email protected]> * Support editing air pressure sensor in the GUI Signed-off-by: Nate Koenig <[email protected]> * Add noise to qrc Signed-off-by: Nate Koenig <[email protected]> * Add noise to qrc Signed-off-by: Nate Koenig <[email protected]> * Fix lint Signed-off-by: Michael Carroll <[email protected]> * Update sensor icon Signed-off-by: Nate Koenig <[email protected]> * Move AirPressure functions out of ComponentInspector (#1179) Signed-off-by: Louise Poubel <[email protected]> * Fix get decimals, and address comments Signed-off-by: Nate Koenig <[email protected]> * cleanup and simplification Signed-off-by: Nate Koenig <[email protected]> * check recreate comp in ancestor Signed-off-by: Ian Chen <[email protected]> * require sdf 12.1.0 Signed-off-by: Nate Koenig <[email protected]> * Revert sdf version requirement Signed-off-by: Nate Koenig <[email protected]> * Fix codecheck Signed-off-by: Nate Koenig <[email protected]> * revert my bad merge Signed-off-by: Nate Koenig <[email protected]> * remvoe sensor icon Signed-off-by: Nate Koenig <[email protected]> * Together (#1187) * add an add entity button to component inspector. Currently only enabled for models Signed-off-by: Ian Chen <[email protected]> * add model editor gui plugin that inserts visuals to the scene in the render thread Signed-off-by: Ian Chen <[email protected]> * write to ECM Signed-off-by: Ian Chen <[email protected]> * support adding light links Signed-off-by: Ian Chen <[email protected]> * notify other GUI plugins of added/removed entities via GUI events Signed-off-by: Ashton Larkin <[email protected]> * use const ref for constructor input params Signed-off-by: Ashton Larkin <[email protected]> * guarantee 64 bit entity IDs with gazebo::Entity instead of unsigned int Signed-off-by: Ashton Larkin <[email protected]> * testing makr as new entity func Signed-off-by: Ian Chen <[email protected]> * rm printouts Signed-off-by: Ian Chen <[email protected]> * register type Signed-off-by: Ian Chen <[email protected]> * refactor render util Signed-off-by: Ian Chen <[email protected]> * workaround for avoiding crash on exit Signed-off-by: Ian Chen <[email protected]> * refactor, comment out unused menu items Signed-off-by: Ian Chen <[email protected]> * remove commented out code, add CreateLight function Signed-off-by: Ian Chen <[email protected]> * add model editor src files Signed-off-by: Ian Chen <[email protected]> * remove more commented out code Signed-off-by: Ian Chen <[email protected]> * use entity instead of entity name (#1176) Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Add link menu updates (#1177) * use entity instead of entity name Signed-off-by: Nate Koenig <[email protected]> * Update link add menu Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Ian Chen <[email protected]> * fix adding ellipsoid Signed-off-by: Ian Chen <[email protected]> * merge model_editor into component_inspector Signed-off-by: Ian Chen <[email protected]> * fixing warnings Signed-off-by: Ian Chen <[email protected]> * Adjust tool tips Signed-off-by: Nate Koenig <[email protected]> * fix adding light Signed-off-by: Ian Chen <[email protected]> * Fix codecheck Signed-off-by: Nate Koenig <[email protected]> * Fixed documentation Signed-off-by: Nate Koenig <[email protected]> * Working on model creation Signed-off-by: Nate Koenig <[email protected]> * Fix build Signed-off-by: Nate Koenig <[email protected]> * Added debug statements Signed-off-by: Nate Koenig <[email protected]> * use each no cache Signed-off-by: Ian Chen <[email protected]> * fix removing component from view Signed-off-by: Ian Chen <[email protected]> * Fix physics Signed-off-by: Nate Koenig <[email protected]> * Fix codecheck Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Ian Chen <[email protected]> Co-authored-by: Ashton Larkin <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Address comments Signed-off-by: Nate Koenig <[email protected]> * update cameras list on sensor removal Signed-off-by: Ian Chen <[email protected]> * update cameras list on sensor removal Signed-off-by: Ian Chen <[email protected]> * Require sensors 6.1 Signed-off-by: Nate Koenig <[email protected]> * sensors 6.0.1 Signed-off-by: Nate Koenig <[email protected]> * Test model recreation with jointed models Signed-off-by: Michael Carroll <[email protected]> * Fix multiple joints with same names There was an issue in searching for joint parent_link and child_link frames if there was another model with the same frame names. This will correctly search for frame entity ids that are children of the same model. Signed-off-by: Michael Carroll <[email protected]> * Recreate entities joints (#1206) * Test model recreation with jointed models * Fix multiple joints with same names There was an issue in searching for joint parent_link and child_link frames if there was another model with the same frame names. This will correctly search for frame entity ids that are children of the same model. Signed-off-by: Michael Carroll <[email protected]> * Fix the ecm test, which had bad parent-child relationships between links and joints Signed-off-by: Nate Koenig <[email protected]> * Added test for world joints Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Michael Carroll <[email protected]> Co-authored-by: Louise Poubel <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Ashton Larkin <[email protected]>
gazebosim · Nov 17, 2021 · ba9403b · ba9403b
1 parent cebbedc
commit ba9403b
Show file tree

Hide file tree

Showing 17 changed files with 1,560 additions and 50 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -110,7 +110,7 @@ set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-sensors
-ign_find_package(ignition-sensors6 REQUIRED
+ign_find_package(ignition-sensors6 REQUIRED VERSION 6.0.1
   # component order is important
   COMPONENTS
     # non-rendering

diff --git a/include/ignition/gazebo/components/Recreate.hh b/include/ignition/gazebo/components/Recreate.hh
@@ -0,0 +1,48 @@
+/*
+ * 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.
+  /// Currently, only Models will be processed for recreation by the
+  /// SimulationRunner in the ProcessRecreateEntitiesRemove and
+  /// ProcessRecreateEntitiesCreate functions.
+  ///
+  /// The GUI ModelEditor contains example code on how to use this
+  /// component. For example, the ModelEditor allows a user to add a link to an
+  /// existing model. The existing model is tagged with this component so
+  /// that it can be recreated by the server.
+  using Recreate = Component<NoData, class RecreateTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Recreate", Recreate)
+}
+}
+}
+}
+
+#endif
diff --git a/include/ignition/gazebo/detail/View.hh b/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) ||

diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc
@@ -39,6 +39,8 @@
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/ParentLinkName.hh"
+#include "ignition/gazebo/components/Recreate.hh"
+#include "ignition/gazebo/components/World.hh"
 
 using namespace ignition;
 using namespace gazebo;
@@ -420,7 +422,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 "
@@ -497,19 +522,35 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent,
     Entity originalParentLink = kNullEntity;
     Entity originalChildLink = kNullEntity;
 
+    auto origParentComp =
+        this->Component<components::ParentEntity>(_entity);
+
     const auto &parentName =
       this->Component<components::ParentLinkName>(_entity);
-    if (parentName)
+    if (parentName && origParentComp)
     {
-      originalParentLink = this->EntityByComponents<components::Name>(
-          components::Name(parentName->Data()));
+      // CHangel the case where the parent link name is the world.
+      if (common::lowercase(parentName->Data()) == "world")
+      {
+        originalParentLink = this->Component<components::ParentEntity>(
+            origParentComp->Data())->Data();
+      }
+      else
+      {
+      originalParentLink =
+        this->EntityByComponents<components::Name, components::ParentEntity>(
+              components::Name(parentName->Data()),
+              components::ParentEntity(origParentComp->Data()));
+      }
     }
 
     const auto &childName = this->Component<components::ChildLinkName>(_entity);
-    if (childName)
+    if (childName && origParentComp)
     {
-      originalChildLink = this->EntityByComponents<components::Name>(
-          components::Name(childName->Data()));
+      originalChildLink =
+        this->EntityByComponents<components::Name, components::ParentEntity>(
+          components::Name(childName->Data()),
+          components::ParentEntity(origParentComp->Data()));
     }
 
     if (!originalParentLink || !originalChildLink)
@@ -533,7 +574,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 +1086,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;
 }
 
@@ -1937,24 +1993,38 @@ bool EntityComponentManagerPrivate::ClonedJointLinkName(Entity _joint,
     return false;
   }
 
-  auto iter = this->originalToClonedLink.find(_originalLink);
-  if (iter == this->originalToClonedLink.end())
+  Entity clonedLink = kNullEntity;
+
+
+  std::string name;
+  // Handle the case where the link coule have been the world.
+  if (_ecm->Component<components::World>(_originalLink) != nullptr)
   {
-    ignerr << "Error: attempted to clone links, but link ["
-           << _originalLink << "] was never cloned.\n";
-    return false;
+    // Use the special identifier "world".
+    name = "world";
   }
-  auto clonedLink = iter->second;
-
-  auto name = _ecm->Component<components::Name>(clonedLink);
-  if (!name)
+  else
   {
-    ignerr << "Link [" << _originalLink << "] was cloned, but its clone has no "
-           << "name.\n";
-    return false;
+    auto iter = this->originalToClonedLink.find(_originalLink);
+    if (iter == this->originalToClonedLink.end())
+    {
+      ignerr << "Error: attempted to clone links, but link ["
+        << _originalLink << "] was never cloned.\n";
+      return false;
+    }
+    clonedLink = iter->second;
+
+    auto nameComp = _ecm->Component<components::Name>(clonedLink);
+    if (!nameComp)
+    {
+      ignerr << "Link [" << _originalLink
+        << "] was cloned, but its clone has no name.\n";
+      return false;
+    }
+    name = nameComp->Data();
   }
 
-  _ecm->SetComponentData<ComponentTypeT>(_joint, name->Data());
+  _ecm->SetComponentData<ComponentTypeT>(_joint, name);
   return true;
 }
 

diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc
@@ -2795,15 +2795,21 @@ TEST_P(EntityComponentManagerFixture, CloneEntities)
   EXPECT_EQ(kNullEntity, failedClonedEntity);
 
   // create a joint with a parent and child link
+  const std::string parentModelEntityName = "parentModelEntity";
   const std::string parentLinkEntityName = "parentLinkEntity";
   const std::string childLinkEntityName = "childLinkEntity";
+  Entity parentModelEntity = manager.CreateEntity();
+  manager.CreateComponent(parentModelEntity,
+      components::Name(parentModelEntityName));
   Entity parentLinkEntity = manager.CreateEntity();
   manager.CreateComponent(parentLinkEntity,
       components::Name(parentLinkEntityName));
   manager.CreateComponent(parentLinkEntity, components::CanonicalLink());
+  manager.CreateComponent(parentLinkEntity,
+      components::ParentEntity(parentModelEntity));
   Entity jointEntity = manager.CreateEntity();
   manager.CreateComponent(jointEntity,
-      components::ParentEntity(parentLinkEntity));
+      components::ParentEntity(parentModelEntity));
   manager.CreateComponent(jointEntity, components::Name("jointEntity"));
   manager.CreateComponent(jointEntity, components::Joint());
   manager.CreateComponent(jointEntity,
@@ -2812,26 +2818,33 @@ TEST_P(EntityComponentManagerFixture, CloneEntities)
       components::ChildLinkName(childLinkEntityName));
   Entity childLinkEntity = manager.CreateEntity();
   manager.CreateComponent(childLinkEntity,
-      components::ParentEntity(jointEntity));
+      components::ParentEntity(parentModelEntity));
   manager.CreateComponent(childLinkEntity,
       components::Name(childLinkEntityName));
   manager.CreateComponent(childLinkEntity, components::Link());
-  EXPECT_EQ(13u, manager.EntityCount());
+  EXPECT_EQ(14u, manager.EntityCount());
 
   // clone a joint that has a parent and child link.
-  auto clonedParentLinkEntity = manager.Clone(parentLinkEntity, kNullEntity,
+  auto clonedParentModelEntity = manager.Clone(parentModelEntity, kNullEntity,
       "", true);
-  ASSERT_NE(kNullEntity, clonedParentLinkEntity);
-  EXPECT_EQ(16u, manager.EntityCount());
-  clonedEntities.insert(clonedParentLinkEntity);
+  ASSERT_NE(kNullEntity, clonedParentModelEntity);
+  // We just cloned a model with two links and a joint, a total of 4 new
+  // entities.
+  EXPECT_EQ(18u, manager.EntityCount());
+  clonedEntities.insert(clonedParentModelEntity);
   auto clonedJoints = manager.EntitiesByComponents(
-      components::ParentEntity(clonedParentLinkEntity));
+      components::ParentEntity(clonedParentModelEntity), components::Joint());
   ASSERT_EQ(1u, clonedJoints.size());
   clonedEntities.insert(clonedJoints[0]);
   auto clonedChildLinks = manager.EntitiesByComponents(
-      components::ParentEntity(clonedJoints[0]));
+      components::ParentEntity(clonedParentModelEntity), components::Link());
   ASSERT_EQ(1u, clonedChildLinks.size());
   clonedEntities.insert(clonedChildLinks[0]);
+  auto clonedChildCanonicalLinks = manager.EntitiesByComponents(
+      components::ParentEntity(clonedParentModelEntity),
+      components::CanonicalLink());
+  ASSERT_EQ(1u, clonedChildCanonicalLinks.size());
+  clonedEntities.insert(clonedChildCanonicalLinks[0]);
 
   // The cloned joint should have the cloned parent/child link names attached to
   // it, not the original parent/child link names
@@ -2843,17 +2856,19 @@ TEST_P(EntityComponentManagerFixture, CloneEntities)
     manager.Component<components::ChildLinkName>(clonedJoints[0]);
   ASSERT_NE(nullptr, clonedJointChildLinkName);
   EXPECT_NE(clonedJointChildLinkName->Data(), childLinkEntityName);
-  auto clonedParentLinkName =
-    manager.Component<components::Name>(clonedParentLinkEntity);
-  ASSERT_NE(nullptr, clonedParentLinkName);
-  EXPECT_EQ(clonedParentLinkName->Data(), clonedJointParentLinkName->Data());
+  auto clonedParentModelName =
+    manager.Component<components::Name>(clonedParentModelEntity);
+  ASSERT_NE(nullptr, clonedParentModelName);
+  auto clonedJointParentModelName = manager.Component<components::Name>(
+    manager.Component<components::ParentEntity>(clonedJoints[0])->Data());
+  EXPECT_EQ(clonedParentModelName->Data(), clonedJointParentModelName->Data());
   auto clonedChildLinkName =
     manager.Component<components::Name>(clonedChildLinks[0]);
   ASSERT_NE(nullptr, clonedChildLinkName);
   EXPECT_EQ(clonedJointChildLinkName->Data(), clonedChildLinkName->Data());
 
   // make sure that the name given to each cloned entity is unique
-  EXPECT_EQ(8u, clonedEntities.size());
+  EXPECT_EQ(9u, clonedEntities.size());
   for (const auto &entity : clonedEntities)
   {
     auto nameComp = manager.Component<components::Name>(entity);
@@ -2864,7 +2879,7 @@ TEST_P(EntityComponentManagerFixture, CloneEntities)
   // try to clone an entity that does not exist
   EXPECT_EQ(kNullEntity, manager.Clone(kNullEntity, topLevelEntity, "",
         allowRename));
-  EXPECT_EQ(16u, manager.EntityCount());
+  EXPECT_EQ(18u, manager.EntityCount());
 }
 
 /////////////////////////////////////////////////