Add addValidNamesToAllSolidShapes function to ModelTransformers (#1173)

src/model/include/iDynTree/ModelTransformers.h

@@ -14,10 +14,14 @@
 #ifndef IDYNTREE_MODEL_TRANSFORMERS_H
 #define IDYNTREE_MODEL_TRANSFORMERS_H
 
+#include <string>
+#include <vector>
+
 namespace iDynTree
 {
 class Model;
 class SensorsList;
+class Traversal;
 
 /**
  * \function Remove all fake links in the model, transforming them in frames.
@@ -101,6 +105,29 @@ bool createModelWithNormalizedJointNumbering(const Model& model,
 bool extractSubModel(const iDynTree::Model& fullModel, const iDynTree::Traversal& subModelTraversal,
                      iDynTree::Model& outputSubModel);
 
+/**
+ * Add automatically generated names to all visual and collision solid shapes of the model.
+ *
+ * This function creates a new iDynTree::Model that is identical to the input one,
+ * but that sets a valid name for all the visual and collision solid shapes of the model.
+ *
+ * For links that already have at least either collision or visual with valid name,
+ * **all** the corresponding shapes (collision or visual) will not be modified.
+ *
+ * For links in which all shapes of a given type that do not have a valid name, a name will
+ * be generated as the following:
+ *   * If there is a single collision or visual element in the link, it will be named
+ *     <linkName>_collision or <linkName>_visual
+ *   * If there are multiple collision or visual elements in the link, the solid shapes will be
+ *     <linkName>_collision_0, <linkName>_collision_1, ... <linkName>_collision_n or
+ *     <linkName>_visual_0, <linkName>_visual_1, ... <linkName>_visual_n
+ *
+ *
+ * @return true if all went well, false if there was an error in creating the sub model.
+ */
+bool addValidNamesToAllSolidShapes(const iDynTree::Model& inputModel,
+                                   iDynTree::Model& outputModel);
+
 }
 
 

src/model/src/ModelTransformers.cpp

@@ -15,6 +15,7 @@
 
 #include <cassert>
 #include <set>
+#include <vector>
 
 
 namespace iDynTree
@@ -805,4 +806,70 @@ bool extractSubModel(const iDynTree::Model& fullModel, const iDynTree::Traversal
     return  true;
 }
 
+void addValidNamesToAllSolidShapesHelper(iDynTree::Model& outputModel,
+                                         std::vector<std::vector<SolidShape *>> solidShapes,
+                                         std::string suffix)
+{
+    for (iDynTree::LinkIndex lnkIndex = 0; lnkIndex < outputModel.getNrOfLinks(); lnkIndex++)
+    {
+        // Check the number of solid shapes for this link
+        size_t nrOfSolidShapes = solidShapes[lnkIndex].size();
+
+        // If there are not shapes, we just continue
+        if (nrOfSolidShapes==0)
+        {
+            continue;
+        }
+
+        if (nrOfSolidShapes == 1)
+        {
+            // In this case, if there is no valid name the shape will just be called <linkName>_collision or <linkName>_visual
+            if (!solidShapes[lnkIndex][0]->isNameValid())
+            {
+                std::string shapeName = outputModel.getLinkName(lnkIndex) + "_" + suffix;
+                solidShapes[lnkIndex][0]->setName(shapeName);
+            }
+        }
+
+
+        if (nrOfSolidShapes > 1)
+        {
+            bool isThereAtLeastAValidName = false;
+            // In this case, if there is no valid name the shape will just be called <linkName>_collision_0, <linkName>_collision_1, etc etc
+            for(int shapeIdx=0; shapeIdx < solidShapes[lnkIndex].size(); shapeIdx++)
+            {
+                if (solidShapes[lnkIndex][shapeIdx]->isNameValid())
+                {
+                    isThereAtLeastAValidName = true;
+                }
+            }
+
+            // In case all names are invalid, set names
+            if (!isThereAtLeastAValidName)
+            {
+                for(int shapeIdx=0; shapeIdx < solidShapes[lnkIndex].size(); shapeIdx++)
+                {
+                    std::string shapeName = outputModel.getLinkName(lnkIndex) + "_" + suffix + "_" + std::to_string(shapeIdx);
+                    solidShapes[lnkIndex][shapeIdx]->setName(shapeName);
+                }
+            }
+        }
+    }
+
+}
+
+bool addValidNamesToAllSolidShapes(const iDynTree::Model& inputModel,
+                                   iDynTree::Model& outputModel)
+{
+    outputModel = inputModel;
+
+    auto& visualSolidShapes = outputModel.visualSolidShapes().getLinkSolidShapes();
+    auto& collisionSolidShapes = outputModel.collisionSolidShapes().getLinkSolidShapes();
+
+    addValidNamesToAllSolidShapesHelper(outputModel, visualSolidShapes, "visual");
+    addValidNamesToAllSolidShapesHelper(outputModel, collisionSolidShapes, "collision");
+
+    return true;
+}
+
 }

src/model/tests/CMakeLists.txt

@@ -19,5 +19,6 @@ endmacro()
 add_unit_test(Joint)
 add_unit_test(Link)
 add_unit_test(Model)
+add_unit_test(ModelTransformers)
 add_unit_test(SensorsList)
 add_unit_test(ThreeAxisForceTorqueContactSensor)

src/model/tests/ModelTransformersUnitTest.cpp

@@ -0,0 +1,52 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include <iDynTree/ModelTransformers.h>
+
+#include <iDynTree/Model.h>
+#include <iDynTree/Link.h>
+
+#include <iDynTree/TestUtils.h>
+
+#include <algorithm>
+#include <cassert>
+#include <cstdio>
+#include <cstdlib>
+#include <vector>
+
+using namespace iDynTree;
+
+void checkThatOneSphereGetsAName()
+{
+    // Create a model with one shape with one sphere without name
+    // and check that a name is correctly created
+    iDynTree::Model oneSphereModel;
+    iDynTree::Link oneSphereLink;
+    oneSphereModel.addLink("link0", oneSphereLink);
+    oneSphereModel.collisionSolidShapes().getLinkSolidShapes()[0].resize(1);
+    iDynTree::Sphere oneSphere;
+    oneSphere.setLink_H_geometry(Transform::Identity());
+    oneSphere.setRadius(1.0);
+    oneSphereModel.collisionSolidShapes().getLinkSolidShapes()[0][0] = new iDynTree::Sphere(oneSphere);
+
+    // Compute the model does not have a valid name
+    ASSERT_IS_TRUE(!oneSphereModel.collisionSolidShapes().getLinkSolidShapes()[0][0]->isNameValid());
+
+    // Create name with valid names
+    iDynTree::Model oneSphereModelWithValidName;
+
+    // Assert that name are valid
+    ASSERT_IS_TRUE(addValidNamesToAllSolidShapes(oneSphereModel, oneSphereModelWithValidName));
+
+    // Check if the resulting name is valid
+    ASSERT_IS_TRUE(oneSphereModelWithValidName.collisionSolidShapes().getLinkSolidShapes()[0][0]->isNameValid());
+    ASSERT_IS_TRUE(oneSphereModelWithValidName.collisionSolidShapes().getLinkSolidShapes()[0][0]->getName() == "link0_collision");
+}
+
+
+int main()
+{
+    checkThatOneSphereGetsAName();
+
+    return EXIT_SUCCESS;
+}