Commit 07bceac6 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add functions to load an urdf/srdf model using ros parameters.

parent af993024
......@@ -73,23 +73,23 @@ namespace hpp
///
/// See resource_retriever documentation for more information.
///
/// \param resourceName resource name using the
/// resource_retriever format.
///
/// \param robotResourceName URDF resource name
/// \param semanticResourceName SRDF resource name
/// \param robot the robot being constructed.
void parse (const std::string& robotResourceName,
const std::string& semanticResourceName,
RobotPtrType robot);
/// \brief Parse an SRDF sent as a stream and add semantic
/// information to humanoid robot.
///
/// \param robotDescription URDF stream
/// \param semanticDescription SRDF stream
void parseStream (const std::string& robotDescription,
const std::string& semanticDescription,
RobotPtrType& robot);
/// Parse a ROS parameter containing a srdf robot description
/// \param urdfParameterName name of the ROS parameter,
/// \param srdfParameterName name of the ROS parameter,
/// \param robot the robot being constructed.
void parseFromParameter (const std::string& urdfParameterName,
const std::string& srdfParameterName,
RobotPtrType robot);
/// \brief Process information parsed from a file or a parameter
void processSemanticDescription ();
protected:
/// \brief Add collision pairs to robot.
......
......@@ -78,7 +78,7 @@ namespace hpp
/// \brief Destructor.
virtual ~Parser ();
/// \brief Parse an URDF file and return a humanoid robot.
/// \brief Parse an URDF file and return a robot.
///
/// The URDF file location must use the resource retriever format.
/// For instance, the following strings are allowed:
......@@ -92,9 +92,12 @@ namespace hpp
/// resource_retriever format.
void parse (const std::string& resourceName);
/// \brief Parse an URDF sent as a stream and return a
/// humanoid robot.
void parseStream (const std::string& robotDescription);
/// Parse a ROS parameter containing a urdf robot description
/// \param parameterName name of the ROS parameter
void parseFromParameter (const std::string& parameterName);
/// \brief Build the robot from the urdf description
void buildRobot ();
/// \brief Set special joints in robot.
void setSpecialJoints ();
......
......@@ -54,6 +54,21 @@ namespace hpp
const std::string& urdfSuffix,
const std::string& srdfSuffix);
/// Load robot model from ROS parameter
///
/// \param robot Empty robot created before calling the function.
/// Users can pass an instance of a class deriving from Device.
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param urdfParameter Parameter containing the urdf description of the
/// robot
/// \param srdfParameter Parameter containing the srdf description of the
/// robot
void loadRobotModelFromParameter (const DevicePtr_t& robot,
const std::string& rootJointType,
const std::string& urdfParameter,
const std::string& srdfParameter);
/// Load humanoid robot model by name
///
/// \param robot Empty robot created before calling the function.
......@@ -78,6 +93,22 @@ namespace hpp
const std::string& urdfSuffix,
const std::string& srdfSuffix);
/// Load humanoid robot model from ROS parameter
///
/// \param robot Empty robot created before calling the function.
/// Users can pass an instance of a class deriving from Device.
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param urdfParameter Parameter containing the urdf description of the
/// robot
/// \param srdfParameter Parameter containing the srdf description of the
/// robot
void loadHumanoidModelFromParameter
(const model::HumanoidRobotPtr_t& robot,
const std::string& rootJointType,
const std::string& urdfParameter,
const std::string& srdfParameter);
/// Load only urdf model file
///
/// \param robot Empty robot created before calling the function.
......
......@@ -25,6 +25,7 @@
#include <sstream>
#include <boost/foreach.hpp>
#include <resource_retriever/retriever.h>
#include <ros/node_handle.h>
#include <urdf/model.h>
......@@ -204,14 +205,6 @@ namespace hpp
for (unsigned i = 0; i < semanticResource.size; ++i)
semanticDescription[i] = semanticResource.data.get()[i];
parseStream (robotDescription, semanticDescription, robot);
}
void
Parser::parseStream (const std::string& robotDescription,
const std::string& semanticDescription,
Parser::RobotPtrType& robot)
{
// Reset the attributes to avoid problems when loading
// multiple robots using the same object.
urdfModel_.clear ();
......@@ -231,7 +224,45 @@ namespace hpp
throw std::runtime_error ("Failed to open SRDF file:\n"
+ semanticDescription);
}
processSemanticDescription ();
}
void Parser::parseFromParameter (const std::string& urdfParameterName,
const std::string& srdfParameterName,
RobotPtrType robot)
{
// Reset the attributes to avoid problems when loading
// multiple robots using the same object.
urdfModel_.clear ();
srdfModel_.clear ();
robot_ = robot;
// Parse urdf model.
if (!urdfModel_.initParam (urdfParameterName))
{
throw std::runtime_error ("Failed to read ROS parameter "+
urdfParameterName);
}
// Parse srdf model. srdf::Model does not support direct parameter
// reading. We need to load the parameter value in a string
ros::NodeHandle nh;
std::string semanticDescription;
if (nh.getParam (srdfParameterName, semanticDescription)) {
if (srdfModel_.initString (urdfModel_, semanticDescription)) {
processSemanticDescription ();
} else {
throw std::runtime_error ("Failed to parse ROS parameter "+
srdfParameterName);
}
} else {
throw std::runtime_error ("Failed to read ROS parameter "+
srdfParameterName);
}
}
void Parser::processSemanticDescription ()
{
// Add collision pairs.
addCollisionPairs ();
}
......
......@@ -1120,6 +1120,22 @@ namespace hpp
return transform;
}
void Parser::parseFromParameter (const std::string& parameterName)
{
// Reset the attributes to avoid problems when loading
// multiple robots using the same object.
model_.clear ();
rootJoint_ = 0;
jointsMap_.clear ();
// Parse urdf model.
if (!model_.initParam (parameterName)) {
throw std::runtime_error ("Failed to read parameter " +
parameterName);
}
buildRobot ();
}
void Parser::parse (const std::string& filename)
{
hppDout (info, "filename: " << filename);
......@@ -1132,11 +1148,7 @@ namespace hpp
unsigned i = 0;
for (; i < resource.size; ++i)
robotDescription[i] = resource.data.get()[i];
parseStream (robotDescription);
}
void Parser::parseStream (const std::string& robotDescription)
{
// Reset the attributes to avoid problems when loading
// multiple robots using the same object.
model_.clear ();
......@@ -1148,7 +1160,11 @@ namespace hpp
throw std::runtime_error ("Failed to open urdf file. "
"robotDescription:\n" + robotDescription);
}
buildRobot ();
}
void Parser::buildRobot ()
{
// Get names of special joints.
findSpecialJoints ();
......
......@@ -81,6 +81,43 @@ namespace hpp
hppDout (notice, "Finished parsing SRDF file.");
}
void loadRobotModelFromParameter (const DevicePtr_t& robot,
const std::string& rootJointType,
const std::string& urdfParameter,
const std::string& srdfParameter)
{
hpp::model::urdf::Parser urdfParser (rootJointType, robot);
hpp::model::srdf::Parser srdfParser;
// Build robot model from URDF.
urdfParser.parseFromParameter (urdfParameter);
hppDout (notice, "Finished parsing URDF file.");
// Set Collision Check Pairs
srdfParser.parseFromParameter (urdfParameter, srdfParameter, robot);
hppDout (notice, "Finished parsing SRDF file.");
}
void loadHumanoidModelFromParameter
(const model::HumanoidRobotPtr_t& robot,
const std::string& rootJointType,
const std::string& urdfParameter,
const std::string& srdfParameter)
{
hpp::model::urdf::Parser urdfParser (rootJointType, robot);
hpp::model::srdf::Parser srdfParser;
// Build robot model from URDF.
urdfParser.parseFromParameter (urdfParameter);
hppDout (notice, "Finished parsing URDF file.");
// Set Collision Check Pairs
srdfParser.parseFromParameter (urdfParameter, srdfParameter, robot);
hppDout (notice, "Finished parsing SRDF file.");
// Look for special joints and attach them to the model.
urdfParser.setSpecialJoints ();
// Fill gaze position and direction.
urdfParser.fillGaze ();
}
void loadUrdfModel (const DevicePtr_t& robot,
const std::string& rootJointType,
const std::string& package,
......
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