Commit ea807f72 authored by jcarpent's avatar jcarpent Committed by Justin Carpentier
Browse files

[C++][Bug fixed] Rework ROS_PACKAGE_PATH parsing.

parent 25f035f5
......@@ -36,6 +36,7 @@
#include <hpp/fcl/BVH/BVH_model.h>
#include "pinocchio/tools/file-explorer.hpp"
#include <boost/filesystem.hpp>
#include <exception>
......
......@@ -132,14 +132,16 @@ namespace se3
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
const std::vector<std::string> & package_dirs)
const std::string & filename,
const std::vector<std::string> & package_dirs)
{
GeometryModel model_geom(model);
std::vector<std::string> hint_directories(package_dirs);
appendRosPackagePaths(hint_directories);
// Append the ROS_PACKAGE_PATH
std::vector<std::string> ros_pkg_paths = extractPathFromEnvVar("ROS_PACKAGE_PATH");
hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), ros_pkg_paths.end());
if(hint_directories.empty())
{
......
......@@ -21,38 +21,42 @@
#include <string>
#include <iostream>
#include <vector>
#include <exception>
#include "boost/filesystem.hpp"
namespace se3
{
/**
* @brief Parse env variable ROS_PACKAGE_PATH to extract paths and append them
* in the package_dirs variable
*
* @param[in][out] paths { The package directories where to search for meshes }
*/
inline void appendRosPackagePaths(std::vector<std::string> & package_dirs)
/**
* @brief Parse an environment variable if exists and extract paths according to the delimiter.
*
* @param[in] env_var_name The name of the environment variable.
* @param[in] delimiter The delimiter between two consecutive paths.
*
* @return The vector of paths extracted from the environment variable value.
*/
inline std::vector<std::string> extractPathFromEnvVar(const std::string & env_var_name, const std::string & delimiter = ":")
{
const char * env_var_value = std::getenv(env_var_name.c_str());
std::vector<std::string> env_var_paths;
if (env_var_value != NULL)
{
std::string delimiter = ":";
std::string policyStr = std::getenv("ROS_PACKAGE_PATH");
size_t lastOffset = 0;
while(true)
{
size_t offset = policyStr.find_first_of(delimiter, lastOffset);
if (offset < policyStr.size())
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
}
std::string policyStr (env_var_value);
size_t lastOffset = 0;
while(true)
{
size_t offset = policyStr.find_first_of(delimiter, lastOffset);
if (offset < policyStr.size())
env_var_paths.push_back(policyStr.substr(lastOffset, offset - lastOffset));
if (offset == std::string::npos)
break;
else
lastOffset = offset + 1; // add one to skip the delimiter
}
}
return env_var_paths;
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment