Commit 1a1e9b02 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

srdf::Parser uses corresonding urdf::Parser instead of reparsing urdf.

parent 24fdaffc
......@@ -53,7 +53,9 @@ namespace hpp
typedef urdf::Parser::RobotPtrType RobotPtrType;
/// \brief Default constructor.
explicit Parser ();
/// \param parser the corresponding URDF parse
/// \note the parser will NOT be deleted by the destructor
explicit Parser (urdf::Parser* parser);
/// \brief Destructor.
virtual ~Parser ();
......@@ -73,19 +75,17 @@ namespace hpp
///
/// See resource_retriever documentation for more information.
///
/// \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,
/// \note the inner URDF parser should have been updated before.
void parse (const std::string& semanticResourceName,
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,
/// \note the inner URDF parser should have been updated before.
void parseFromParameter (const std::string& srdfParameterName,
RobotPtrType robot);
/// \brief Process information parsed from a file or a parameter
......@@ -105,7 +105,7 @@ namespace hpp
std::string& jointType);
private:
::urdf::Model urdfModel_;
urdf::Parser* urdfParser_;
::srdf::Model srdfModel_;
RobotPtrType robot_;
......
......@@ -43,6 +43,10 @@ namespace hpp
{
namespace model
{
namespace srdf {
class Parser;
}
namespace urdf
{
/// \brief Parse an URDF file and return a
......@@ -241,6 +245,8 @@ namespace hpp
std::vector <fcl::Vec3f> vertices_;
std::vector <fcl::Triangle> triangles_;
ObjectFactory objectFactory_;
friend class srdf::Parser;
}; // class Parser
} // end of namespace urdf.
} // end of namespace model.
......
......@@ -42,10 +42,8 @@ namespace hpp
{
namespace srdf
{
Parser::Parser ()
: urdfModel_ (),
srdfModel_ (),
robot_ ()
Parser::Parser (urdf::Parser* parser)
: urdfParser_ (parser), srdfModel_ (), robot_ ()
{}
Parser::~Parser ()
......@@ -68,12 +66,13 @@ namespace hpp
void Parser::addCollisionPairs ()
{
JointVector_t joints = robot_->getJointVector ();
typedef urdf::Parser::MapHppJointType MapHppJointType;
MapHppJointType& jmap = urdfParser_->jointsMap_;
// Cycle through all joint pairs
for (JointVector_t::iterator it1 = joints.begin ();
it1 != joints.end (); it1++) {
JointPtr_t joint1 = *it1;
for (MapHppJointType::const_iterator it1 = jmap.begin ();
it1 != jmap.end (); it1++) {
JointPtr_t joint1 = it1->second;
hppDout (info, "Cycling through joint " << joint1->name ()
<< ": " << joint1);
Body* body1 = joint1->linkedBody ();
......@@ -83,9 +82,9 @@ namespace hpp
} else {
hppDout (info, "body " << body1);
std::string bodyName1 = body1->name ();
for (JointVector_t::iterator it2 = joints.begin ();
for (MapHppJointType::const_iterator it2 = jmap.begin ();
it2 != it1; it2++) {
JointPtr_t joint2 = *it2;
JointPtr_t joint2 = it2->second;
hppDout (info, " Cycling through joint " << joint2->name ());
Body* body2 = joint2->linkedBody ();
if (!body2) {
......@@ -133,7 +132,7 @@ namespace hpp
const std::string& jointName,
std::string& jointType)
{
switch (urdfModel_.joints_[jointName]->type)
switch (urdfParser_->model_.joints_[jointName]->type)
{
case ::urdf::Joint::REVOLUTE:
if (dofs.size () != 1)
......@@ -185,19 +184,11 @@ namespace hpp
}
void
Parser::parse (const std::string& robotResourceName,
const std::string& semanticResourceName,
Parser::parse (const std::string& semanticResourceName,
Parser::RobotPtrType robot)
{
resource_retriever::Retriever resourceRetriever;
resource_retriever::MemoryResource robotResource =
resourceRetriever.get(robotResourceName);
std::string robotDescription;
robotDescription.resize(robotResource.size);
for (unsigned i = 0; i < robotResource.size; ++i)
robotDescription[i] = robotResource.data.get()[i];
resource_retriever::MemoryResource semanticResource =
resourceRetriever.get(semanticResourceName);
std::string semanticDescription;
......@@ -207,19 +198,11 @@ namespace hpp
// 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_.initString (robotDescription))
{
throw std::runtime_error ("Failed to open URDF file:\n"+
robotDescription);
}
// Parse srdf model.
if (!srdfModel_.initString (urdfModel_, semanticDescription))
if (!srdfModel_.initString (urdfParser_->model_, semanticDescription))
{
throw std::runtime_error ("Failed to open SRDF file:\n"
+ semanticDescription);
......@@ -227,29 +210,20 @@ namespace hpp
processSemanticDescription ();
}
void Parser::parseFromParameter (const std::string& urdfParameterName,
const std::string& srdfParameterName,
void Parser::parseFromParameter (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)) {
if (srdfModel_.initString (urdfParser_->model_, semanticDescription)) {
processSemanticDescription ();
} else {
throw std::runtime_error ("Failed to parse ROS parameter "+
......
......@@ -37,7 +37,7 @@ namespace hpp
const std::string& srdfSuffix)
{
hpp::model::urdf::Parser urdfParser (rootJointType, robot);
hpp::model::srdf::Parser srdfParser;
hpp::model::srdf::Parser srdfParser (&urdfParser);
std::string urdfPath = "package://" + package + "/urdf/"
+ modelName + urdfSuffix + ".urdf";
......@@ -48,7 +48,7 @@ namespace hpp
urdfParser.parse (urdfPath);
hppDout (notice, "Finished parsing URDF file.");
// Set Collision Check Pairs
srdfParser.parse (urdfPath, srdfPath, robot);
srdfParser.parse (srdfPath, robot);
hppDout (notice, "Finished parsing SRDF file.");
}
......@@ -60,7 +60,7 @@ namespace hpp
const std::string& srdfSuffix)
{
hpp::model::urdf::Parser urdfParser (rootJointType, robot);
hpp::model::srdf::Parser srdfParser;
hpp::model::srdf::Parser srdfParser (&urdfParser);
std::string urdfPath = "package://" + package + "/urdf/"
+ modelName + urdfSuffix + ".urdf";
......@@ -77,7 +77,7 @@ namespace hpp
// Set Collision Check Pairs
srdfParser.parse (urdfPath, srdfPath, robot);
srdfParser.parse (srdfPath, robot);
hppDout (notice, "Finished parsing SRDF file.");
}
......@@ -87,13 +87,13 @@ namespace hpp
const std::string& srdfParameter)
{
hpp::model::urdf::Parser urdfParser (rootJointType, robot);
hpp::model::srdf::Parser srdfParser;
hpp::model::srdf::Parser srdfParser (&urdfParser);
// Build robot model from URDF.
urdfParser.parseFromParameter (urdfParameter);
hppDout (notice, "Finished parsing URDF file.");
// Set Collision Check Pairs
srdfParser.parseFromParameter (urdfParameter, srdfParameter, robot);
srdfParser.parseFromParameter (srdfParameter, robot);
hppDout (notice, "Finished parsing SRDF file.");
}
......@@ -104,13 +104,13 @@ namespace hpp
const std::string& srdfParameter)
{
hpp::model::urdf::Parser urdfParser (rootJointType, robot);
hpp::model::srdf::Parser srdfParser;
hpp::model::srdf::Parser srdfParser (&urdfParser);
// Build robot model from URDF.
urdfParser.parseFromParameter (urdfParameter);
hppDout (notice, "Finished parsing URDF file.");
// Set Collision Check Pairs
srdfParser.parseFromParameter (urdfParameter, srdfParameter, robot);
srdfParser.parseFromParameter (srdfParameter, robot);
hppDout (notice, "Finished parsing SRDF file.");
// Look for special joints and attach them to the model.
urdfParser.setSpecialJoints ();
......
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