Add the possibility to specify the mesh format

It fixes #61
icub-tech-iit · Dec 29, 2023 · 639073f · 639073f
1 parent ed6b2d1
commit 639073f
Show file tree

Hide file tree

Showing 5 changed files with 68 additions and 24 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -8,6 +8,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 - Removed the assumptions on the matching of axis and csys for defining joints.
 - Added the possibility to chose the output folder.
+- Added the possibility to chose the mesh format.
 
 ## [0.4.1] - 2023-11-23
 

diff --git a/README.md b/README.md
@@ -122,6 +122,7 @@ The parameter accepted by the plugin are documented in the following.
 | `stringToRemoveFromMeshFileName` | String |  None | This parameter allows to specify a string that will be removed from the mesh file names. Example: "_prt"  |
 | `assignedCollisionGeometry` | Array |  None | Structure for redefining the collision geometry for a given link.  |
 | `assignedColors` | Map |  {} (Empty Map) | If a link is in this map, the color found in the SimMechanics file is substituted with the one passed through this map. The color is represented by a 4 element vector of containing numbers from 0 to 1 representing the red, green, blue and alpha component.  |
+| `meshFormat` | String |  `stl_binary` | Format of the meshes exported. Allowed values: `stl_binary`, `stl_ascii`, `step`, `neutral` |
 
 ###### Assigned collision geometries (keys of elements of `assignedCollisionGeometry`)
 | Attribute name   | Type   | Default Value | Description  |

diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h
@@ -29,7 +29,7 @@ class Creo2Urdf : public pfcUICommandActionListener {
     void readAssignedInertiasFromConfig();
     void readAssignedCollisionGeometryFromConfig();
     void readExportedFramesFromConfig();
-    bool addMeshAndExport(pfcModel_ptr component_handle, const std::string& stl_transform);
+    bool addMeshAndExport(pfcModel_ptr component_handle, const std::string& mesh_transform);
     bool loadYamlConfig(const std::string& filename);
     std::string getRenameElementFromConfig(const std::string& elem_name);
 

diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h
@@ -13,6 +13,7 @@
 #include <string>
 #include <array>
 #include <map>
+#include <unordered_map>
 
 #include <pfcGlobal.h>
 #include <pfcModel.h>
@@ -49,6 +50,12 @@ constexpr double rad2deg = 180.0 / M_PI;
 constexpr double deg2rad = 1 / rad2deg;
 constexpr double gravity_z = -9.81;
 
+const std::unordered_map<std::string, std::string> mesh_types_supported_extension_map{{"stl_binary", ".stl"},
+                                                                                      {"stl_ascii",  ".stl"},
+                                                                                      {"neutral",    ".neu"},
+                                                                                      {"step",       ".stp"}
+};
+
 enum class c2uLogLevel
 {
     NONE = 0,

diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp
@@ -8,6 +8,7 @@
 
 #include <creo2urdf/Creo2Urdf.h>
 #include <creo2urdf/Utils.h>
+#include <pfcExceptions.h>
 
 void Creo2Urdf::OnCommand() {
 
@@ -558,48 +559,82 @@ void Creo2Urdf::readExportedFramesFromConfig() {
     }
 }
 
-bool Creo2Urdf::addMeshAndExport(pfcModel_ptr component_handle, const std::string& stl_transform)
+bool Creo2Urdf::addMeshAndExport(pfcModel_ptr component_handle, const std::string& mesh_transform)
 {
     std::string file_extension = ".stl";
-    std::string link_child_name = component_handle->GetFullName();
-    std::string renamed_link_child_name = link_child_name;
+    std::string link_name = component_handle->GetFullName();
+    std::string renamed_link_name = link_name;
 
-    if (config["rename"][link_child_name].IsDefined())
+    if (config["rename"][link_name].IsDefined())
     {
-        renamed_link_child_name = config["rename"][link_child_name].Scalar();
+        renamed_link_name = config["rename"][link_name].Scalar();
     }
 
 
     if (config["stringToRemoveFromMeshFileName"].IsDefined())
     {
-        link_child_name.erase(link_child_name.find(config["stringToRemoveFromMeshFileName"].Scalar()), 
+        link_name.erase(link_name.find(config["stringToRemoveFromMeshFileName"].Scalar()), 
             config["stringToRemoveFromMeshFileName"].Scalar().length());
     }
+    std::string meshFormat = "stl_binary";
+    if (config["meshFormat"].IsDefined()) {
+
+        meshFormat = config["meshFormat"].Scalar();
+        if (mesh_types_supported_extension_map.find(meshFormat) != mesh_types_supported_extension_map.end()) {
+            file_extension = mesh_types_supported_extension_map.at(meshFormat);
+        }
+        else {
+            printToMessageWindow("Mesh format " + meshFormat + " is not supported", c2uLogLevel::WARN);
+            if (warningsAreFatal) {
+                return false;
+            }
+        }
+    }
 
     // Make all alphabetic characters lowercase
     if (config["forcelowercase"].as<bool>())
     {
-        std::transform(link_child_name.begin(), link_child_name.end(), link_child_name.begin(),
+        std::transform(link_name.begin(), link_name.end(), link_name.begin(),
             [](unsigned char c) { return std::tolower(c); });
     }
 
-    std::string stl_file_name = m_output_path + "\\" + link_child_name + file_extension;
+    std::string mesh_file_name = m_output_path + "\\" + link_name;
+
+    if (meshFormat != "step") {
+        mesh_file_name += file_extension;
+    }
     try {
-        component_handle->Export(stl_file_name.c_str(), pfcExportInstructions::cast(pfcSTLBinaryExportInstructions().Create(stl_transform.c_str())));
+        if (meshFormat == "stl_binary") {
+            component_handle->Export(mesh_file_name.c_str(), pfcExportInstructions::cast(pfcSTLBinaryExportInstructions().Create(mesh_transform.c_str())));
+        }
+        else if(meshFormat =="stl_ascii") {
+            component_handle->Export(mesh_file_name.c_str(), pfcExportInstructions::cast(pfcSTLASCIIExportInstructions().Create(mesh_transform.c_str())));
+        }
+        else if (meshFormat == "neutral") {
+            component_handle->Export(mesh_file_name.c_str(), pfcExportInstructions::cast(pfcNEUTRALFileExportInstructions().Create()));
+        }
+        else if (meshFormat == "step") {
+            component_handle->ExportIntf3D(mesh_file_name.c_str(), pfcExportType::pfcEXPORT_STEP);
+        }
+        else {
+            return false;
+        }
+
     }
     xcatchbegin
     xcatchcip(defaultEx)
     {
-        // TODO: find a way to get the error message
-        printToMessageWindow(": exception caught: ");
+        printToMessageWindow(": exception caught: " + string(pfcXPFC::cast(defaultEx)->GetMessage()));
         return false;
     }
     xcatchend
 
     // Replace the first 5 bytes of the binary file with a string different than "solid"
     // to avoid issues with stl parsers.
     // For details see: https://github.com/icub-tech-iit/creo2urdf/issues/16
-    sanitizeSTL(stl_file_name);
+    if (meshFormat == "stl_binary") {
+        sanitizeSTL(mesh_file_name);
+    }
 
     // Lets add the mesh to the link
     iDynTree::ExternalMesh visualMesh;
@@ -609,10 +644,10 @@ bool Creo2Urdf::addMeshAndExport(pfcModel_ptr component_handle, const std::strin
     iDynTree::Vector4 color;
     iDynTree::Material material;
 
-    if(config["assignedColors"][renamed_link_child_name].IsDefined())
+    if(config["assignedColors"][renamed_link_name].IsDefined())
     {
-        for (size_t i = 0; i < config["assignedColors"][renamed_link_child_name].size(); i++)
-            color(i) = config["assignedColors"][renamed_link_child_name][i].as<double>();
+        for (size_t i = 0; i < config["assignedColors"][renamed_link_name].size(); i++)
+            color(i) = config["assignedColors"][renamed_link_name][i].as<double>();
     } 
     else
     {
@@ -633,35 +668,35 @@ bool Creo2Urdf::addMeshAndExport(pfcModel_ptr component_handle, const std::strin
     }
 
     // We assume there is only one of occurrence to replace
-    file_format.replace(file_format.find("%s"), file_format.length(), link_child_name);
+    file_format.replace(file_format.find("%s"), file_format.length(), link_name);
     file_format += file_extension;
 
     visualMesh.setFilename(file_format);
 
-    if (assigned_collision_geometry_map.find(renamed_link_child_name) != assigned_collision_geometry_map.end()) {
-        auto geometry_info = assigned_collision_geometry_map.at(renamed_link_child_name);
+    if (assigned_collision_geometry_map.find(renamed_link_name) != assigned_collision_geometry_map.end()) {
+        auto geometry_info = assigned_collision_geometry_map.at(renamed_link_name);
         switch (geometry_info.shape)
         {
         case ShapeType::Box: {
             iDynTree::Box idyn_box;
             idyn_box.setX(geometry_info.size[0]); idyn_box.setY(geometry_info.size[1]); idyn_box.setZ(geometry_info.size[2]);
             idyn_box.setLink_H_geometry(geometry_info.link_H_geometry);
-            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_child_name)].push_back(idyn_box.clone());
+            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_name)].push_back(idyn_box.clone());
         }
             break;
         case ShapeType::Cylinder: {
             iDynTree::Cylinder idyn_cylinder;
             idyn_cylinder.setLength(geometry_info.length);
             idyn_cylinder.setRadius(geometry_info.radius);
             idyn_cylinder.setLink_H_geometry(geometry_info.link_H_geometry);
-            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_child_name)].push_back(idyn_cylinder.clone());
+            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_name)].push_back(idyn_cylinder.clone());
         }
             break;
         case ShapeType::Sphere: {
             iDynTree::Sphere idyn_sphere;
             idyn_sphere.setRadius(geometry_info.radius);
             idyn_sphere.setLink_H_geometry(geometry_info.link_H_geometry);
-            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_child_name)].push_back(idyn_sphere.clone());
+            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_name)].push_back(idyn_sphere.clone());
         }
             break;
         case ShapeType::None:
@@ -672,9 +707,9 @@ bool Creo2Urdf::addMeshAndExport(pfcModel_ptr component_handle, const std::strin
 
     }
     else {
-        idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_child_name)].push_back(visualMesh.clone());
+        idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_name)].push_back(visualMesh.clone());
     }
-    idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_child_name)].push_back(visualMesh.clone());
+    idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(renamed_link_name)].push_back(visualMesh.clone());
 
 
     return true;