major cleanup of urdf, urdf_parser, collada_parser and urdf_interface

collada_parser/include/collada_parser/collada_parser.h

@@ -46,30 +46,8 @@
 
 namespace urdf{
 
-class ColladaParser : public ModelInterface
-{
-public:
-
   /// \brief Load Model from string
-  bool initCollada(const std::string &xml_string );
+  boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_string );
-
-protected:
-  /// non-const get Collada Link()
-  void getColladaLink(const std::string& name,boost::shared_ptr<Link> &link) const;
-
-  /// non-const getColladaMaterial()
-  //boost::shared_ptr<Material> getColladaMaterial(const std::string& name) const;
-
-  /// in initXml(), onece all links are loaded,
-  /// it's time to build a tree
-  bool initColladaTree(std::map<std::string, std::string> &parent_link_tree);
-
-  /// in initXml(), onece tree is built,
-  /// it's time to find the root Link
-  bool initColladaRoot(std::map<std::string, std::string> &parent_link_tree);
-
-  friend class ColladaModelReader;
-};
 
 }
 

urdf/CMakeLists.txt

@@ -21,7 +21,9 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
 rosbuild_gensrv()
 
+rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
+
 rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp)
-rosbuild_add_gtest_build_flags(test_parser)
 target_link_libraries(test_parser ${PROJECT_NAME})
+rosbuild_add_gtest_build_flags(test_parser)
 rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch)

urdf/include/urdf/model.h

