removing unimplemented functions + fixing bugs

CMakeLists.txt

@@ -2,8 +2,8 @@ cmake_minimum_required( VERSION 2.8 FATAL_ERROR )
 project (urdfdom_headers)
 
 set (URDF_MAJOR_VERSION 0)
-set (URDF_MINOR_VERSION 1)
-set (URDF_PATCH_VERSION 4)
+set (URDF_MINOR_VERSION 2)
+set (URDF_PATCH_VERSION 0)
 
 set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION})
 
@@ -18,6 +18,7 @@ endif (MSVC OR MSVC90 OR MSVC10)
 add_subdirectory(urdf_sensor)
 add_subdirectory(urdf_model)
 add_subdirectory(urdf_model_state)
+add_subdirectory(urdf_exception)
 
 set(PACKAGE_NAME ${PROJECT_NAME})
 set(cmake_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}-config.cmake")

urdf_exception/CMakeLists.txt

@@ -0,0 +1 @@
+INSTALL(DIRECTORY include/urdf_exception DESTINATION include)

urdf_exception/include/urdf_exception/exception.h

@@ -0,0 +1,19 @@
+// URDF exceptions
+#ifndef URDF_INTERFACE_EXCEPTION_H_
+#define URDF_INTERFACE_EXCEPTION_H_
+
+#include <string>
+#include <stdexcept>
+
+namespace urdf
+{
+
+class ParseError: public std::runtime_error
+{
+public:
+  ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {};
+};
+
+}
+
+#endif

urdf_model/include/urdf_model/color.h

@@ -76,14 +76,13 @@ class Color
         }
         catch (boost::bad_lexical_cast &e)
         {
-          throw("color rgba element ("+pieces[i]+") is not a valid float");
+          return false;
         }
       }
     }
 
     if (rgba.size() != 4)
     {
-      //ROS_ERROR("Color contains %i elements instead of 4 elements", (int)rgba.size());
       return false;
     }
 

urdf_model/include/urdf_model/joint.h

@@ -41,9 +41,8 @@
 #include <vector>
 #include <boost/shared_ptr.hpp>
 
-#include "pose.h"
+#include "urdf_model/pose.h"
 
-class TiXmlElement;
 
 namespace urdf{
 
@@ -61,7 +60,6 @@ class JointDynamics
     damping = 0;
     friction = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 class JointLimits
@@ -80,7 +78,6 @@ class JointLimits
     effort = 0;
     velocity = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 /// \brief Parameters for Joint Safety Controllers
@@ -133,7 +130,6 @@ class JointSafety
     k_position = 0;
     k_velocity = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 
@@ -148,7 +144,6 @@ class JointCalibration
   {
     reference_position = 0;
   };
-  bool initXml(TiXmlElement* config);
 };
 
 class JointMimic
@@ -165,7 +160,6 @@ class JointMimic
     multiplier = 0.0;
     joint_name.clear();
   };
-  bool initXml(TiXmlElement* config);
 };
 
 
@@ -216,7 +210,6 @@ class Joint
   /// Option to Mimic another Joint
   boost::shared_ptr<JointMimic> mimic;
 
-  bool initXml(TiXmlElement* xml);
   void clear()
   {
     this->axis.clear();

urdf_model/include/urdf_model/link.h

@@ -46,17 +46,16 @@
 #include "joint.h"
 #include "color.h"
 
-class TiXmlElement;
-
 namespace urdf{
 
 class Geometry
 {
 public:
   enum {SPHERE, BOX, CYLINDER, MESH} type;
 
-  virtual void initXml(TiXmlElement *) = 0;
-
+  virtual ~Geometry(void)
+  {
+  }  
 };
 
 class Sphere : public Geometry
@@ -69,7 +68,6 @@ class Sphere : public Geometry
   {
     radius = 0;
   };
-  void initXml(TiXmlElement *);
 };
 
 class Box : public Geometry
@@ -82,7 +80,6 @@ class Box : public Geometry
   {
     this->dim.clear();
   };
-  void initXml(TiXmlElement *);
 };
 
 class Cylinder : public Geometry
@@ -97,7 +94,6 @@ class Cylinder : public Geometry
     length = 0;
     radius = 0;
   };
-  void initXml(TiXmlElement *);
 };
 
 class Mesh : public Geometry
@@ -115,8 +111,6 @@ class Mesh : public Geometry
     scale.y = 1;
     scale.z = 1;
   };
-  void initXml(TiXmlElement *);
-  bool fileExists(std::string filename);
 };
 
 class Material
@@ -133,7 +127,6 @@ class Material
     texture_filename.clear();
     name.clear();
   };
-  void initXml(TiXmlElement* config);
 };
 
 class Inertial
@@ -150,7 +143,6 @@ class Inertial
     mass = 0;
     ixx = ixy = ixz = iyy = iyz = izz = 0;
   };
