Add support for cloning entities (#959)

Signed-off-by: Ashton Larkin <[email protected]>
Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>

Author: adlarkin

include/ignition/gazebo/EntityComponentManager.hh

@@ -28,6 +28,7 @@
 #include <typeinfo>
 #include <type_traits>
 #include <unordered_set>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -74,6 +75,38 @@ namespace ignition
       /// \return An id for the Entity, or kNullEntity on failure.
       public: Entity CreateEntity();
 
+      /// \brief Clone an entity and its components. If the entity has any child
+      /// entities, they will also be cloned.
+      /// When cloning entities, the following rules apply:
+      ///   1. The name component of a cloned entity will consist of a unique
+      ///      name, since all entities should have a unique name.
+      ///   2. Cloned entities that have a canonical link will have their
+      ///      canonical link set to the cloned canonical link, not the original
+      ///      canonical link.
+      ///   3. Child entities that are cloned will have their parent set to the
+      ///      cloned parent entity.
+      ///   4. Aside from the changes listed above, all other cloned components
+      ///      remain unchanged.
+      /// \param[in] _entity The entity to clone.
+      /// \param[in] _parent The parent of the cloned entity. Set this to
+      /// kNullEntity if the cloned entity should not have a parent.
+      /// \param[in] _name The name that should be given to the cloned entity.
+      /// Set this to an empty string if the cloned entity name should be
+      /// auto-generated to something unique.
+      /// \param[in] _allowRename True if _name can be modified to be a unique
+      /// name if it isn't already a unique name. False if _name cannot be
+      /// modified to be a unique name. If _allowRename is set to False, and
+      /// _name is not unique, _entity will not be cloned. If _name is an
+      /// empty string, _allowRename is ignored since the cloned entity will
+      /// have an auto-generated unique name.
+      /// \return The cloned entity, which will have a unique name. kNullEntity
+      /// is returned if cloning failed. Failure could occur if _entity does not
+      /// exist, or if a unique name could not be generated for the entity to be
+      /// cloned.
+      /// \sa Clone
+      public: Entity Clone(Entity _entity, Entity _parent,
+                  const std::string &_name, bool _allowRename);
+
       /// \brief Get the number of entities on the server.
       /// \return Entity count.
       public: size_t EntityCount() const;
@@ -333,6 +366,21 @@ namespace ignition
       private: template <typename T>
                struct identity;  // NOLINT
 
+      /// \brief Helper function for cloning an entity and its children (this
+      /// includes cloning components attached to these entities). This method
+      /// should never be called directly - it is called internally from the
+      /// public Clone method.
+      /// \param[in] _entity The entity to clone.
+      /// \param[in] _parent The parent of the cloned entity.
+      /// \param[in] _name The name that should be given to the cloned entity.
+      /// \param[in] _allowRename True if _name can be modified to be a unique
+      /// name if it isn't already a unique name. False if _name cannot be
+      /// modified to be a unique name.
+      /// \return The cloned entity. kNullEntity is returned if cloning failed.
+      /// \sa Clone
+      private: Entity CloneImpl(Entity _entity, Entity _parent,
+                  const std::string &_name, bool _allowRename);
+
       /// \brief A version of Each() that doesn't use a cache. The cached
       /// version, Each(), is preferred.
       /// Get all entities which contain given component types, as well

include/ignition/gazebo/components/Component.hh

@@ -279,6 +279,10 @@ namespace components
     /// Factory registration and is guaranteed to be the same across compilers
     /// and runs.
     public: virtual ComponentTypeId TypeId() const = 0;
+
+    /// \brief Clone the component.
+    /// \return A pointer to the component.
+    public: virtual std::unique_ptr<BaseComponent> Clone() = 0;
   };
 
   /// \brief A component type that wraps any data type. The intention is for
@@ -342,6 +346,9 @@ namespace components
     /// \return True if different.
     public: bool operator!=(const Component &_component) const;
 
+    // Documentation inherited
+    public: std::unique_ptr<BaseComponent> Clone() override;
+
     // Documentation inherited
     public: ComponentTypeId TypeId() const override;
 
@@ -408,6 +415,9 @@ namespace components
     public: bool operator!=(const Component<NoData, Identifier,
                             Serializer> &_component) const;
 
+    // Documentation inherited
+    public: std::unique_ptr<BaseComponent> Clone() override;
+
     // Documentation inherited
     public: ComponentTypeId TypeId() const override;
 
