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

include/hpp/model/srdf/parser.hh

@@ -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_;
 

include/hpp/model/urdf/parser.hh

@@ -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.

src/srdf/parser.cc

@@ -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 "+

src/urdf/util.cc

@@ -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 ();