Commit f905f7a2 authored by Valenza Florian's avatar Valenza Florian
Browse files

[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
parent 6e5fa47e
......@@ -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,
......
......@@ -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;
}
......
......@@ -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 "
......
......@@ -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;
}
}
......
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