@@ -45,7 +45,7 @@
 
 namespace urdf{
 
-class Model: public URDFParser, ColladaParser
+class Model: public ModelInterface
 {
 public:
   /// \brief Load Model from TiXMLElement

urdf/src/model.cpp

@@ -38,6 +38,8 @@
 #include <ros/ros.h>
 #include <vector>
 #include "urdf/model.h"
+#include <fstream>
+#include <iostream>
 
 namespace urdf{
 
@@ -49,7 +51,7 @@ bool Model::initFile(const std::string& filename)
 
   // get the entire file
   std::string xml_string;
-  std::fstream xml_file(filename, std::fstream::in);
+  std::fstream xml_file(filename.c_str(), std::fstream::in);
   if (xml_file.is_open())
   {
     while ( xml_file.good() )
@@ -113,22 +115,36 @@ bool Model::initXml(TiXmlElement *robot_xml)
   }
 
   std::stringstream ss;
-  ss << *xml_doc;
+  ss << (*robot_xml);
 
   return Model::initString(ss.str());
 }
 
 bool Model::initString(const std::string& xml_string)
 {
+  boost::shared_ptr<ModelInterface> model;
+
   // necessary for COLLADA compatibility
   if( IsColladaData(xml_string) ) {
     ROS_DEBUG("Parsing robot collada xml string");
-    return this->initCollada(xml_string);
+    model = parseCollada(xml_string);
   }
   else {
     ROS_DEBUG("Parsing robot urdf xml string");
-    return this->initURDF(xml_string);
+    model = parseURDF(xml_string);
   }
+
+  // copy data from model into this object
+  if (model){
+    this->links_ = model->links_;
+    this->joints_ = model->joints_;
+    this->materials_ = model->materials_;
+    this->name_ = model->name_;
+    this->root_link_ = model->root_link_;
+    return true;
+  }
+  else
+    return false;
 }
 
 

urdf_interface/include/urdf_interface/model.h

@@ -88,14 +88,117 @@ public:
     this->root_link_.reset();
   };
 
-  /// \brief get parent Link of a Link given name
+  /// non-const getLink()
-  //virtual boost::shared_ptr<const Link> getParentLink(const std::string& name) const = 0;
+  void getLink(const std::string& name,boost::shared_ptr<Link> &link) const
-  /// \brief get parent Joint of a Link given name
+  {
-  //virtual boost::shared_ptr<const Joint> getParentJoint(const std::string& name) const = 0;
+    boost::shared_ptr<Link> ptr;
-  /// \brief get child Link of a Link given name
+    if (this->links_.find(name) == this->links_.end())
-  //virtual boost::shared_ptr<const Link> getChildLink(const std::string& name) const = 0;
+      ptr.reset();
-  /// \brief get child Joint of a Link given name
+    else
-  //virtual boost::shared_ptr<const Joint> getChildJoint(const std::string& name) const = 0;
+      ptr = this->links_.find(name)->second;
+    link = ptr;
+  };
+
+  /// non-const getMaterial()
+  boost::shared_ptr<Material> getMaterial(const std::string& name) const
+  {
+    boost::shared_ptr<Material> ptr;
+    if (this->materials_.find(name) == this->materials_.end())
+      ptr.reset();
+    else
+      ptr = this->materials_.find(name)->second;
+    return ptr;
+  };
+
+  /// in initXml(), onece all links are loaded,
+  /// it's time to build a tree
+  bool initTree(std::map<std::string, std::string> &parent_link_tree)
+  {
+    // loop through all joints, for every link, assign children links and children joints
+    for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
+    {
+      std::string parent_link_name = joint->second->parent_link_name;
+      std::string child_link_name = joint->second->child_link_name;
+
+      ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child  link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
+      if (parent_link_name.empty() || child_link_name.empty())
+      {
+        ROS_ERROR("    Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
+        return false;
+      }
+      else
+      {
+        // find child and parent links
+        boost::shared_ptr<Link> child_link, parent_link;
+        this->getLink(child_link_name, child_link);
+        if (!child_link)
+        {
+          ROS_ERROR("    child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
+          return false;
+        }
+        this->getLink(parent_link_name, parent_link);
+        if (!parent_link)
+        {
+          ROS_ERROR("    parent link '%s' of joint '%s' not found.  The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"<link name=\"%s\" />\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() );
+          return false;
+        }
+
+        //set parent link for child link
+        child_link->setParent(parent_link);
+
+        //set parent joint for child link
+        child_link->setParentJoint(joint->second);
+
+        //set child joint for parent link
+        parent_link->addChildJoint(joint->second);
+      
+        //set child link for parent link
+        parent_link->addChild(child_link);
+      
+        // fill in child/parent string map
+        parent_link_tree[child_link->name] = parent_link_name;
+      
+        ROS_DEBUG("    now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
+      }
+    }
+
+    return true;
+  };
+
+
+  /// in initXml(), onece tree is built,
+  /// it's time to find the root Link
+  bool initRoot(std::map<std::string, std::string> &parent_link_tree)
+  {
+    this->root_link_.reset();
+
+    // find the links that have no parent in the tree
+    for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)  
+    {
+      std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
+      if (parent == parent_link_tree.end())
+      {
+        // store root link
+        if (!this->root_link_)
+        {
+          getLink(l->first, this->root_link_);
+        }
+        // we already found a root link
+        else{
+          ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
+          return false;
+        }
+      }
+    }
+    if (!this->root_link_)
+    {
+      ROS_ERROR("No root link found. The robot xml is not a valid tree.");
+      return false;
+    }
+    ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
+
+    return true;
+  };
 
   /// \brief complete list of Links
   std::map<std::string, boost::shared_ptr<Link> > links_;
@@ -112,6 +215,8 @@ public:
   ///  hmm...
   boost::shared_ptr<Link> root_link_;
 
+
+
 };
 
 }

urdf_parser/include/urdf_parser/urdf_parser.h

@@ -41,36 +41,12 @@
 #include <map>
 #include <tinyxml/tinyxml.h>
 #include <boost/function.hpp>
-
 #include <urdf_interface/model.h>
 
 
 namespace urdf{
 
-class URDFParser : public ModelInterface
+  boost::shared_ptr<ModelInterface> parseURDF(const std::string &xml_string);
-{
-public:
-  URDFParser();
-
-  /// \brief Load Model from string
-  bool initURDF(const std::string &xml_string );
-
-private:
-  /// non-const getLink()
-  void getURDFLink(const std::string& name,boost::shared_ptr<Link> &link) const;
-
-  /// non-const getURDFMaterial()
-  boost::shared_ptr<Material> getURDFMaterial(const std::string& name) const;
-
-  /// in initXml(), onece all links are loaded,
-  /// it's time to build a tree
-  bool initURDFTree(std::map<std::string, std::string> &parent_link_tree);
-
-  /// in initXml(), onece tree is built,
-  /// it's time to find the root Link
-  bool initURDFRoot(std::map<std::string, std::string> &parent_link_tree);
-
-};
 
 }
 

urdf_parser/src/check_urdf.cpp

@@ -70,8 +70,6 @@ int main(int argc, char** argv)
     return -1;
   }
 
-  URDFParser robot;
-
   std::string xml_string;
   std::fstream xml_file(argv[1], std::fstream::in);
   while ( xml_file.good() )
@@ -82,17 +80,17 @@ int main(int argc, char** argv)
   }
   xml_file.close();
 
