diff --git a/include/ignition/gazebo/Model.hh b/include/ignition/gazebo/Model.hh index 89450cb45c..172c7399ae 100644 --- 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 dataPtr; }; diff --git a/src/Model.cc b/src/Model.cc index d9776b94a3..0b59c1132b 100644 --- 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 index 4ffa3cf594..d3aeb14f91 100644 --- a/test/integration/model.cc +++ b/test/integration/model.cc @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -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)); +} +