update canonical link poses first

Signed-off-by: Ashton Larkin <[email protected]>
gazebosim · Jun 30, 2021 · 862f0b7 · 862f0b7
1 parent dc5e39b
commit 862f0b7
Show file tree

Hide file tree

Showing 4 changed files with 114 additions and 255 deletions.
diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh
@@ -18,7 +18,6 @@
 #define IGNITION_GAZEBO_UTIL_HH_
 
 #include <string>
-#include <unordered_set>
 #include <vector>
 
 #include <ignition/math/Pose3.hh>
@@ -141,14 +140,6 @@ namespace ignition
         const Entity &_entity,
         const EntityComponentManager &_ecm);
 
-    /// \brief Get all of the links that belong to a model. If the model has
-    /// nested models, the links for the nested models will also be retrieved.
-    /// \param[in] _model The model to retrieve links from.
-    /// \param[in] _ecm The entity component manager.
-    /// \return All of the links that belong to _model.
-    std::unordered_set<Entity> allModelLinks(const Entity &_model,
-        const EntityComponentManager &_ecm);
-
     /// \brief Helper function to generate a valid transport topic, given
     /// a list of topics ordered by preference. The generated topic will be,
     /// in this order:

diff --git a/src/Util.cc b/src/Util.cc
@@ -15,8 +15,6 @@
  *
 */
 
-#include <unordered_set>
-
 #ifndef __APPLE__
   #if (defined(_MSVC_LANG))
     #if (_MSVC_LANG >= 201703L || __cplusplus >= 201703L)
@@ -446,36 +444,6 @@ ignition::gazebo::Entity topLevelModel(const Entity &_entity,
   return modelEntity;
 }
 
-//////////////////////////////////////////////////
-/// \brief Helper function for AllModelLinks. This helper uses recursion
-/// to account for nested model links.
-/// \param[in] _model The model to retrieve links from.
-/// \param[in] _ecm The entity component manager.
-/// \param[in, out] _links The links that belong to _model.
-static void AllModelLinksHelper(const Entity &_model,
-    const EntityComponentManager &_ecm,
-    std::unordered_set<Entity> &_links)
-{
-  auto childLinks = _ecm.ChildrenByComponents<components::Link>(_model,
-      components::Link());
-  for (const auto &link : childLinks)
-    _links.insert(link);
-
-  auto nestedModels = _ecm.ChildrenByComponents<components::Model>(_model,
-      components::Model());
-  for (const auto &nestedModel : nestedModels)
-    AllModelLinksHelper(nestedModel, _ecm, _links);
-}
-
-//////////////////////////////////////////////////
-std::unordered_set<Entity> allModelLinks(const Entity &_model,
-    const EntityComponentManager &_ecm)
-{
-  std::unordered_set<Entity> links;
-  AllModelLinksHelper(_model, _ecm, links);
-  return links;
-}
-
 //////////////////////////////////////////////////
 std::string topicFromScopedName(const Entity &_entity,
     const EntityComponentManager &_ecm, bool _excludeWorld)

diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc
@@ -496,101 +496,6 @@ TEST_F(UtilTest, TopLevelModel)
   EXPECT_EQ(kNullEntity, topLevelModel(worldEntity, ecm));
 }
 
