attempt to refactor, now we have diamond inheritance, and won't compile. make init* static functions.

assimp/Makefile

@@ -5,7 +5,7 @@ PACKAGE_NAME=`rospack find assimp`
 TARBALL_NAME = assimp-svn-811
 TARBALL = build/$(TARBALL_NAME).tar.gz
 TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL_NAME).tar.gz
-TARBALL_PATCH = assimp.patch
+TARBALL_PATCH = assimp.patch define_arch_arm.patch
 UNPACK_CMD = tar xzf
 SOURCE_DIR = build/$(TARBALL_NAME)
 include $(shell rospack find mk)/download_unpack_build.mk

assimp/define_arch_arm.patch

@@ -0,0 +1,29 @@
+--- code/Importer.cpp.orig	2011-06-20 11:57:37.234886987 +0200
++++ code/Importer.cpp	2011-06-20 11:58:26.891136987 +0200
+@@ -904,6 +904,10 @@
+ 		<< " amd64"
+ #elif defined(ASSIMP_BUILD_IA_64BIT_ARCHITECTURE)
+ 		<< " itanium"
++#elif defined(ASSIMP_BUILD_PPC_32BIT_ARCHITECTURE)
++		<< " ppc32"
++#elif defined(ASSIMP_BUILD_ARM_32BIT_ARCHITECTURE)
++		<< " arm"
+ #else
+ #	error unknown architecture
+ #endif
+--- include/aiDefines.h.orig	2011-06-20 11:57:41.570824487 +0200
++++ include/aiDefines.h	2011-06-20 11:59:24.648949487 +0200
+@@ -238,8 +238,12 @@
+ #		define ASSIMP_BUILD_X86_32BIT_ARCHITECTURE
+ #	elif defined(__x86_64__)
+ #		define ASSIMP_BUILD_X86_64BIT_ARCHITECTURE
++#	elif defined(__ppc__)
++#		define ASSIMP_BUILD_PPC_32BIT_ARCHITECTURE
++#	elif defined(__arm__)
++#	       define ASSIMP_BUILD_ARM_32BIT_ARCHITECTURE
+ #	else
+-#		error unknown architecture
++#		error "unknown architecture"
+ #	endif
+ #else
+ #	error unknown compiler