-  if (!robot.initURDF(xml_string)){
+  boost::shared_ptr<ModelInterface> robot = parseURDF(xml_string);
+  if (!robot){
     std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
     return -1;
   }
-
+  std::cout << "robot name is: " << robot->getName() << std::endl;
-  std::cout << "robot name is: " << robot.getName() << std::endl;
 
   // get info from parser
   std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
   // get root link
-  boost::shared_ptr<const Link> root_link=robot.getRoot();
+  boost::shared_ptr<const Link> root_link=robot->getRoot();
   if (!root_link) return -1;
 
   std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;

urdf_parser/src/urdf_parser.cpp

@@ -42,14 +42,11 @@
 namespace urdf{
 
 
-URDFParser::URDFParser()
-{
-  this->clear();
-}
 
-bool URDFParser::initURDF(const std::string &xml_string)
+boost::shared_ptr<ModelInterface>  parseURDF(const std::string &xml_string)
 {
-  this->clear();
+  boost::shared_ptr<ModelInterface> model(new ModelInterface);
+  model->clear();
 
   TiXmlDocument xml_doc;
   xml_doc.Parse(xml_string.c_str());
@@ -58,20 +55,19 @@ bool URDFParser::initURDF(const std::string &xml_string)
   if (!robot_xml)
   {
     ROS_ERROR("Could not find the 'robot' element in the xml file");
-    return false;
+    model.reset();
+    return model;
   }
 
-  ROS_DEBUG("Parsing robot xml");
-  if (!robot_xml) return false;
-
   // Get robot name
   const char *name = robot_xml->Attribute("name");
   if (!name)
   {
     ROS_ERROR("No name given for the robot.");
-    return false;
+    model.reset();
+    return model;
   }
-  this->name_ = std::string(name);
+  model->name_ = std::string(name);
 
   // Get all Material elements
   for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
@@ -81,15 +77,16 @@ bool URDFParser::initURDF(const std::string &xml_string)
 
     if (material->initXml(material_xml))
     {
-      if (this->getURDFMaterial(material->name))
+      if (model->getMaterial(material->name))
       {
         ROS_ERROR("material '%s' is not unique.", material->name.c_str());
         material.reset();
-        return false;
+        model.reset();
+        return model;
       }
       else
       {
-        this->materials_.insert(make_pair(material->name,material));
+        model->materials_.insert(make_pair(material->name,material));
         ROS_DEBUG("successfully added a new material '%s'", material->name.c_str());
       }
     }
@@ -97,7 +94,8 @@ bool URDFParser::initURDF(const std::string &xml_string)
     {
       ROS_ERROR("material xml is not initialized correctly");
       material.reset();
-      return false;
+      model.reset();
+      return model;
     }
   }
 
@@ -109,11 +107,11 @@ bool URDFParser::initURDF(const std::string &xml_string)
 
     if (link->initXml(link_xml))
     {
-      if (this->getLink(link->name))
+      if (model->getLink(link->name))
       {
         ROS_ERROR("link '%s' is not unique.", link->name.c_str());
-        link.reset();
+        model.reset();
-        return false;
+        return model;
       }
       else
       {
@@ -123,42 +121,43 @@ bool URDFParser::initURDF(const std::string &xml_string)
         {
           if (!link->visual->material_name.empty())
           {
-            if (this->getURDFMaterial(link->visual->material_name))
+            if (model->getMaterial(link->visual->material_name))
             {
               ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
-              link->visual->material = this->getURDFMaterial( link->visual->material_name.c_str() );
+              link->visual->material = model->getMaterial( link->visual->material_name.c_str() );
             }
             else
             {
               if (link->visual->material)
               {
                 ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
-                this->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
+                model->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
               }
               else
               {
                 ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
-                link.reset();
+                model.reset();
-                return false;
+                return model;
               }
             }
           }
         }
 
-        this->links_.insert(make_pair(link->name,link));
+        model->links_.insert(make_pair(link->name,link));
         ROS_DEBUG("successfully added a new link '%s'", link->name.c_str());
       }
     }
     else
     {
       ROS_ERROR("link xml is not initialized correctly");
-      link.reset();
+      model.reset();
-      return false;
+      return model;
     }
   }
-  if (this->links_.empty()){
+  if (model->links_.empty()){
     ROS_ERROR("No link elements found in urdf file");
-    return false;
+    model.reset();
+    return model;
   }
 
   // Get all Joint elements
@@ -169,23 +168,23 @@ bool URDFParser::initURDF(const std::string &xml_string)
 
     if (joint->initXml(joint_xml))
     {
-      if (this->getJoint(joint->name))
+      if (model->getJoint(joint->name))
       {
         ROS_ERROR("joint '%s' is not unique.", joint->name.c_str());
-        joint.reset();
+        model.reset();
-        return false;
+        return model;
       }
       else
       {
-        this->joints_.insert(make_pair(joint->name,joint));
+        model->joints_.insert(make_pair(joint->name,joint));
         ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str());
       }
     }
     else
     {
       ROS_ERROR("joint xml is not initialized correctly");
-      joint.reset();
+      model.reset();
-      return false;
+      return model;
     }
   }
 
