Support individual canonical links for nested models (#685)

This creates a new component ModelCanonicalLink that is attached to models and contains a reference to the link entity that serves as the canonical link of the model. This allows us to update the poses of all models including nested ones.

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>

include/ignition/gazebo/SdfEntityCreator.hh

@@ -156,16 +156,13 @@ namespace ignition
       public: void SetParent(Entity _child, Entity _parent);
 
       /// \brief Overloaded function to recursively create model entities
-      /// and make sure a) only one canonical link is created per model tree,
-      /// and b) we override the nested model's static property to true if
+      /// making sure to override the nested model's static property to true if
       /// its parent is static
       /// \param[in] _model SDF model object.
-      /// \param[in] _createCanonicalLink True to create a canonical link
-      /// component and attach to its child link entity
       /// \param[in] _staticParent True if parent is static, false otherwise.
       /// \return Model entity.
       private: Entity CreateEntities(const sdf::Model *_model,
-          bool _createCanonicalLink, bool _staticParent);
+                                     bool _staticParent);
 
       /// \brief Pointer to private data.
       private: std::unique_ptr<SdfEntityCreatorPrivate> dataPtr;

include/ignition/gazebo/components/CanonicalLink.hh

@@ -20,6 +20,7 @@
 #include <ignition/gazebo/components/Factory.hh>
 #include <ignition/gazebo/components/Component.hh>
 #include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Entity.hh>
 
 namespace ignition
 {
@@ -33,6 +34,12 @@ namespace components
   using CanonicalLink = Component<NoData, class CanonicalLinkTag>;
   IGN_GAZEBO_REGISTER_COMPONENT(
       "ign_gazebo_components.CanonicalLink", CanonicalLink)
+
+  /// \brief A component that contains a reference to the canonical link entity
+  /// of a model.
+  using ModelCanonicalLink = Component<Entity, class ModelCanonicalLinkTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT(
+      "ign_gazebo_components.ModelCanonicalLink", ModelCanonicalLink)
 }
 }
 }

src/SdfEntityCreator.cc

@@ -17,6 +17,7 @@
 
 #include <ignition/common/Console.hh>
 #include <ignition/common/Profiler.hh>
+#include <sdf/Types.hh>
 
 #include "ignition/gazebo/Events.hh"
 #include "ignition/gazebo/SdfEntityCreator.hh"
@@ -111,6 +112,7 @@ static math::Pose3d ResolveSdfPose(const sdf::SemanticPose &_semPose)
   return pose;
 }
 
+/////////////////////////////////////////////////
 static std::optional<sdf::JointAxis> ResolveJointAxis(
     const sdf::JointAxis &_unresolvedAxis)
 {
@@ -135,6 +137,44 @@ static std::optional<sdf::JointAxis> ResolveJointAxis(
   return resolvedAxis;
 }
 
+//////////////////////////////////////////////////
+/// \brief Find a descendent child link entity by name.
+/// \param[in] _name The relative name of the link with "::" as the scope
+/// delimiter
+/// \param[in] _model Model entity that defines the scope
+/// \param[in] _ecm Entity component manager
+/// \return The Entity of the descendent link or kNullEntity if link was not
+/// found
+static Entity FindDescendentLinkEntityByName(const std::string &_name,
+                                             const Entity &_model,
+                                             const EntityComponentManager &_ecm)
+{
+  auto ind = _name.find(sdf::kSdfScopeDelimiter);
+  std::vector<Entity> candidates;
+  if (ind != std::string::npos)
+  {
+    candidates = _ecm.ChildrenByComponents(
+        _model, components::Model(), components::Name(_name.substr(0, ind)));
+    if (candidates.size() != 1 || (ind + 2 >= _name.size()))
+    {
+      return kNullEntity;
+    }
+    return FindDescendentLinkEntityByName(_name.substr(ind + 2),
+                                          candidates.front(), _ecm);
+  }
+  else
+  {
+    candidates = _ecm.ChildrenByComponents(_model, components::Link(),
+                                           components::Name(_name));
+
+    if (candidates.size() != 1)
+    {
+      return kNullEntity;
+    }
+    return candidates.front();
+  }
+}
+
 //////////////////////////////////////////////////
 SdfEntityCreator::SdfEntityCreator(EntityComponentManager &_ecm,
           EventManager &_eventManager)
@@ -258,12 +298,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model)
 {
   IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Model)");
 
