[C++][Pyhton][Geom parser] ROS_PACKAGE_PATHS directories are now added to the...

[C++][Pyhton][Geom parser] ROS_PACKAGE_PATHS directories are now added to the ones specified by user

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

@@ -65,28 +65,38 @@ namespace se3
 
 
     /**
-     * @brief      Build The GeometryModel from a URDF file 
+     * @brief      Build The GeometryModel from a URDF file. Search for meshes
+     *             in the directories specified by the user first and then in
+     *             the environment variable ROS_PACKAGE_PATH
      *
-     * @param      model         The model of the robot, built with urdf::buildModel
+     * @param      model         The model of the robot, built with
+     *                           urdf::buildModel
      * @param[in]  filename      The URDF complete (absolute) file path
-     * @param[in]  package_dirs  A vector containing the different directories where to search for packages
-     * @param[in]  root_joint    If we added a root joint to the Model in addition to the urdf kinematic chain
+     * @param[in]  package_dirs  A vector containing the different directories
+     *                           where to search for packages
+     * @param[in]  root_joint    If we added a root joint to the Model in
+     *                           addition to the urdf kinematic chain
      *
-     * @return     The GeometryModel associated to the urdf file and the Model given
+     * @return     The GeometryModel associated to the urdf file and the Model
+     *             given
      */
     inline GeometryModel buildGeom(const Model & model,
                                   const std::string & filename,
-                                  const std::vector<std::string> & package_dirs,
+                                  std::vector<std::string> & package_dirs,
                                   const bool root_joint = false );
 
     /**
-     * @brief      Build The GeometryModel from a URDF file 
+     * @brief      Build The GeometryModel from a URDF file. Search for meshes
+     *             in the environment variable ROS_PACKAGE_PATH
      *
-     * @param      model         The model of the robot, built with urdf::buildModel
+     * @param      model       The model of the robot, built with
+     *                         urdf::buildModel
      * @param[in]  filename      The URDF complete (absolute) file path
-     * @param[in]  root_joint    If we added a root joint to the Model in addition to the urdf kinematic chain
+     * @param[in]  root_joint  If we added a root joint to the Model in addition
+     *                         to the urdf kinematic chain
      *
-     * @return     The GeometryModel associated to the urdf file and the Model given
+     * @return     The GeometryModel associated to the urdf file and the Model
+     *             given
      */
     inline GeometryModel buildGeom(const Model & model,
                                   const std::string & filename,

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

@@ -144,11 +144,12 @@ namespace se3
 
     inline GeometryModel buildGeom(const Model & model,
                                   const std::string & filename,
-                                  const std::vector<std::string> & package_dirs,
+                                  std::vector<std::string> & package_dirs,
                                   const bool root_joint)
     {
       GeometryModel model_geom(model);
 
+      appendRosPackagePaths(package_dirs);
       ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
       parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs, root_joint);
       return model_geom;
@@ -160,8 +161,11 @@ namespace se3
     {
       GeometryModel model_geom(model);
 
+      std::vector<std::string> package_dirs;
+      appendRosPackagePaths(package_dirs);
+
       ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
-      parseTreeForGeom(urdfTree->getRoot(), model, model_geom, getRosPackagePaths(), root_joint);
+      parseTreeForGeom(urdfTree->getRoot(), model, model_geom, package_dirs, root_joint);
       return model_geom;
     }
 

src/python/parsers.hpp

@@ -100,7 +100,7 @@ namespace se3
       static GeometryModelHandler
       buildGeomFromUrdf(const ModelHandler & model,
                         const std::string & filename,
-                        const std::vector<std::string> & package_dirs,
+                        std::vector<std::string> & package_dirs,
                         const bool root_added
                         )
       {
@@ -152,7 +152,7 @@ namespace se3
       bp::to_python_converter<std::pair<ModelHandler, GeometryModelHandler>, PairToTupleConverter<ModelHandler, GeometryModelHandler> >();
       
       bp::def("buildGeomFromUrdf",
-              static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, const std::vector<std::string> &, const bool)> (&ParsersPythonVisitor::buildGeomFromUrdf),
+              static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, std::vector<std::string> &, const bool)> (&ParsersPythonVisitor::buildGeomFromUrdf),
               bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)",
                        "bool stating if we added a custom root joint to the Model"),
               "Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "

src/tools/file-explorer.hpp

@@ -28,13 +28,13 @@ namespace se3
 {
 
     /**
-     * @brief      Parse env variable ROS_PACKAGE_PATH to extract paths
+     * @brief      Parse env variable ROS_PACKAGE_PATH to extract paths and append them
+     *              in the package_dirs variable
      *
-     * @return     The different paths in ROS_PACKAGE_PATH
+     * @param[in][out]  paths  { The package directories where to search for meshes }
      */
-    inline std::vector < std::string > getRosPackagePaths()
+    void appendRosPackagePaths(std::vector<std::string> & package_dirs)
     {
-        std::vector<std::string> results;
 
         std::string delimiter = ":";
         std::string policyStr = std::getenv("ROS_PACKAGE_PATH");
@@ -44,14 +44,13 @@ namespace se3
         {
             size_t offset = policyStr.find_first_of(delimiter, lastOffset);
             if (offset < policyStr.size())
-              results.push_back(policyStr.substr(lastOffset, offset - lastOffset));
+              package_dirs.push_back(policyStr.substr(lastOffset, offset - lastOffset));
             if (offset == std::string::npos)
                 break;
             else
                 lastOffset = offset + 1; // add one to skip the delimiter
         }
 
-        return results;
     }
 
 }