Used std methods to find the meshes path.

S-Dafarra · Jan 8, 2021 · b359c4b · b359c4b
1 parent c9659a9
commit b359c4b
Show file tree

Hide file tree

Showing 2 changed files with 53 additions and 18 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -107,7 +107,6 @@ endif()
 find_package(Eigen3 3.2.92 REQUIRED)
 find_package(iDynTree 2.0.3 REQUIRED)
 find_package(YARP)
-find_package(Qt5 COMPONENTS Core REQUIRED)
 
 include_directories(${CMAKE_CURRENT_BINARY_DIR}/data)
 configure_file("${CMAKE_CURRENT_SOURCE_DIR}/data/URDFdir.h.in" "${CMAKE_CURRENT_BINARY_DIR}/data/URDFdir.h" @ONLY)
@@ -120,8 +119,7 @@ set(HDR ${CMAKE_CURRENT_BINARY_DIR}/data/URDFdir.h)
 add_executable(dyn-visualizer ${SRC} ${HDR})
 target_link_libraries(dyn-visualizer PRIVATE Eigen3::Eigen
                                              iDynTree::idyntree-core iDynTree::idyntree-modelio-urdf iDynTree::idyntree-visualization
-                                             ${YARP_LIBRARIES}
-                                             Qt5::Core)
+                                             ${YARP_LIBRARIES})
 
 ### Compile- and install-related commands.
 # Create and install CMake configuration files for your project that are

diff --git a/src/main.cpp b/src/main.cpp
@@ -18,18 +18,59 @@
 #include <URDFdir.h>
 #include <cmath>
 #include <chrono>
-#include <QProcessEnvironment>
-#include <QDir>
-#include <QFile>
-#include <QFileInfo>
-
+#include <cstdlib>
+#include <fstream>
 
 int main()
 {
-    QProcessEnvironment env = QProcessEnvironment::systemEnvironment();
-    QString gazeboModelPath = env.value("GAZEBO_MODEL_PATH");
-    QStringList paths = gazeboModelPath.split(":");
-    QDir::setSearchPaths("meshPrefix", paths);
+    const char * env_var_value = std::getenv("GAZEBO_MODEL_PATH");
+
+    std::stringstream env_var_string(env_var_value);
+
+    std::string individualPath;
+    std::vector<std::string> pathList;
+
+    while(std::getline(env_var_string, individualPath, ':'))
+    {
+       pathList.push_back(individualPath);
+    }
+
+    auto isFileExisting = [](const std::string& filename)->bool
+    {
+        if (FILE *file = fopen(filename.c_str(), "r")) {
+            fclose(file);
+            return true;
+        } else {
+            return false;
+        }
+    };
+
+    auto getFilePath = [isFileExisting](const std::string& filename, const std::string& prefixToRemove, const std::vector<std::string>& paths)
+    {
+        if(isFileExisting(filename))
+        {
+            return filename;
+        }
+
+        if (filename.substr(0, prefixToRemove.size()) == prefixToRemove)
+        {
+            std::string filename_noprefix = filename;
+            filename_noprefix.erase(0, prefixToRemove.size());
+            for (size_t i = 0; i < paths.size(); ++i)
+            {
+                std::string testPath;
+                testPath = paths[i] + filename_noprefix;
+                if (isFileExisting(testPath))
+                {
+                    return testPath;
+                }
+            }
+
+        }
+
+        return filename; //By default return the input;
+    };
+
 
     iDynTree::ModelLoader modelLoader;
     std::string pathToModel = yarp::os::ResourceFinder::getResourceFinderSingleton().findFileByName("model.urdf");
@@ -49,12 +90,8 @@ int main()
             {
                 iDynTree::ExternalMesh* mesh = shape->asExternalMesh();
                 std::string originalPath = mesh->getFilename();
-                QString meshPath = QString::fromStdString(originalPath);
-                meshPath.replace("package://", "meshPrefix:");
-                QFile meshFile(meshPath);
-                QFileInfo meshFileInfo(meshFile);
-                std::string meshFileInfoPath = meshFileInfo.absoluteFilePath().toStdString();
-                mesh->setFilename(meshFileInfoPath);
+                std::string otherMeshFile = getFilePath(originalPath, "package:/", pathList);
+                mesh->setFilename(otherMeshFile);
                 iDynTree::Material material;
                 iDynTree::Vector4 color;
                 color[0] = 0.0; //r