-  // todo(anyone) Support multiple canonical links in nested models
-  // This version of CreateEntties keeps track whether or not to create a
-  // canonical link in a model tree using the second arg in this recursive
-  // function. We also override child nested models static property if parent
-  // model is static
-  auto ent = this->CreateEntities(_model, true, false);
+  auto ent = this->CreateEntities(_model, false);
 
   // Load all model plugins afterwards, so we get scoped name for nested models.
   for (const auto &[entity, element] : this->dataPtr->newModels)
@@ -291,7 +326,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model)
 
 //////////////////////////////////////////////////
 Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model,
-    bool _createCanonicalLink, bool _staticParent)
+                                        bool _staticParent)
 {
   // Entity
   Entity modelEntity = this->dataPtr->ecm->CreateEntity();
@@ -316,7 +351,6 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model,
   // the parent frame until we get frames working.
 
   // Links
-  bool canonicalLinkCreated = false;
   const auto *canonicalLink = _model->CanonicalLink();
 
   for (uint64_t linkIndex = 0; linkIndex < _model->LinkCount();
@@ -327,11 +361,10 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model,
 
     this->SetParent(linkEntity, modelEntity);
 
-    if (_createCanonicalLink && canonicalLink == link)
+    if (canonicalLink == link)
     {
       this->dataPtr->ecm->CreateComponent(linkEntity,
           components::CanonicalLink());
-      canonicalLinkCreated = true;
     }
 
     // Set wind mode if the link didn't override it
@@ -357,26 +390,34 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model,
       ++modelIndex)
   {
     auto nestedModel = _model->ModelByIndex(modelIndex);
-
-    // Create nested model. Make sure to only create canonical link component
-    // in the nested model if a canonical link has not been created in this
-    // model yet and the canonical link of the top-level model is actually in
-    // the nested model. Also override static propery of the nested model if
-    // this model is static
-
-    const bool canonicalLinkInNestedModel =
-      (canonicalLink == nestedModel->CanonicalLink());
-    const bool createCanonicalLinkInNestedModel =
-      _createCanonicalLink
-      && !canonicalLinkCreated
-      && canonicalLinkInNestedModel;
-
-    auto nestedModelEntity = this->CreateEntities(nestedModel,
-        createCanonicalLinkInNestedModel, isStatic);
+    auto nestedModelEntity = this->CreateEntities(nestedModel, isStatic);
 
     this->SetParent(nestedModelEntity, modelEntity);
   }
 
+  // Find canonical link
+  const auto canonicalLinkPair = _model->CanonicalLinkAndRelativeName();
+  if (canonicalLinkPair.first)
+  {
+    Entity canonicalLinkEntity = FindDescendentLinkEntityByName(
+        canonicalLinkPair.second, modelEntity, *this->dataPtr->ecm);
+    if (kNullEntity != canonicalLinkEntity)
+    {
+      this->dataPtr->ecm->CreateComponent(
+          modelEntity, components::ModelCanonicalLink(canonicalLinkEntity));
+    }
+    else
+    {
+      ignerr << "Could not find the canonical link entity for "
+             << canonicalLinkPair.second << "\n";
+    }
+  }
+  else
+  {
+    ignerr << "Could not resolve the canonical link for " << _model->Name()
+           << "\n";
+  }
+
   // Store the model's SDF DOM to be used when saving the world to file
   this->dataPtr->ecm->CreateComponent(
       modelEntity, components::ModelSdf(*_model));