add support to read robot description from parameter server, plus add regression tests

urdf/include/urdf/model.h

@@ -58,6 +58,8 @@ public:
   bool initXml(TiXmlDocument *xml);
   /// \brief Load Model given a filename
   bool initFile(const std::string& filename);
+  /// \brief Load Model given the name of a parameter on the parameter server
+  bool initParam(const std::string& param);
   /// \brief Load Model from a XML-string
   bool initString(const std::string& xmlstring);
 

urdf/src/model.cpp

@@ -66,6 +66,18 @@ bool Model::initFile(const std::string& filename)
 }
 
 
+bool Model::initParam(const std::string& param)
+{
+  ros::NodeHandle nh;
+  std::string xml_string;
+  if (!nh.getParam(param, xml_string)){
+      ROS_ERROR("Could not find parameter %s on parameter server", param.c_str());
+      return false;
+    }
+  return initString(xml_string);
+}
+
+
 bool Model::initString(const std::string& xml_string)
 {
   TiXmlDocument xml_doc;

urdf/test/test_robot_model_parser.cpp

@@ -81,6 +81,10 @@ TEST_F(TestParser, test)
     else
       ASSERT_TRUE(robot.initFile(folder + file));
   }
+
+  // test reading from parameter server
+  ASSERT_TRUE(robot.initParam("robot_description"));
+  ASSERT_FALSE(robot.initParam("robot_description_wim"));
   SUCCEED();
 }
 

urdf/test/test_robot_model_parser.launch

@@ -1,3 +1,5 @@
 <launch>
+  <param name="robot_description" textfile="$(find urdf)/test/pr2_desc.urdf" />
+
   <test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf fail_pr2_desc_double_joint.urdf fail_pr2_desc_double.urdf fail_pr2_desc_loop.urdf fail_pr2_desc_no_filename_in_mesh.urdf fail_pr2_desc_no_joint2.urdf fail_pr2_desc_parent_itself.urdf fail_pr2_desc_two_trees.urdf fail_three_links_one_joint.urdf no_visual.urdf one_link.urdf pr2_desc_explicit_world.urdf pr2_desc_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf" />
 </launch>