[C++][GeomParser] Throw runtime if ROS_PACKAGE_PATH is empty

src/multibody/parser/urdf-with-geometry.hxx

@@ -149,7 +149,15 @@ namespace se3
     {
       GeometryModel model_geom(model);
 
-      appendRosPackagePaths(package_dirs);
+      try
+      {
+        appendRosPackagePaths(package_dirs);
+      }
+      catch (std::runtime_error & e)
+      {
+        std::cout << "ROS_PACKAGE_PATH environment variable is empty. Meshes may not be found when looking for geometry" << std::endl;
+        assert(!package_dirs.empty() && "You did not specify any package directory. Geometric parsing will crash" );
+      }
       ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
       parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs, root_joint);
       return model_geom;
@@ -162,7 +170,15 @@ namespace se3
       GeometryModel model_geom(model);
 
       std::vector<std::string> package_dirs;
-      appendRosPackagePaths(package_dirs);
+      try
+      {
+        appendRosPackagePaths(package_dirs);
+      }
+      catch (std::runtime_error & e)
+      {
+        std::cout << "ROS_PACKAGE_PATH environment variable is empty. Meshes may not be found when looking for geometry" << std::endl;
+        assert(false && "You did not specify any package directory. Geometric parsing will crash" );
+      }
 
       ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
       parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs, root_joint);

src/tools/file-explorer.hpp

@@ -21,6 +21,7 @@
 #include <string>
 #include <iostream>
 #include <vector>
+#include <exception>
 
 #include "boost/filesystem.hpp"
 
@@ -33,13 +34,17 @@ namespace se3
      *
      * @param[in][out]  paths  { The package directories where to search for meshes }
      */
-    void appendRosPackagePaths(std::vector<std::string> & package_dirs)
+    void appendRosPackagePaths(std::vector<std::string> & package_dirs) throw (std::runtime_error)
     {
 
         std::string delimiter = ":";
         std::string policyStr = std::getenv("ROS_PACKAGE_PATH");
         size_t lastOffset = 0;
 
+        if(policyStr.empty())
+        {
+          throw std::runtime_error("ROS_PACKAGE_PATH environment variable is empty");
+        }
         while(true)
         {
             size_t offset = policyStr.find_first_of(delimiter, lastOffset);