collada_parser/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+# necessary for collada reader to create the temporary dae files due to limitations in the URDF geometry
+check_function_exists(mkstemps HAVE_MKSTEMPS)
+if( HAVE_MKSTEMPS )
+  add_definitions("-DHAVE_MKSTEMPS")
+endif()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(${PROJECT_NAME} src/collada_parser.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+

collada_parser/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

collada_parser/include/collada_parser/collada_parser.h

@@ -0,0 +1,76 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef COLLADA_PARSER_COLLADA_PARSER_H
+#define COLLADA_PARSER_COLLADA_PARSER_H
+
+#include <string>
+#include <map>
+#include <boost/function.hpp>
+
+#include <urdf_interface/model.h>
+
+
+namespace urdf{
+
+class ColladaParser : public ModelInterface
+{
+public:
+
+  /// \brief Load Model from string
+  bool initCollada(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;
+};
+
+}
+
+#endif

collada_parser/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b collada_parser is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

collada_parser/manifest.xml

@@ -0,0 +1,21 @@
+<package>
+  <description brief="collada_parser">
+
+     collada_parser
+
+  </description>
+  <author>John Hsu</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/collada_parser</url>
+  <depend package="roscpp"/>
+  <depend package="urdf_interface"/>
+  <depend package="colladadom"/>
+
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollada_parser"/>
+  </export>
+
+</package>
+
+

collada_parser/src/collada_parser.cpp

@@ -33,8 +33,6 @@
 *********************************************************************/
 
 /* Author: Rosen Diankov, used OpenRAVE files for reference  */
-#include "urdf/model.h"
-
 #include <vector>
 #include <list>
 #include <map>
@@ -56,6 +54,14 @@
 #include <boost/format.hpp>
 #include <boost/shared_ptr.hpp>
 
+#include <ros/ros.h>
+#include <collada_parser/collada_parser.h>
+#include <urdf_interface/model.h>
+
+#ifndef HAVE_MKSTEMPS
+#include <fstream>
+#include <fcntl.h>
+#endif
 #ifndef HAVE_MKSTEMPS
 #include <fstream>
 #include <fcntl.h>
@@ -66,6 +72,7 @@
 
 namespace urdf{
 
+
 class UnlinkFilename
 {
 public:
@@ -382,7 +389,7 @@ class ColladaModelReader : public daeErrorHandler
     };
 
  public:
-    ColladaModelReader(Model& model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
+    ColladaModelReader(ColladaParser& model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
         daeErrorHandler::setErrorHandler(this);
         _resourcedir = ".";
     }
@@ -475,12 +482,12 @@ protected:
     {
         std::map<std::string, std::string> parent_link_tree;
         // building tree: name mapping
-        if (!_model.initTree(parent_link_tree)) {
+        if (!_model.initColladaTree(parent_link_tree)) {
             ROS_ERROR("failed to build tree");
         }
 
         // find the root link
-        if (!_model.initRoot(parent_link_tree)) {
+        if (!_model.initColladaRoot(parent_link_tree)) {
             ROS_ERROR("failed to find root link");
         }
     }
@@ -757,7 +764,7 @@ protected:
         }
 
         boost::shared_ptr<Link> plink;
-        _model.getLink(linkname,plink);
+        _model.getColladaLink(linkname,plink);
         if( !plink ) {
             plink.reset(new Link());
             plink->name = linkname;
@@ -2507,30 +2514,22 @@ protected:
     int _nGlobalSensorId, _nGlobalManipulatorId;
     std::string _filename;
     std::string _resourcedir;
-    Model& _model;
+    ColladaParser& _model;
+
 };
 
-bool urdfFromColladaFile(std::string const& daefilename, Model& model)
+bool urdfFromColladaFile(std::string const& daefilename, ColladaParser& model)
 {
     ColladaModelReader reader(model);
     return reader.InitFromFile(daefilename);
 }
 
-bool urdfFromColladaData(std::string const& data, Model& model)
+bool urdfFromColladaData(std::string const& data, ColladaParser& model)
 {
     ColladaModelReader reader(model);
     return reader.InitFromData(data);
 }
 
-bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model)
-{
-    ColladaModelReader reader(model);
-    // have to convert all xml back to string (sigh..)
-    std::stringstream ss;
-    ss << *robot_xml;
-    return reader.InitFromData(ss.str());
-}
-
 bool IsColladaFile(const std::string& filename)
 {
     size_t len = filename.size();
@@ -2544,4 +2543,107 @@ bool IsColladaData(const std::string& data)
     return data.find("<COLLADA") != std::string::npos;
 }
 
+bool ColladaParser::initCollada(const std::string &xml_str)
+{
+  ColladaModelReader reader(*this);
+  return reader.InitFromData(xml_str);
+}
+
+bool ColladaParser::initColladaTree(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->getColladaLink(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->getColladaLink(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 ColladaParser::initColladaRoot(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_)
+      {
+         getColladaLink(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;
+}
+
+void ColladaParser::getColladaLink(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/CMakeLists.txt

@@ -21,18 +21,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
 rosbuild_gensrv()
 
-# necessary for collada reader to create the temporary dae files due to limitations in the URDF geometry
-check_function_exists(mkstemps HAVE_MKSTEMPS)
-if( HAVE_MKSTEMPS )
-  add_definitions("-DHAVE_MKSTEMPS")
-endif()
-
-#common commands for building c++ executables and libraries
-rosbuild_add_library(${PROJECT_NAME} src/model.cpp src/collada_model_reader.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-
 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})

urdf/include/urdf/model.h

@@ -34,17 +34,18 @@
 
 /* Author: Wim Meeussen */
 
-#ifndef ROBOT_MODEL_URDF_H
+#ifndef URDF_MODEL_H
-#define ROBOT_MODEL_URDF_H
+#define URDF_MODEL_H
 
 #include <string>
 #include <map>
-#include <urdf_parser/parser.h>
+#include <urdf_parser/urdf_parser.h>
+#include <collada_parser/collada_parser.h>
 
 
 namespace urdf{
 
-class Model: public urdf::Parser
+class Model: public URDFParser, ColladaParser
 {
 public:
   /// \brief Load Model from TiXMLElement
@@ -57,8 +58,6 @@ public:
   bool initParam(const std::string& param);
   /// \brief Load Model from a XML-string
   bool initString(const std::string& xmlstring);
-
-  friend class ColladaModelReader;
 };
 
 }

urdf/manifest.xml

@@ -14,6 +14,7 @@
   <depend package="roscpp" />
   <depend package="colladadom" />
   <depend package="urdf_parser" />
+  <depend package="collada_parser" />
 
   <export>
     <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>

urdf/src/model.cpp

@@ -41,23 +41,32 @@
 
 namespace urdf{
 
-bool urdfFromColladaFile(std::string const& daefilename, Model& model);
-bool urdfFromColladaData(std::string const& data, Model& model);
-bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model);
-bool IsColladaFile(const std::string& filename);
 bool IsColladaData(const std::string& data);
 
 
 bool Model::initFile(const std::string& filename)
 {
-  // necessary for COLLADA compatibility
-  if( IsColladaFile(filename) ) {
-    return urdfFromColladaFile(filename,*this);
-  }
-  TiXmlDocument xml_doc;
-  xml_doc.LoadFile(filename);
 
-  return init(&xml_doc);
+  // get the entire file
+  std::string xml_string;
+  std::fstream xml_file(filename, std::fstream::in);
+  if (xml_file.is_open())
+  {
+    while ( xml_file.good() )
+    {
+      std::string line;
+      std::getline( xml_file, line);
+      xml_string += (line + "\n");
+    }
+    xml_file.close();
+    return Model::initString(xml_string);
+  }
+  else
+  {
+    ROS_ERROR("Could not open file [%s] for parsing.",filename.c_str());
+    return false;
+  }
+
 }
 
 
@@ -78,53 +87,50 @@ bool Model::initParam(const std::string& param)
     ROS_ERROR("Could not read parameter %s on parameter server", full_param.c_str());
     return false;
   }
-  return initString(xml_string);
+  return Model::initString(xml_string);
 }
 
-
-bool Model::initString(const std::string& xml_string)
-{
-  // necessary for COLLADA compatibility
-  if( IsColladaData(xml_string) ) {
-    return urdfFromColladaData(xml_string,*this);
-  }
-
-  TiXmlDocument xml_doc;
-  xml_doc.Parse(xml_string.c_str());
-
-  return init(&xml_doc);
-}
-
-
 bool Model::initXml(TiXmlDocument *xml_doc)
 {
   if (!xml_doc)
   {
-    ROS_ERROR("Could not parse the xml");
+    ROS_ERROR("Could not parse the xml document");
     return false;
   }
 
-  // necessary for COLLADA compatibility
+  std::stringstream ss;
-  if( !!xml_doc->RootElement() ) {
+  ss << *xml_doc;
-    if( std::string("COLLADA") == xml_doc->RootElement()->ValueStr() ) {
-      return urdfFromTiXML(xml_doc->RootElement(),*this);
-    }
-  }
 
-  return init(xml_doc);
+  return Model::initString(ss.str());
 }
 
 bool Model::initXml(TiXmlElement *robot_xml)
 {
-  ROS_DEBUG("Parsing robot xml");
+  if (!robot_xml)
-  if (!robot_xml) return false;
+  {
+    ROS_ERROR("Could not parse the xml element");
+    return false;
+  }
 
+  std::stringstream ss;
+  ss << *xml_doc;
+
+  return Model::initString(ss.str());
+}
+
+bool Model::initString(const std::string& xml_string)
+{
   // necessary for COLLADA compatibility
-  if( std::string("COLLADA") == robot_xml->ValueStr() ) {
+  if( IsColladaData(xml_string) ) {
-    return urdfFromTiXML(robot_xml,*this);
+    ROS_DEBUG("Parsing robot collada xml string");
+    return this->initCollada(xml_string);
+  }
+  else {
+    ROS_DEBUG("Parsing robot urdf xml string");
+    return this->initURDF(xml_string);
+  }
 }
 
-  return init(robot_xml);
+
-}
 
 }// namespace

urdf_interface/include/urdf_interface/color.h

@@ -34,8 +34,8 @@
 
 /* Author: Josh Faust */
 
-#ifndef URDF_COLOR_H
+#ifndef URDF_INTERFACE_COLOR_H
-#define URDF_COLOR_H
+#define URDF_INTERFACE_COLOR_H
 
 #include <string>
 #include <vector>

urdf_interface/include/urdf_interface/joint.h

@@ -34,8 +34,8 @@
 
 /* Author: Wim Meeussen */
 
-#ifndef URDF_JOINT_H
+#ifndef URDF_INTERFACE_JOINT_H
-#define URDF_JOINT_H
+#define URDF_INTERFACE_JOINT_H
 
 #include <string>
 #include <vector>

urdf_interface/include/urdf_interface/link.h

@@ -34,8 +34,8 @@
 
 /* Author: Wim Meeussen */
 
-#ifndef URDF_LINK_H
+#ifndef URDF_INTERFACE_LINK_H
-#define URDF_LINK_H
+#define URDF_INTERFACE_LINK_H
 
 #include <string>
 #include <vector>

urdf_interface/include/urdf_interface/model.h

@@ -34,43 +34,68 @@
 
 /* Author: Wim Meeussen */
 
-#ifndef URDF_PARSER_H
+#ifndef URDF_INTERFACE_MODEL_H
-#define URDF_PARSER_H
+#define URDF_INTERFACE_MODEL_H
 
 #include <string>
 #include <map>
-#include <tinyxml/tinyxml.h>
 #include <boost/function.hpp>
 #include <urdf_interface/link.h>
 
 
 namespace urdf {
 
-class Parser
+class ModelInterface
 {
 public:
-  Parser();
-
-  /// \brief Load Model from TiXMLElement
-  bool init(TiXmlElement *xml);
-  /// \brief Load Model from TiXMLDocument
-  bool init(TiXmlDocument *xml);
-
   boost::shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
-  boost::shared_ptr<const Link> getLink(const std::string& name) const;
+  boost::shared_ptr<const Link> getLink(const std::string& name) const
-  boost::shared_ptr<const Joint> getJoint(const std::string& name) const;
+  {
-  const std::string& getName() const {return name_;};
+    boost::shared_ptr<const Link> ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset();
+    else
+      ptr = this->links_.find(name)->second;
+    return ptr;
+  };
 
-  void getLinks(std::vector<boost::shared_ptr<Link> >& links) const;
+  boost::shared_ptr<const Joint> getJoint(const std::string& name) const
+  {
+    boost::shared_ptr<const Joint> ptr;
+    if (this->joints_.find(name) == this->joints_.end())
+      ptr.reset();
+    else
+      ptr = this->joints_.find(name)->second;
+    return ptr;
+  };
+
+
+  const std::string& getName() const {return name_;};
+  void getLinks(std::vector<boost::shared_ptr<Link> >& links) const
+  {
+    for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
+    {
+      links.push_back(link->second);
+    }
+  };
+
+  void clear()
+  {
+    name_.clear();
+    this->links_.clear();
+    this->joints_.clear();
+    this->materials_.clear();
+    this->root_link_.reset();
+  };
 
   /// \brief get parent Link of a Link given name
-  boost::shared_ptr<const Link> getParentLink(const std::string& name) const;
+  //virtual boost::shared_ptr<const Link> getParentLink(const std::string& name) const = 0;
   /// \brief get parent Joint of a Link given name
-  boost::shared_ptr<const Joint> getParentJoint(const std::string& name) const;
+  //virtual boost::shared_ptr<const Joint> getParentJoint(const std::string& name) const = 0;
   /// \brief get child Link of a Link given name
-  boost::shared_ptr<const Link> getChildLink(const std::string& name) const;
+  //virtual boost::shared_ptr<const Link> getChildLink(const std::string& name) const = 0;
   /// \brief get child Joint of a Link given name
-  boost::shared_ptr<const Joint> getChildJoint(const std::string& name) const;
+  //virtual boost::shared_ptr<const Joint> getChildJoint(const std::string& name) const = 0;
 
   /// \brief complete list of Links
   std::map<std::string, boost::shared_ptr<Link> > links_;
@@ -79,30 +104,14 @@ public:
   /// \brief complete list of Materials
   std::map<std::string, boost::shared_ptr<Material> > materials_;
 
-protected:
-  void clear();
-
   std::string name_;
 
-  /// non-const getLink()
+  /// ModelInterface is restricted to a tree for now, which means there exists one root link
-  void getLink(const std::string& name,boost::shared_ptr<Link> &link) const;
-
-  /// non-const getMaterial()
-  boost::shared_ptr<Material> getMaterial(const std::string& name) const;
-
-  /// 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);
-
-  /// 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);
-
-  /// Model is restricted to a tree for now, which means there exists one root link
   ///  typically, root link is the world(inertial).  Where world is a special link
   /// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint?
   ///  hmm...
   boost::shared_ptr<Link> root_link_;
+
 };
 
 }

urdf_interface/include/urdf_interface/pose.h

@@ -34,8 +34,8 @@
 
 /* Author: Wim Meeussen */
 
-#ifndef URDF_POSE_H
+#ifndef URDF_INTERFACE_POSE_H
-#define URDF_POSE_H
+#define URDF_INTERFACE_POSE_H
 
 #include <string>
 #include <vector>
@@ -44,6 +44,9 @@
 #include <boost/algorithm/string.hpp>
 #include <boost/lexical_cast.hpp>
 
+#include <tinyxml/tinyxml.h> // FIXME: remove parser from here
+#include <ros/ros.h>
+
 namespace urdf{
 
 class Vector3

urdf_interface/manifest.xml

@@ -6,7 +6,7 @@
   <license>BSD</license>
   <review status="Doc reviewed" notes=""/>
   <url>http://ros.org/wiki/urdf_interface</url>
-
+  <depend package="tinyxml" />
   <export>
     <cpp cflags="-I${prefix}/include"/>
   </export>

urdf_parser/CMakeLists.txt

@@ -22,7 +22,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 rosbuild_gensrv()
 
 #common commands for building c++ executables and libraries
-rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/parser.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/urdf_parser.cpp)
 #target_link_libraries(${PROJECT_NAME} another_library)
 rosbuild_add_boost_directories()
 #rosbuild_link_boost(${PROJECT_NAME} thread)

urdf_parser/include/urdf_parser/urdf_parser.h

@@ -0,0 +1,77 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_PARSER_URDF_PARSER_H
+#define URDF_PARSER_URDF_PARSER_H
+
+#include <string>
+#include <map>
+#include <tinyxml/tinyxml.h>
+#include <boost/function.hpp>
+
+#include <urdf_interface/model.h>
+
+
+namespace urdf{
+
+class URDFParser : public ModelInterface
+{
+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);
+
+};
+
+}
+
+#endif

urdf_parser/src/check_urdf.cpp

@@ -34,8 +34,9 @@
 
 /* Author: Wim Meeussen */
 
-#include "urdf_parser/parser.h"
+#include "urdf_parser/urdf_parser.h"
 #include <iostream>
+#include <fstream>
 
 using namespace urdf;
 
@@ -68,16 +69,20 @@ int main(int argc, char** argv)
     std::cerr << "Expect xml file to parse" << std::endl;
     return -1;
   }
-  TiXmlDocument robot_model_xml;
-  robot_model_xml.LoadFile(argv[1]);
-  TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
-  if (!robot_xml){
-    std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
-    return -1;
-  }
 
-  Parser robot;
+  URDFParser robot;
-  if (!robot.init(robot_xml)){
+
+  std::string xml_string;
+  std::fstream xml_file(argv[1], std::fstream::in);
+  while ( xml_file.good() )
+  {
+    std::string line;
+    std::getline( xml_file, line);
+    xml_string += (line + "\n");
+  }
+  xml_file.close();
+
+  if (!robot.initURDF(xml_string)){
     std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
     return -1;
   }

urdf_parser/src/urdf_parser.cpp

@@ -37,48 +37,29 @@
 #include <boost/algorithm/string.hpp>
 #include <ros/ros.h>
 #include <vector>
-#include "urdf_parser/parser.h"
+#include "urdf_parser/urdf_parser.h"
 
 namespace urdf{
 
 
-Parser::Parser()
+URDFParser::URDFParser()
 {
   this->clear();
 }
 
-void Parser::clear()
+bool URDFParser::initURDF(const std::string &xml_string)
 {
-  name_.clear();
+  this->clear();
-  this->links_.clear();
-  this->joints_.clear();
-  this->materials_.clear();
-  this->root_link_.reset();
-}
 
+  TiXmlDocument xml_doc;
+  xml_doc.Parse(xml_string.c_str());
 
-
+  TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
-bool Parser::init(TiXmlDocument *xml_doc)
-{
-  if (!xml_doc)
-  {
-    ROS_ERROR("Could not parse the xml");
-    return false;
-  }
-
-  TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot");
   if (!robot_xml)
   {
     ROS_ERROR("Could not find the 'robot' element in the xml file");
     return false;
   }
-  return init(robot_xml);
-}
-
-
-bool Parser::init(TiXmlElement *robot_xml)
-{
-  this->clear();
 
   ROS_DEBUG("Parsing robot xml");
   if (!robot_xml) return false;
@@ -100,7 +81,7 @@ bool Parser::init(TiXmlElement *robot_xml)
 
     if (material->initXml(material_xml))
     {
-      if (this->getMaterial(material->name))
+      if (this->getURDFMaterial(material->name))
       {
         ROS_ERROR("material '%s' is not unique.", material->name.c_str());
         material.reset();
@@ -142,10 +123,10 @@ bool Parser::init(TiXmlElement *robot_xml)
         {
           if (!link->visual->material_name.empty())
           {
-            if (this->getMaterial(link->visual->material_name))
+            if (this->getURDFMaterial(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->getMaterial( link->visual->material_name.c_str() );
+              link->visual->material = this->getURDFMaterial( link->visual->material_name.c_str() );
             }
             else
             {
@@ -215,14 +196,14 @@ bool Parser::init(TiXmlElement *robot_xml)
   parent_link_tree.clear();
 
   // building tree: name mapping
-  if (!this->initTree(parent_link_tree))
+  if (!this->initURDFTree(parent_link_tree))
   {
     ROS_ERROR("failed to build tree");
     return false;
   }
 
   // find the root link
-  if (!this->initRoot(parent_link_tree))
+  if (!this->initURDFRoot(parent_link_tree))
   {
     ROS_ERROR("failed to find root link");
     return false;
@@ -231,7 +212,7 @@ bool Parser::init(TiXmlElement *robot_xml)
   return true;
 }
 
-bool Parser::initTree(std::map<std::string, std::string> &parent_link_tree)
+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++)
@@ -249,13 +230,13 @@ bool Parser::initTree(std::map<std::string, std::string> &parent_link_tree)
     {
       // find child and parent links
       boost::shared_ptr<Link> child_link, parent_link;
-      this->getLink(child_link_name, child_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->getLink(parent_link_name, parent_link);
+      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() );
@@ -286,7 +267,7 @@ bool Parser::initTree(std::map<std::string, std::string> &parent_link_tree)
 
 
 
-bool Parser::initRoot(std::map<std::string, std::string> &parent_link_tree)
+bool URDFParser::initURDFRoot(std::map<std::string, std::string> &parent_link_tree)
 {
 
   this->root_link_.reset();
@@ -300,7 +281,7 @@ bool Parser::initRoot(std::map<std::string, std::string> &parent_link_tree)
       // store root link
       if (!this->root_link_)
       {
-         getLink(l->first, this->root_link_);
+         getURDFLink(l->first, this->root_link_);
       }
       // we already found a root link
       else{
@@ -319,7 +300,7 @@ bool Parser::initRoot(std::map<std::string, std::string> &parent_link_tree)
   return true;
 }
 
-boost::shared_ptr<Material> Parser::getMaterial(const std::string& name) const
+boost::shared_ptr<Material> URDFParser::getURDFMaterial(const std::string& name) const
 {
   boost::shared_ptr<Material> ptr;
   if (this->materials_.find(name) == this->materials_.end())
@@ -329,25 +310,7 @@ boost::shared_ptr<Material> Parser::getMaterial(const std::string& name) const
   return ptr;
 }
 
-boost::shared_ptr<const Link> Parser::getLink(const std::string& name) const
+void URDFParser::getURDFLink(const std::string& name,boost::shared_ptr<Link> &link) const
-{
-  boost::shared_ptr<const Link> ptr;
-  if (this->links_.find(name) == this->links_.end())
-    ptr.reset();
-  else
-    ptr = this->links_.find(name)->second;
-  return ptr;
-}
-
-void Parser::getLinks(std::vector<boost::shared_ptr<Link> >& links) const
-{
-  for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
-  {
-    links.push_back(link->second);
-  }
-}
-
-void Parser::getLink(const std::string& name,boost::shared_ptr<Link> &link) const
 {
   boost::shared_ptr<Link> ptr;
   if (this->links_.find(name) == this->links_.end())
@@ -357,15 +320,5 @@ void Parser::getLink(const std::string& name,boost::shared_ptr<Link> &link) cons
   link = ptr;
 }
 
-boost::shared_ptr<const Joint> Parser::getJoint(const std::string& name) const
-{
-  boost::shared_ptr<const Joint> ptr;
-  if (this->joints_.find(name) == this->joints_.end())
-    ptr.reset();
-  else
-    ptr = this->joints_.find(name)->second;
-  return ptr;
-}
-
 }
 

urdf_parser/src/urdf_to_graphiz.cpp

@@ -34,7 +34,7 @@
 
 /* Author: Wim Meeussen */
 
-#include "urdf_parser/parser.h"
+#include "urdf_parser/urdf_parser.h"
 #include <iostream>
 #include <fstream>
 
@@ -90,16 +90,20 @@ int main(int argc, char** argv)
     return -1;
   }
 
-  TiXmlDocument robot_model_xml;
+  URDFParser robot;
-  robot_model_xml.LoadFile(argv[1]);
-  TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
-  if (!robot_xml){
-    std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
-    return -1;
-  }
 
-  Parser robot;
+  // get the entire file
-  if (!robot.init(robot_xml)){
+  std::string xml_string;
+  std::fstream xml_file(argv[1], std::fstream::in);
+  while ( xml_file.good() )
+  {
+    std::string line;
+    std::getline( xml_file, line);
+    xml_string += (line + "\n");
+  }
+  xml_file.close();
+
+  if (!robot.initURDF(xml_string)){
     std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
     return -1;
   }

urdf_parser/test/memtest.cpp

@@ -1,13 +1,24 @@
-#include <urdf_parser/parser.h>
+#include <urdf_parser/urdf_parser.h>
 #include <ros/ros.h>
+#include <fstream>
+#include <iostream>
 
 int main(int argc, char** argv){
   ros::init(argc, argv, "memtest");
   while (ros::ok()){
-    urdf::Parser urdf;
+    urdf::URDFParser urdf;
 
-    TiXmlDocument xml_doc;
+    std::string xml_string;
-    xml_doc.LoadFile(std::string(argv[1]));
+    std::fstream xml_file(argv[1], std::fstream::in);
-    urdf.init(&xml_doc);
+    while ( xml_file.good() )
+    {
+      std::string line;
+      std::getline( xml_file, line);
+      xml_string += (line + "\n");
+    }
+    xml_file.close();
+
+
+    urdf.initURDF(xml_string);
   }
 }