-  void initXml(TiXmlElement* config);
 };
 
 class Visual
@@ -171,7 +163,6 @@ class Visual
     geometry.reset();
     this->group_name.clear();
   };
-  void initXml(TiXmlElement* config);
   std::string group_name;
 };
 
@@ -188,7 +179,6 @@ class Collision
     geometry.reset();
     this->group_name.clear();
   };
-  void initXml(TiXmlElement* config);
   std::string group_name;
 };
 
@@ -223,13 +213,12 @@ class Link
   std::vector<boost::shared_ptr<Joint> > child_joints;
   std::vector<boost::shared_ptr<Link> > child_links;
 
-  void initXml(TiXmlElement* config);
-
   boost::shared_ptr<Link> getParent() const
   {return parent_link_.lock();};
 
-  void setParent(boost::shared_ptr<Link> parent);
-
+  void setParent(const boost::shared_ptr<Link> &parent)
+  { parent_link_ = parent; }
+
   void clear()
   {
     this->name.clear();
@@ -241,14 +230,28 @@ class Link
     this->child_links.clear();
     this->collision_groups.clear();
   };
+
+  boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > getVisuals(const std::string& group_name) const
+  {
+    if (this->visual_groups.find(group_name) != this->visual_groups.end())
+      return this->visual_groups.at(group_name);
+    return boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > >();
+  }
+
+  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > getCollisions(const std::string& group_name) const
+  {
+    if (this->collision_groups.find(group_name) != this->collision_groups.end())
+      return this->collision_groups.at(group_name);
+    return boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > >();
+  }
+
+  /*
   void setParentJoint(boost::shared_ptr<Joint> child);
   void addChild(boost::shared_ptr<Link> child);
   void addChildJoint(boost::shared_ptr<Joint> child);
 
-  void addVisual(std::string group_name, boost::shared_ptr<Visual> visual);
-  boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > getVisuals(const std::string& group_name) const;
-  void addCollision(std::string group_name, boost::shared_ptr<Collision> collision);
-  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > getCollisions(const std::string& group_name) const;
+
+  */
 private:
   boost::weak_ptr<Link> parent_link_;
 

urdf_model/include/urdf_model/model.h

@@ -41,7 +41,7 @@
 #include <map>
 #include <boost/function.hpp>
 #include <urdf_model/link.h>
-
+#include <urdf_exception/exception.h>
 
 namespace urdf {
 
@@ -58,7 +58,7 @@ class ModelInterface
       ptr = this->links_.find(name)->second;
     return ptr;
   };
-
+  
   boost::shared_ptr<const Joint> getJoint(const std::string& name) const
   {
     boost::shared_ptr<const Joint> ptr;
@@ -68,8 +68,8 @@ class ModelInterface
       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
   {
@@ -78,7 +78,7 @@ class ModelInterface
       links.push_back(link->second);
     }
   };
-
+  
   void clear()
   {
     name_.clear();
@@ -87,7 +87,7 @@ class ModelInterface
     this->materials_.clear();
     this->root_link_.reset();
   };
-
+  
   /// non-const getLink()
   void getLink(const std::string& name,boost::shared_ptr<Link> &link) const
   {
@@ -98,7 +98,7 @@ class ModelInterface
       ptr = this->links_.find(name)->second;
     link = ptr;
   };
-
+  
   /// non-const getMaterial()
   boost::shared_ptr<Material> getMaterial(const std::string& name) const
   {
@@ -109,15 +109,81 @@ class ModelInterface
       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);
-
-  /// 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);
-
+
+  void 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;
+
+      if (parent_link_name.empty() || child_link_name.empty())
+      {
+        throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
+      }
+      else
+      {
+        // find child and parent links
+        boost::shared_ptr<Link> child_link, parent_link;
+        this->getLink(child_link_name, child_link);
+        if (!child_link)
+        {
+          throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
+        }
+        this->getLink(parent_link_name, parent_link);
+        if (!parent_link)
+        {
+          throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found.  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 [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
+        }
+
+        //set parent link for child link
+        child_link->setParent(parent_link);
+
+        //set parent joint for child link        
+        child_link->parent_joint = joint->second;
+
+        //set child joint for parent link
+        parent_link->child_joints.push_back(joint->second);
+
+        //set child link for parent link
+        parent_link->child_links.push_back(child_link);
+
+        // fill in child/parent string map
+        parent_link_tree[child_link->name] = parent_link_name;
+      }
+    }
+  }
+
+  void initRoot(const 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> >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)  
+    {
+      std::map<std::string, std::string >::const_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
+        {
+          throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
+        }
+      }
+    }
+    if (!this->root_link_)
+    {
+      throw ParseError("No root link found. The robot xml is not a valid tree.");
+    }
+  }
+
+
   /// \brief complete list of Links
   std::map<std::string, boost::shared_ptr<Link> > links_;
   /// \brief complete list of Joints