-/////////////////////////////////////////////////
-TEST_F(UtilTest, AllModelLinks)
-{
-  EntityComponentManager ecm;
-
-  // - modelA
-  //    - linkAA
-  //    - modelB
-  //      - linkBA
-  //      - modelC
-  //          - linkCA
-  //          - linkCB
-  //    - linkAB
-  // - modelD
-  //    - linkDA
-  //    - linkDB
-
-  // model A
-  auto modelAEntity = ecm.CreateEntity();
-  ecm.CreateComponent(modelAEntity, components::Model());
-
-  // link AA - child of model A
-  auto linkAAEntity = ecm.CreateEntity();
-  ecm.CreateComponent(linkAAEntity, components::Link());
-  ecm.SetParentEntity(linkAAEntity, modelAEntity);
-
-  // model B - nested inside model A
-  auto modelBEntity = ecm.CreateEntity();
-  ecm.CreateComponent(modelBEntity, components::Model());
-  ecm.SetParentEntity(modelBEntity, modelAEntity);
-
-  // link BA - child of model B
-  auto linkBAEntity = ecm.CreateEntity();
-  ecm.CreateComponent(linkBAEntity, components::Link());
-  ecm.SetParentEntity(linkBAEntity, modelBEntity);
-
-  // model C - nested inside model B
-  auto modelCEntity = ecm.CreateEntity();
-  ecm.CreateComponent(modelCEntity, components::Model());
-  ecm.SetParentEntity(modelCEntity, modelBEntity);
-
-  // link CA - child of model C
-  auto linkCAEntity = ecm.CreateEntity();
-  ecm.CreateComponent(linkCAEntity, components::Link());
-  ecm.SetParentEntity(linkCAEntity, modelCEntity);
-
-  // link CB - child of model C
-  auto linkCBEntity = ecm.CreateEntity();
-  ecm.CreateComponent(linkCBEntity, components::Link());
-  ecm.SetParentEntity(linkCBEntity, modelCEntity);
-
-  // link AB - child of model A
-  auto linkABEntity = ecm.CreateEntity();
-  ecm.CreateComponent(linkABEntity, components::Link());
-  ecm.SetParentEntity(linkABEntity, modelAEntity);
-
-  // model D
-  auto modelDEntity = ecm.CreateEntity();
-  ecm.CreateComponent(modelDEntity, components::Model());
-
-  // link DA - child of model D
-  auto linkDAEntity = ecm.CreateEntity();
-  ecm.CreateComponent(linkDAEntity, components::Link());
-  ecm.SetParentEntity(linkDAEntity, modelDEntity);
-
-  // link DB - child of model D
-  auto linkDBEntity = ecm.CreateEntity();
-  ecm.CreateComponent(linkDBEntity, components::Link());
-  ecm.SetParentEntity(linkDBEntity, modelDEntity);
-
-  auto modelALinks = allModelLinks(modelAEntity, ecm);
-  EXPECT_EQ(5u, modelALinks.size());
-  EXPECT_TRUE(modelALinks.find(linkAAEntity) != modelALinks.end());
-  EXPECT_TRUE(modelALinks.find(linkBAEntity) != modelALinks.end());
-  EXPECT_TRUE(modelALinks.find(linkCAEntity) != modelALinks.end());
-  EXPECT_TRUE(modelALinks.find(linkCBEntity) != modelALinks.end());
-  EXPECT_TRUE(modelALinks.find(linkABEntity) != modelALinks.end());
-
-  auto modelBLinks = allModelLinks(modelBEntity, ecm);
-  EXPECT_EQ(3u, modelBLinks.size());
-  EXPECT_TRUE(modelBLinks.find(linkBAEntity) != modelBLinks.end());
-  EXPECT_TRUE(modelBLinks.find(linkCAEntity) != modelBLinks.end());
-  EXPECT_TRUE(modelBLinks.find(linkCBEntity) != modelBLinks.end());
-
-  auto modelCLinks = allModelLinks(modelCEntity, ecm);
-  EXPECT_EQ(2u, modelCLinks.size());
-  EXPECT_TRUE(modelCLinks.find(linkCAEntity) != modelCLinks.end());
-  EXPECT_TRUE(modelCLinks.find(linkCBEntity) != modelCLinks.end());
-
-  auto modelDLinks = allModelLinks(modelDEntity, ecm);
-  EXPECT_EQ(2u, modelDLinks.size());
-  EXPECT_TRUE(modelDLinks.find(linkDAEntity) != modelDLinks.end());
-  EXPECT_TRUE(modelDLinks.find(linkDBEntity) != modelDLinks.end());
-}
-
 /////////////////////////////////////////////////
 TEST_F(UtilTest, ValidTopic)
 {