[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

@@ -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);
+}
+
 }
 }
 }

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);
 };
 
 }