From 99a0b5062320dc3c3436b6f2b4904990615ba98d Mon Sep 17 00:00:00 2001
From: wim <devnull@localhost>
Date: Wed, 7 Apr 2010 23:50:02 +0000
Subject: [PATCH] add support to read robot description from parameter server,
 plus add regression tests

---
 urdf/include/urdf/model.h                |  2 ++
 urdf/src/model.cpp                       | 12 ++++++++++++
 urdf/test/test_robot_model_parser.cpp    |  4 ++++
 urdf/test/test_robot_model_parser.launch |  2 ++
 4 files changed, 20 insertions(+)

diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h
index c54ba52..264520f 100644
--- a/urdf/include/urdf/model.h
+++ b/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);
 
diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp
index 65e8a14..dd5a56d 100644
--- a/urdf/src/model.cpp
+++ b/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;
diff --git a/urdf/test/test_robot_model_parser.cpp b/urdf/test/test_robot_model_parser.cpp
index 13236f6..0be1a77 100644
--- a/urdf/test/test_robot_model_parser.cpp
+++ b/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();
 }
 
diff --git a/urdf/test/test_robot_model_parser.launch b/urdf/test/test_robot_model_parser.launch
index a6d913e..81f5408 100644
--- a/urdf/test/test_robot_model_parser.launch
+++ b/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>