From d138bd73e04b7a7e3e707e3508e17b7059996da1 Mon Sep 17 00:00:00 2001
From: hsu <devnull@localhost>
Date: Fri, 17 Dec 2010 23:39:50 +0000
Subject: [PATCH] new multiple collision support with group tags per ticket
 #4932

---
 urdf/include/urdf/link.h |  7 +++++
 urdf/src/link.cpp        | 61 ++++++++++++++++++++++++++++++++++++----
 2 files changed, 63 insertions(+), 5 deletions(-)

diff --git a/urdf/include/urdf/link.h b/urdf/include/urdf/link.h
index 0990407..af973e2 100644
--- a/urdf/include/urdf/link.h
+++ b/urdf/include/urdf/link.h
@@ -180,8 +180,10 @@ public:
   {
     origin.clear();
     geometry.reset();
+    this->group_name.clear();
   };
   bool initXml(TiXmlElement* config);
+  std::string group_name;
 };
 
 
@@ -201,6 +203,9 @@ public:
   /// collision element
   boost::shared_ptr<Collision> collision;
 
+  /// a collection of collision elements, keyed by a string tag called "group"
+  std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<Collision> > > > collision_groups;
+
   /// Parent Joint element
   ///   explicitly stating "parent" because we want directional-ness for tree structure
   ///   every link can have one parent
@@ -225,11 +230,13 @@ public:
     this->parent_joint.reset();
     this->child_joints.clear();
     this->child_links.clear();
+    this->collision_groups.clear();
   };
   void setParentJoint(boost::shared_ptr<Joint> child);
   void addChild(boost::shared_ptr<Link> child);
   void addChildJoint(boost::shared_ptr<Joint> child);
 
+  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > getCollision(const std::string& grouip_name) const;
 private:
   boost::weak_ptr<Link> parent_link_;
 
diff --git a/urdf/src/link.cpp b/urdf/src/link.cpp
index ec44af3..367fe1b 100644
--- a/urdf/src/link.cpp
+++ b/urdf/src/link.cpp
@@ -297,6 +297,14 @@ bool Collision::initXml(TiXmlElement* config)
     return false;
   }
 
+  // Group Tag (optional)
+  // collision blocks without a group tag are designated to the "default" group
+  const char *group_name_char = config->Attribute("group");
+  if (!group_name_char)
+    group_name = std::string("default");
+  else
+    group_name = std::string(group_name_char);
+
   return true;
 }
 
@@ -443,21 +451,64 @@ bool Link::initXml(TiXmlElement* config)
     }
   }
 
-  // Collision (optional)
-  TiXmlElement *col = config->FirstChildElement("collision");
-  if (col)
+  // Multiple Collisions (optional)
+  for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
   {
-    collision.reset(new Collision);
-    if (!collision->initXml(col))
+    boost::shared_ptr<Collision> col;
+    col.reset(new Collision);
+
+    if (col->initXml(col_xml))
+    {
+      boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols = this->getCollision(col->group_name);
+      if (!cols)
+      {
+        // group does not exist, create one and add to map
+        cols.reset(new std::vector<boost::shared_ptr<Collision > >);
+        // new group name, create vector, add vector to map and add Collision to the vector
+        this->collision_groups.insert(make_pair(col->group_name,cols));
+        ROS_DEBUG("successfully added a new collision group name '%s'",col->group_name.c_str());
+      }
+
+      // group exists, add Collision to the vector in the map
+      cols->push_back(col);
+      ROS_DEBUG("successfully added a new collision under group name '%s'",col->group_name.c_str());
+    }
+    else
     {
       ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str());
+      col.reset();
       return false;
     }
   }
 
+  // Collision (optional)
+  // Assign one single default collision pointer from the collision_groups map
+  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > default_collision = this->getCollision("default");
+  if (!default_collision)
+  {
+    ROS_DEBUG("No 'default' collision group for Link '%s'", this->name.c_str());
+  }
+  else if (default_collision->empty())
+  {
+    ROS_DEBUG("'default' collision group is empty for Link '%s'", this->name.c_str());
+  }
+  else
+    this->collision = (*default_collision->begin());
+
   return true;
 }
 
+boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > Link::getCollision(const std::string& group_name) const
+{
+  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > ptr;
+  if (this->collision_groups.find(group_name) == this->collision_groups.end())
+    ptr.reset();
+  else
+    ptr = this->collision_groups.find(group_name)->second;
+  return ptr;
+}
+
+
 void Link::setParent(boost::shared_ptr<Link> parent)
 {
   this->parent_link_ = parent;