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

include/hpp/model/srdf/parser.hh

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

include/hpp/model/urdf/parser.hh

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

include/hpp/model/urdf/util.hh

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

src/srdf/parser.cc

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

src/urdf/parser.cc

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

src/urdf/util.cc

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