Add Model::CanonicalLink getter (#1594)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
gazebosim · Jul 15, 2022 · 33a0fce · 33a0fce
1 parent df45262
commit 33a0fce
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 0 deletions.
diff --git a/include/ignition/gazebo/Model.hh b/include/ignition/gazebo/Model.hh
@@ -169,6 +169,12 @@ namespace ignition
       public: void SetWorldPoseCmd(EntityComponentManager &_ecm,
           const math::Pose3d &_pose);
 
+      /// \brief Get the model's canonical link entity.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Link entity.
+      public: gazebo::Entity CanonicalLink(
+          const EntityComponentManager &_ecm) const;
+
       /// \brief Pointer to private data.
       private: std::unique_ptr<ModelPrivate> dataPtr;
     };

diff --git a/src/Model.cc b/src/Model.cc
@@ -15,6 +15,7 @@
  *
 */
 
+#include "ignition/gazebo/components/CanonicalLink.hh"
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Model.hh"
@@ -193,3 +194,10 @@ void Model::SetWorldPoseCmd(EntityComponentManager &_ecm,
   }
 }
 
+//////////////////////////////////////////////////
+Entity Model::CanonicalLink(const EntityComponentManager &_ecm) const
+{
+  return _ecm.EntityByComponents(
+      components::ParentEntity(this->dataPtr->id),
+      components::CanonicalLink());
+}
diff --git a/test/integration/model.cc b/test/integration/model.cc
@@ -23,6 +23,7 @@
 
 #include <ignition/gazebo/EntityComponentManager.hh>
 #include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/components/CanonicalLink.hh>
 #include <ignition/gazebo/components/Joint.hh>
 #include <ignition/gazebo/components/Link.hh>
 #include <ignition/gazebo/components/Model.hh>
@@ -232,3 +233,32 @@ TEST_F(ModelIntegrationTest, SetWorldPoseCmd)
   EXPECT_TRUE(ecm.HasOneTimeComponentChanges());
 }
 
+//////////////////////////////////////////////////
+TEST_F(ModelIntegrationTest, CanonicalLink)
+{
+  EntityComponentManager ecm;
+
+  // Model
+  auto eModel = ecm.CreateEntity();
+  Model model(eModel);
+  EXPECT_EQ(eModel, model.Entity());
+  EXPECT_EQ(0u, model.LinkCount(ecm));
+
+  // Links
+  auto canonicalLink = ecm.CreateEntity();
+  ecm.CreateComponent(canonicalLink, components::Link());
+  ecm.CreateComponent(canonicalLink, components::CanonicalLink());
+  ecm.CreateComponent(canonicalLink,
+      components::ParentEntity(eModel));
+  ecm.CreateComponent(canonicalLink, components::Name("canonical"));
+
+  auto anotherLink = ecm.CreateEntity();
+  ecm.CreateComponent(anotherLink, components::Link());
+  ecm.CreateComponent(anotherLink, components::ParentEntity(eModel));
+  ecm.CreateComponent(anotherLink, components::Name("another"));
+
+  // Check model
+  EXPECT_EQ(canonicalLink, model.CanonicalLink(ecm));
+  EXPECT_EQ(2u, model.LinkCount(ecm));
+}
+