@@ -490,6 +500,16 @@ namespace components
     Serializer::Deserialize(_in, this->Data());
   }
 
+  //////////////////////////////////////////////////
+  template <typename DataType, typename Identifier, typename Serializer>
+  std::unique_ptr<BaseComponent>
+  Component<DataType, Identifier, Serializer>::Clone()
+  {
+    Component<DataType, Identifier, Serializer> clonedComp(this->Data());
+    return std::make_unique<Component<DataType, Identifier, Serializer>>(
+        clonedComp);
+  }
+
   //////////////////////////////////////////////////
   template <typename DataType, typename Identifier, typename Serializer>
   ComponentTypeId Component<DataType, Identifier, Serializer>::TypeId() const
@@ -513,6 +533,14 @@ namespace components
     return false;
   }
 
+  //////////////////////////////////////////////////
+  template <typename Identifier, typename Serializer>
+  std::unique_ptr<BaseComponent>
+  Component<NoData, Identifier, Serializer>::Clone()
+  {
+    return std::make_unique<Component<NoData, Identifier, Serializer>>();
+  }
+
   //////////////////////////////////////////////////
   template <typename Identifier, typename Serializer>
   ComponentTypeId Component<NoData, Identifier, Serializer>::TypeId() const

include/ignition/gazebo/rendering/SceneManager.hh

@@ -177,26 +177,30 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Create an actor
     /// \param[in] _id Unique actor id
     /// \param[in] _actor Actor sdf dom
+    /// \param[in] _name Actor's name
     /// \param[in] _parentId Parent id
     /// \return Actor object created from the sdf dom
     public: rendering::VisualPtr CreateActor(Entity _id,
-        const sdf::Actor &_actor, Entity _parentId = 0);
+        const sdf::Actor &_actor, const std::string &_name,
+        Entity _parentId = 0);
 
     /// \brief Create a light
     /// \param[in] _id Unique light id
     /// \param[in] _light Light sdf dom
+    /// \param[in] _name Light's name
     /// \param[in] _parentId Parent id
     /// \return Light object created from the sdf dom
     public: rendering::LightPtr CreateLight(Entity _id,
-        const sdf::Light &_light, Entity _parentId);
+        const sdf::Light &_light, const std::string &_name, Entity _parentId);
 
     /// \brief Create a light
     /// \param[in] _id Unique light id
     /// \param[in] _light Light sdf dom
+    /// \param[in] _name Light's name
     /// \param[in] _parentId Parent id
     /// \return Light object created from the sdf dom
     public: rendering::VisualPtr CreateLightVisual(Entity _id,
-        const sdf::Light &_light, Entity _parentId);
+        const sdf::Light &_light, const std::string &_name, Entity _parentId);
 
     /// \brief Create a particle emitter.
     /// \param[in] _id Unique particle emitter id

src/Component_TEST.cc

@@ -177,6 +177,11 @@ class NoSerialize : public components::BaseComponent
   {
     return 0;
   }
+
+  public: std::unique_ptr<BaseComponent> Clone() override
+  {
+    return nullptr;
+  }
 };
 
 //////////////////////////////////////////////////
@@ -550,3 +555,40 @@ TEST_F(ComponentTest, TypeName)
     EXPECT_EQ("123456", comp.typeName);
   }
 }
+
+//////////////////////////////////////////////////
+TEST_F(ComponentTest, Clone)
+{
+  // Component with data
+  {
+    using Custom = components::Component<int, class CustomTag>;
+
+    // create a component and a clone of it. The clone should initially have the
+    // same data as the original component
+    Custom comp(5);
+    auto clonedComp = comp.Clone();
+    auto derivedClone = static_cast<Custom *>(clonedComp.get());
+    EXPECT_EQ(comp, *derivedClone);
+
+    // modify the data of the cloned component, and make sure that only the
+    // cloned component is modified, not the original component
+    derivedClone->Data() = 10;
+    EXPECT_NE(comp, *derivedClone);
+    EXPECT_EQ(5, comp.Data());
+    EXPECT_EQ(10, derivedClone->Data());
+  }
+
+  // Component without data
+  {
+    using Custom = components::Component<components::NoData, class CustomTag>;
+
+    Custom comp;
+    auto clonedComp = comp.Clone();
+    auto derivedClone = static_cast<Custom *>(clonedComp.get());
+
+    // since this component has no data, we cannot do the same check as we did
+    // above for a component with data. However, we can make sure that the
+    // pointers for the components are different
+    EXPECT_NE(&comp, derivedClone);
+  }
+}