@@ -196,128 +195,22 @@ bool URDFParser::initURDF(const std::string &xml_string)
   parent_link_tree.clear();
 
   // building tree: name mapping
-  if (!this->initURDFTree(parent_link_tree))
+  if (!model->initTree(parent_link_tree))
   {
     ROS_ERROR("failed to build tree");
-    return false;
+    model.reset();
+    return model;
   }
 
   // find the root link
-  if (!this->initURDFRoot(parent_link_tree))
+  if (!model->initRoot(parent_link_tree))
   {
     ROS_ERROR("failed to find root link");
-    return false;
+    model.reset();
+    return model;
   }
  
-  return true;
+  return model;
-}
-
-bool URDFParser::initURDFTree(std::map<std::string, std::string> &parent_link_tree)
-{
-  // loop through all joints, for every link, assign children links and children joints
-  for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
-  {
-    std::string parent_link_name = joint->second->parent_link_name;
-    std::string child_link_name = joint->second->child_link_name;
-
-    ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child  link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
-    if (parent_link_name.empty() || child_link_name.empty())
-    {
-      ROS_ERROR("    Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
-      return false;
-    }
-    else
-    {
-      // find child and parent links
-      boost::shared_ptr<Link> child_link, parent_link;
-      this->getURDFLink(child_link_name, child_link);
-      if (!child_link)
-      {
-        ROS_ERROR("    child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
-        return false;
-      }
-      this->getURDFLink(parent_link_name, parent_link);
-      if (!parent_link)
-      {
-        ROS_ERROR("    parent link '%s' of joint '%s' not found.  The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"<link name=\"%s\" />\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() );
-        return false;
-      }
-
-      //set parent link for child link
-      child_link->setParent(parent_link);
-
-      //set parent joint for child link
-      child_link->setParentJoint(joint->second);
-
-      //set child joint for parent link
-      parent_link->addChildJoint(joint->second);
-      
-      //set child link for parent link
-      parent_link->addChild(child_link);
-      
-      // fill in child/parent string map
-      parent_link_tree[child_link->name] = parent_link_name;
-      
-      ROS_DEBUG("    now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
-    }
-  }
-
-  return true;
-}
-
-
-
-bool URDFParser::initURDFRoot(std::map<std::string, std::string> &parent_link_tree)
-{
-
-  this->root_link_.reset();
-
-  // find the links that have no parent in the tree
-  for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)  
-  {
-    std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
-    if (parent == parent_link_tree.end())
-    {
-      // store root link
-      if (!this->root_link_)
-      {
-         getURDFLink(l->first, this->root_link_);
-      }
-      // we already found a root link
-      else{
-        ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
-        return false;
-      }
-    }
-  }
-  if (!this->root_link_)
-  {
-    ROS_ERROR("No root link found. The robot xml is not a valid tree.");
-    return false;
-  }
-  ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
-
-  return true;
-}
-
-boost::shared_ptr<Material> URDFParser::getURDFMaterial(const std::string& name) const
-{
-  boost::shared_ptr<Material> ptr;
-  if (this->materials_.find(name) == this->materials_.end())
-    ptr.reset();
-  else
-    ptr = this->materials_.find(name)->second;
-  return ptr;
-}
-
-void URDFParser::getURDFLink(const std::string& name,boost::shared_ptr<Link> &link) const
-{
-  boost::shared_ptr<Link> ptr;
-  if (this->links_.find(name) == this->links_.end())
-    ptr.reset();
-  else
-    ptr = this->links_.find(name)->second;
-  link = ptr;
 }
 
 }

urdf_parser/src/urdf_to_graphiz.cpp

@@ -90,8 +90,6 @@ int main(int argc, char** argv)
     return -1;
   }
 
-  URDFParser robot;
-
   // get the entire file
   std::string xml_string;
   std::fstream xml_file(argv[1], std::fstream::in);
@@ -103,14 +101,15 @@ int main(int argc, char** argv)
   }
   xml_file.close();
 
