Commit 95564d8e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add overload of buildModel and buildGeom

* in order to build model from XML string (standard in ROS)
parent 79251600
......@@ -24,8 +24,6 @@
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/parsers/urdf/types.hpp"
#include <urdf_model/model.h>
namespace se3
{
namespace urdf
......@@ -59,6 +57,37 @@ namespace se3
Model & model,
const bool verbose = false) throw (std::invalid_argument);
///
/// \brief Build the model from a URDF model with a particular joint as root of the model tree inside
/// the model given as reference argument.
///
/// \param[in] urdfTree the tree build from the URDF
/// \param[in] rootJoint The joint at the root of the model tree.
/// \param[in] verbose Print parsing info.
/// \param[out] model Reference model where to put the parsed information.
/// \return Return the reference on argument model for convenience.
///
/// \note urdfTree can be build from ::urdf::parseURDF
/// or ::urdf::parseURDFFile
Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const JointModelVariant & rootJoint,
Model & model,
const bool verbose = false);
///
/// \brief Build the model from a URDF model
///
/// \param[in] urdfTree the tree build from the URDF
/// \param[in] verbose Print parsing info.
/// \param[out] model Reference model where to put the parsed information.
/// \return Return the reference on argument model for convenience.
///
/// \note urdfTree can be build from ::urdf::parseURDF
/// or ::urdf::parseURDFFile
Model & buildModel (const ::urdf::ModelInterfaceSharedPtr & urdfTree,
Model & model,
const bool verbose = false);
/**
* @brief Build The GeometryModel from a URDF file. Search for meshes
......@@ -87,6 +116,33 @@ namespace se3
const std::vector<std::string> & packageDirs = std::vector<std::string> ())
throw (std::invalid_argument);
/**
* @brief Build The GeometryModel from a URDF model. Search for meshes
* in the directories specified by the user first and then in
* the environment variable ROS_PACKAGE_PATH
*
* @param[in] model The model of the robot, built with
* urdf::buildModel
* @param[in] urdfTree The URDF model
* @param[in] packageDirs A vector containing the different directories
* where to search for models and meshes, typically
* obtained from calling se3::rosPaths()
*
* @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION)
* @param[out] geomModel Reference where to put the parsed information.
*
* @return Returns the reference on geom model for convenience.
*
* \warning If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
*
*/
GeometryModel& buildGeom(const Model & model,
const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & packageDirs = std::vector<std::string> ())
throw (std::invalid_argument);
} // namespace urdf
} // namespace se3
......
......@@ -343,6 +343,17 @@ namespace se3
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs)
throw(std::invalid_argument)
{
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
return buildGeom (model, urdfTree, type, geomModel, package_dirs);
}
GeometryModel& buildGeom(const Model & model,
const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs)
throw(std::invalid_argument)
{
std::vector<std::string> hint_directories(package_dirs);
......@@ -355,7 +366,6 @@ namespace se3
throw std::runtime_error("You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash");
}
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type);
return geomModel;
}
......
......@@ -587,8 +587,7 @@ namespace se3
if (urdfTree)
{
model.name = urdfTree->getName();
ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
return buildModel (urdfTree, root_joint, model, verbose);
}
else
{
......@@ -604,8 +603,7 @@ namespace se3
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
if (urdfTree)
{
model.name = urdfTree->getName();
details::parseRootTree(urdfTree->getRoot(),model,verbose);
return buildModel (urdfTree, model, verbose);
}
else
{
......@@ -616,5 +614,26 @@ namespace se3
return model;
}
Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
const JointModelVariant & root_joint,
Model & model,
const bool verbose)
{
assert(urdfTree);
model.name = urdfTree->getName();
ParseRootTreeVisitor::run(urdfTree->getRoot(),model,root_joint,verbose);
return model;
}
Model& buildModel(const ::urdf::ModelInterfaceSharedPtr & urdfTree,
Model& model,
const bool verbose)
{
assert(urdfTree);
model.name = urdfTree->getName();
details::parseRootTree(urdfTree->getRoot(),model,verbose);
return model;
}
} // namespace urdf
} // namespace se3
......@@ -87,6 +87,10 @@ namespace urdf
#undef URDF_TYPEDEF_CLASS_POINTER
#endif
#else // URDFDOM_TYPEDEF_SHARED_PTR
#include <urdf_world/types.h>
#endif // URDFDOM_TYPEDEF_SHARED_PTR
#endif // ifndef __se3_parsers_urdf_types_hpp__
Markdown is supported
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