From 24041d8b10b3ea20ea71e36dabeff2fb841f0f65 Mon Sep 17 00:00:00 2001
From: Jorge Perez <j.j.perez13@hotmail.com>
Date: Thu, 10 Dec 2020 12:09:03 -0300
Subject: [PATCH] [bullet] Add find or construct link (#175)

* Add Find or Construct Link function
* Change function to return entity instead of identity
* Remove loop creating links inside model

Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com>
Signed-off-by: Tomas Lorente <jtlorente@ekumenlabs.com>
---
 bullet/src/SDFFeatures.cc | 27 +++++++++++++++++++++------
 bullet/src/SDFFeatures.hh |  3 +++
 2 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc
index 74c752c0d..6470a0c44 100644
--- a/bullet/src/SDFFeatures.cc
+++ b/bullet/src/SDFFeatures.cc
@@ -39,12 +39,6 @@ Identity SDFFeatures::ConstructSdfModel(
   // const auto &world = this->worlds.at(_worldID)->world;
   const auto modelIdentity = this->AddModel({name, _worldID, isStatic, pose});
 
-  // Build links
-  for (std::size_t i = 0; i < _sdfModel.LinkCount(); ++i)
-  {
-    this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i));
-  }
-
   return modelIdentity;
 }
 
@@ -172,6 +166,27 @@ Identity SDFFeatures::ConstructSdfCollision(
   return this->GenerateInvalidId();
 }
 
+/////////////////////////////////////////////////
+std::size_t SDFFeatures::FindOrConstructSdfLink(
+  const Identity &_modelID,
+  const ::sdf::Link &_sdfLink)
+{
+  // Check if there is a model with the requested name
+  const std::string linkName = _sdfLink.Name();
+
+  for (const auto &link : this->links)
+  {
+    const auto &linkInfo = link.second;
+    if (linkInfo->name == linkName)
+    {
+      // A link was previously created with that name,
+      // Return its entity value
+      return link.first;
+    }
+  }
+  return this->ConstructSdfLink(_modelID, _sdfLink);
+}
+
 }
 }
 }
diff --git a/bullet/src/SDFFeatures.hh b/bullet/src/SDFFeatures.hh
index 3202c05e4..ac40b3e35 100644
--- a/bullet/src/SDFFeatures.hh
+++ b/bullet/src/SDFFeatures.hh
@@ -40,6 +40,9 @@ class SDFFeatures :
   private: Identity ConstructSdfCollision(
       const Identity &_linkID,
       const ::sdf::Collision &_collision) override;
+  private: std::size_t FindOrConstructSdfLink(
+      const Identity &_modelID,
+      const ::sdf::Link &_sdfLink);
 };
 
 }