-  if (!robot.initURDF(xml_string)){
+  boost::shared_ptr<ModelInterface> robot = parseURDF(xml_string);
+  if (!robot){
     std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
     return -1;
   }
-  string output = robot.getName();
+  string output = robot->getName();
 
   // print entire tree to file
-  printTree(robot.getRoot(), output+".gv");
+  printTree(robot->getRoot(), output+".gv");
   cout << "Created file " << output << ".gv" << endl;
 
   string command = "dot -Tpdf "+output+".gv  -o "+output+".pdf";

urdf_parser/test/memtest.cpp

@@ -1,4 +1,4 @@
-#include <urdf_parser/urdf_parser.h>
+#include "urdf_parser/urdf_parser.h"
 #include <ros/ros.h>
 #include <fstream>
 #include <iostream>
@@ -6,8 +6,6 @@
 int main(int argc, char** argv){
   ros::init(argc, argv, "memtest");
   while (ros::ok()){
-    urdf::URDFParser urdf;
-
     std::string xml_string;
     std::fstream xml_file(argv[1], std::fstream::in);
     while ( xml_file.good() )
@@ -19,6 +17,6 @@ int main(int argc, char** argv){
     xml_file.close();
 
 
-    urdf.initURDF(xml_string);
+    urdf::parseURDF(xml_string);
   }
 }