be consistent with e-turtle collision space interface

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@14 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/collision_space_fcl/include/collision_space_fcl/environment.h

@@ -34,8 +34,8 @@
 
 /** \author Ioan Sucan */
 
-#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_
-#define COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_
+#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_H
+#define COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_H
 
 #include "collision_space_fcl/environment_objects.h"
 #include <planning_models/kinematic_model.h>
@@ -92,14 +92,16 @@ public:
   {
     /// the bound where the contact is allowed 
     boost::shared_ptr<bodies::Body> bound;
-	    
-    /// the set of link names that are allowed to make contact
-    std::vector<std::string> links;
+    
+    std::string body_name_1;
+    std::string body_name_2;
 
     /// tha maximum depth for the contact
     double depth;
   };
 
+  typedef std::map<std::string, std::map<std::string, std::vector<AllowedContact> > > AllowedContactMap;
+
   /** \brief Definition of a structure for the allowed collision matrix */
   /* False means that no collisions are allowed, true means ok */
   class AllowedCollisionMatrix
@@ -169,6 +171,8 @@ public:
     const entry_type& getEntriesBimap() const {
       return allowed_entries_bimap_;
     }
+
+    void print(std::ostream& out) const;
     
   private:
 
@@ -236,14 +240,12 @@ public:
   /** \brief Check whether the model is in collision with the environment.  Self collisions are not checked */
   virtual bool isEnvironmentCollision(void) const = 0;
 	
-  /** \brief Get the list of contacts (collisions). The maximum number of contacts to be returned can be specified. If the value is 0, all found contacts are returned. */
-  virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1) const = 0;
+  /** \brief Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that are in collision*/
+  virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const = 0;
 
   /** \brief This function will get the complete list of contacts between any two potentially colliding bodies.  The num per contacts specifies the number of contacts per pair that will be returned */
-  virtual bool getAllCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const = 0;
+  virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const = 0;
 
-  bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) const;
-	
   /**********************************************************************/
   /* Collision Bodies                                                   */
   /**********************************************************************/
@@ -293,6 +295,18 @@ public:
   /** \brief reverts to using default settings for allowed collisions */  
   virtual void revertAlteredCollisionMatrix();
 
+  /** \brief sets the allowed contacts that will be used in collision checking */
+  virtual void setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts);
+
+  /** \brief gets the allowed contacts that will be used in collision checking */
+  const std::vector<AllowedContact>& getAllowedContacts() const;
+  
+  /** \brief clear out all allowed contacts */
+  void clearAllowedContacts() {
+    allowed_contact_map_.clear();
+    allowed_contacts_.clear();
+  }
+
   /**********************************************************************/
   /* Miscellaneous Routines                                             */
   /**********************************************************************/
@@ -341,6 +355,9 @@ protected:
   std::map<std::string, double> altered_link_padding_map_;
 
   bool use_altered_link_padding_map_;
+
+  AllowedContactMap allowed_contact_map_;
+  std::vector<AllowedContact> allowed_contacts_;
 	
 };
 }

trunk/collision_space_fcl/include/collision_space_fcl/environmentFCL.h

@@ -41,7 +41,6 @@
 #include "fcl/broad_phase_collision.h"
 #include <map>
 #include <vector>
-#include <set>
 
 namespace collision_space_fcl
 {
@@ -59,11 +58,11 @@ public:
   virtual ~EnvironmentModelFCL(void);
 
 
-  /** \brief Get the list of contacts (collisions) */
-  virtual bool getCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int max_count = 1) const;
+  /** \brief Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that's in collision*/
+  virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const;
 
   /** \brief This function will get the complete list of contacts between any two potentially colliding bodies.  The num per contacts specifies the number of contacts per pair that will be returned */
-  virtual bool getAllCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int num_per_contact = 1) const;
+  virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const;
 
   /** \brief Check if a model is in collision */
   virtual bool isCollision(void) const;
@@ -185,7 +184,8 @@ protected:
     {
       done = false;
       collides = false;
-      max_contacts = 1;
+      max_contacts_total = 0;
+      max_contacts_pair = 0;
       contacts = NULL;
       allowed_collision_matrix = NULL;
       allowed = NULL;
@@ -193,10 +193,11 @@ protected:
     }
 
     //these are parameters
-    unsigned int max_contacts;
+    unsigned int max_contacts_total;
+    unsigned int max_contacts_pair;
     const AllowedCollisionMatrix* allowed_collision_matrix;
     const std::map<fcl::CollisionObject*, std::pair<std::string, BodyType> >* geom_lookup_map;
-    const std::vector<AllowedContact>* allowed;
+    const AllowedContactMap *allowed;
     bool exhaustive;
 
     //these are for return info

trunk/collision_space_fcl/include/collision_space_fcl/environment_objects.h

@@ -34,8 +34,8 @@
 
 /** \author Ioan Sucan, E. Gil Jones */
 
-#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_
-#define COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_
+#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_H
+#define COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_H
 
 #include <vector>
 #include <string>

trunk/collision_space_fcl/src/environment.cpp

@@ -36,6 +36,7 @@
 
 #include "collision_space_fcl/environment.h"
 #include <ros/console.h>
+#include <iomanip>
 
 collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names,
                                                                                   bool allowed) 
@@ -94,7 +95,7 @@ collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionM
   allowed_entries_bimap_ = acm.allowed_entries_bimap_;
 }
 
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(const std::string& name1,
+bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, 
                                                                                     const std::string& name2,
                                                                                     bool& allowed_collision) const
 {
@@ -245,7 +246,7 @@ bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(
   allowed_entries_[j][i] = allowed;
   return true;
 }
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
+bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name, 
                                                                             bool allowed)
 {
   if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end()) {
@@ -259,7 +260,7 @@ bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(
   return true;
 }
 
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
+bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name, 
                                                                             const std::vector<std::string>& change_names,
                                                                             bool allowed)
 {
@@ -306,10 +307,15 @@ bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(
   return ok;
 }
 
-bool collision_space_fcl::EnvironmentModel::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count) const
-{
-  std::vector<AllowedContact> allowed;
-  return getCollisionContacts(allowed, contacts, max_count);
+void collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::print(std::ostream& out) const {
+  for(entry_type::right_const_iterator it = allowed_entries_bimap_.right.begin(); it != allowed_entries_bimap_.right.end(); it++) {
+    out << std::setw(40) << it->second;
+    out << " | ";
+    for(entry_type::right_const_iterator it2 = allowed_entries_bimap_.right.begin(); it2 != allowed_entries_bimap_.right.end(); it2++) {
+      out << std::setw(3) << allowed_entries_[it->first][it2->first];
+    }
+    out << std::endl;
+  }
 }
 
 bool collision_space_fcl::EnvironmentModel::getVerbose(void) const
@@ -342,7 +348,7 @@ double collision_space_fcl::EnvironmentModel::getRobotPadding(void) const
   return default_robot_padding_;
 }
 	
-void collision_space_fcl::EnvironmentModel::setRobotModel(const planning_models::KinematicModel* model,
+void collision_space_fcl::EnvironmentModel::setRobotModel(const planning_models::KinematicModel* model, 
                                                       const AllowedCollisionMatrix& acm,
                                                       const std::map<std::string, double>& link_padding_map,
                                                       double default_padding,
@@ -365,13 +371,13 @@ void collision_space_fcl::EnvironmentModel::unlock(void) const
   lock_.unlock();
 }
 
-const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix&
+const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix& 
 collision_space_fcl::EnvironmentModel::getDefaultAllowedCollisionMatrix() const
 {
   return default_collision_matrix_;
 }
 
-const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix&
+const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix& 
 collision_space_fcl::EnvironmentModel::getCurrentAllowedCollisionMatrix() const {
   if(use_altered_collision_matrix_) {
     return altered_collision_matrix_;
@@ -432,3 +438,17 @@ double collision_space_fcl::EnvironmentModel::getCurrentLinkPadding(std::string
   }
   return 0.0;
 }
+
+void collision_space_fcl::EnvironmentModel::setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts)
+{
+  allowed_contact_map_.clear();
+  allowed_contacts_ = allowed_contacts;
+  for(unsigned int i = 0; i < allowed_contacts.size(); i++) {
+    allowed_contact_map_[allowed_contacts_[i].body_name_1][allowed_contacts_[i].body_name_2].push_back(allowed_contacts_[i]);
+    allowed_contact_map_[allowed_contacts_[i].body_name_2][allowed_contacts_[i].body_name_1].push_back(allowed_contacts_[i]);
+  }
+}
+
+const std::vector<collision_space_fcl::EnvironmentModel::AllowedContact>& collision_space_fcl::EnvironmentModel::getAllowedContacts() const {
+  return allowed_contacts_;
+}

trunk/collision_space_fcl/src/environmentFCL.cpp

@@ -351,14 +351,26 @@ void EnvironmentModelFCL::addAttachedBody(LinkGeom* lg, const planning_models::K
   {
     ROS_WARN_STREAM("Must already have an entry in allowed collision matrix for " << attm->getName());
   } 
+  else
+  {
+      ROS_DEBUG_STREAM("Adding entry for " << attm->getName());
+  }
+
   attached_bodies_in_collision_matrix_[attm->getName()] = true;
   default_collision_matrix_.getEntryIndex(attm->getName(), attg->index);
   //setting touch links
   for(unsigned int i = 0; i < attm->getTouchLinks().size(); i++)
   {
-    if(!default_collision_matrix_.changeEntry(attm->getName(), attm->getTouchLinks()[i], true))
+    if(default_collision_matrix_.hasEntry(attm->getTouchLinks()[i]))
     {
-      ROS_WARN_STREAM("No entry in allowed collision matrix for " << attm->getName() << " and " << attm->getTouchLinks()[i]);
+      if(!default_collision_matrix_.changeEntry(attm->getName(), attm->getTouchLinks()[i], true))
+      {
+        ROS_WARN_STREAM("No entry in allowed collision matrix for " << attm->getName() << " and " << attm->getTouchLinks()[i]);
+      }
+      else
+      {
+        ROS_DEBUG_STREAM("Adding touch link for " << attm->getName() << " and " << attm->getTouchLinks()[i]);
+      }
     }
   }
   for(unsigned int i = 0; i < attm->getShapes().size(); i++)
@@ -556,45 +568,55 @@ void EnvironmentModelFCL::revertAlteredLinkPadding()
 } 
 
 
-bool EnvironmentModelFCL::getCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int max_count) const
+bool EnvironmentModelFCL::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total, unsigned int max_per_pair) const
 {
   contacts.clear();
   CollisionData cdata;
   cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
   cdata.geom_lookup_map = &geom_lookup_map_;
   cdata.contacts = &contacts;
-  cdata.max_contacts = max_count;
-  if (!allowedContacts.empty())
-    cdata.allowed = &allowedContacts;
+  cdata.max_contacts_total = max_total;
+  cdata.max_contacts_pair = max_per_pair;
+  if (!allowed_contacts_.empty())
+    cdata.allowed = &allowed_contact_map_;
   contacts.clear();
   testCollision(&cdata);
   return cdata.collides;
 }
 
 
-bool EnvironmentModelFCL::getAllCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int num_contacts_per_pair) const
+bool EnvironmentModelFCL::getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_contacts_per_pair) const
 {
   contacts.clear();
   CollisionData cdata;
   cdata.geom_lookup_map = &geom_lookup_map_;
   cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
   cdata.contacts = &contacts;
-  cdata.max_contacts = num_contacts_per_pair;
-  if (!allowedContacts.empty())
-    cdata.allowed = &allowedContacts;
+  cdata.max_contacts_total = UINT_MAX;
+  cdata.max_contacts_pair = num_contacts_per_pair;
   cdata.exhaustive = true;
+  if (!allowed_contacts_.empty())
+    cdata.allowed = &allowed_contact_map_;
   contacts.clear();
   testCollision(&cdata);
   return cdata.collides;
 }
 
-
-
 bool EnvironmentModelFCL::isCollision(void) const
 {
   CollisionData cdata;
   cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
   cdata.geom_lookup_map = &geom_lookup_map_;
+  if(!allowed_contacts_.empty())
+  {
+    cdata.allowed = &allowed_contact_map_;
+    ROS_DEBUG_STREAM("Got contacts size " << cdata.allowed->size());
+  }
+  else
+  {
+    ROS_DEBUG_STREAM("No allowed contacts");
+  }
+
   testCollision(&cdata);
   return cdata.collides;
 }
@@ -605,6 +627,8 @@ bool EnvironmentModelFCL::isSelfCollision(void) const
   CollisionData cdata; 
   cdata.geom_lookup_map = &geom_lookup_map_;
   cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
+  if (!allowed_contacts_.empty())
+    cdata.allowed = &allowed_contact_map_;
   testSelfCollision(&cdata);
   return cdata.collides;
 }
@@ -615,6 +639,8 @@ bool EnvironmentModelFCL::isEnvironmentCollision(void) const
   CollisionData cdata; 
   cdata.geom_lookup_map = &geom_lookup_map_;
   cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
+  if (!allowed_contacts_.empty())
+    cdata.allowed = &allowed_contact_map_;
   testEnvironmentCollision(&cdata);
   return cdata.collides;
 }
@@ -629,6 +655,8 @@ bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
   // first figure out what check is happening
   bool check_in_allowed_collision_matrix = true;
 
+  std::string object_name;
+
   std::map<fcl::CollisionObject*, std::pair<std::string, EnvironmentModelFCL::BodyType> >::const_iterator it1 = cdata->geom_lookup_map->find(o1);
   std::map<fcl::CollisionObject*, std::pair<std::string, EnvironmentModelFCL::BodyType> >::const_iterator it2 = cdata->geom_lookup_map->find(o2);
 
@@ -639,6 +667,7 @@ bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
   }
   else
   {
+    //ROS_WARN_STREAM("Object to be added does not have entry in geometry lookup map");
     cdata->body_name_1 = "";
     cdata->body_type_1 = EnvironmentModelFCL::OBJECT;
     check_in_allowed_collision_matrix = false;
@@ -651,6 +680,7 @@ bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
   }
   else
   {
+    //ROS_WARN_STREAM("Object to be added does not have entry in geometry lookup map");
     cdata->body_name_2 = "";
     cdata->body_type_2 = EnvironmentModelFCL::OBJECT;
     check_in_allowed_collision_matrix = false;
@@ -668,18 +698,19 @@ bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
     }
     if(allowed)
     {
-      ROS_DEBUG_STREAM("Collision but allowed touch between " << cdata->body_name_1 << " and " << cdata->body_name_2);
+      ROS_DEBUG_STREAM("Will not test for collision between " << cdata->body_name_1 << " and " << cdata->body_name_2);
       return cdata->done;
     }
     else
     {
-      ROS_DEBUG_STREAM("Collision and no allowed touch between " << cdata->body_name_1 << " and " << cdata->body_name_2);
+      ROS_DEBUG_STREAM("Will test for collision between " << cdata->body_name_1 << " and " << cdata->body_name_2);
     }
+
   }
 
   // do the actual collision check to get the desired number of contacts
 
-  if(!cdata->contacts)
+  if(!cdata->contacts && !cdata->allowed)
   {
     bool enable_contact = false;
     bool exhaustive = false;
@@ -694,15 +725,16 @@ bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
   }
   else
   {
+    unsigned int num_not_allowed = 0;
     // here is quite subtle: the exhaustive is high level one in collision space
     // if exhaustive, then each pair will report at most cdata->max_contacts contacts
     // else, then we report at most cdata->max_contacts in whole.
     // so both cases correspond to the exhaustive = false in low level FCL collision.
     int num_max_contacts;
     if(cdata->exhaustive)
-      num_max_contacts = cdata->max_contacts;
+      num_max_contacts = cdata->max_contacts_pair;
     else
-      num_max_contacts = cdata->max_contacts - cdata->contacts->size();
+      num_max_contacts = cdata->max_contacts_total - cdata->contacts->size();
 
 
     bool enable_contact = true;
@@ -712,7 +744,7 @@ bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
     for(int i = 0; i < num_contacts; ++i)
     {
       //already enough contacts, so just quit
-      if(!cdata->exhaustive && cdata->max_contacts > 0 && cdata->contacts->size() >= cdata->max_contacts)
+      if(!cdata->exhaustive && cdata->max_contacts_total > 0 && cdata->contacts->size() >= cdata->max_contacts_total)
       {
         break;
       }
@@ -722,65 +754,86 @@ bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void*
       fcl::BVH_REAL depth = contacts[i].penetration_depth;
 
       btVector3 pos(contact_p[0], contact_p[1], contact_p[2]);
+
       //figure out whether the contact is allowed
       //allowed contacts only allowed with objects for now
-
-      if(cdata->allowed && (cdata->body_type_1 == EnvironmentModelFCL::OBJECT || cdata->body_type_2 == EnvironmentModelFCL::OBJECT))
+      bool allowed = false;
+      if(cdata->allowed)
       {
-        std::string body_name;
-        if(cdata->body_type_1 != EnvironmentModelFCL::OBJECT)
+        EnvironmentModel::AllowedContactMap::const_iterator it1 = cdata->allowed->find(cdata->body_name_1);
+        if(it1 != cdata->allowed->end())
         {
-          body_name = cdata->body_name_1;
-        }
-        else
-        {
-          body_name = cdata->body_name_2;
-        }
-
-        bool allow = false;
-        for(unsigned int j = 0; !allow && j < cdata->allowed->size(); ++j)
-        {
-          if(cdata->allowed->at(j).bound->containsPoint(pos) && cdata->allowed->at(j).depth > fabs(depth))
+          std::map<std::string, std::vector<EnvironmentModel::AllowedContact> >::const_iterator it2 = it1->second.find(cdata->body_name_2);
+          if(it2 != it1->second.end())
           {
-            for(unsigned int k = 0; k < cdata->allowed->at(j).links.size(); ++k)
+            ROS_DEBUG_STREAM("Testing allowed contact for " << cdata->body_name_1 << " and " << cdata->body_name_2 << " num " << i);
+            ROS_DEBUG_STREAM("Contact at " << pos[0] << " " << pos[1] << " " << pos[2]);
+
+            const std::vector<EnvironmentModel::AllowedContact>& av = it2->second;
+            for(unsigned int j = 0; j < av.size(); j++)
             {
-              if(cdata->allowed->at(j).links[k] == body_name)
+              if(av[j].bound->containsPoint(pos))
               {
-                allow = true;
-                break;
+                if(av[j].depth >= fabs(depth))
+                {
+                  allowed = true;
+                  ROS_DEBUG_STREAM("Contact allowed by allowed collision region");
+                  break;
+                }
+                else
+                {
+                  ROS_DEBUG_STREAM("Depth check failing " << av[j].depth << " detected " << depth);
+                }
               }
             }
           }
         }
-
-        if(allow) continue;
       }
 
-      cdata->collides = true;
-
-      EnvironmentModelFCL::Contact add;
-      add.pos = pos;
-      add.normal = btVector3(normal[0], normal[1], normal[2]);
-      add.depth = depth;
-      add.body_name_1 = cdata->body_name_1;
-      add.body_name_2 = cdata->body_name_2;
-      add.body_type_1 = cdata->body_type_1;
-      add.body_type_2 = cdata->body_type_2;
+      if(!allowed)
+      {
+        cdata->collides = true;
+        num_not_allowed++;
 
-      cdata->contacts->push_back(add);
+        if(cdata->contacts != NULL)
+        {
+          if(num_not_allowed <= cdata->max_contacts_pair)
+          {
+            EnvironmentModelFCL::Contact add;
+            add.pos = pos;
+            add.normal = btVector3(normal[0], normal[1], normal[2]);
+            add.depth = depth;
+
+            add.body_name_1 = cdata->body_name_1;
+            add.body_name_2 = cdata->body_name_2;
+            add.body_type_1 = cdata->body_type_1;
+            add.body_type_2 = cdata->body_type_2;
+
+            cdata->contacts->push_back(add);
+            if(!cdata->exhaustive && cdata->contacts->size() >= cdata->max_contacts_total)
+              cdata->done = true;
+          }
+        }
+      }
+      else
+      {
+        cdata->done = true;
+      }
     }
-
-    if (!cdata->exhaustive && cdata->collides && cdata->contacts->size() >= cdata->max_contacts)
-      cdata->done = true;
   }
 
-
   return cdata->done;
 }
 
 
 void EnvironmentModelFCL::testObjectCollision(CollisionNamespace *cn, CollisionData *cdata) const
 { 
+  if(cn->env_geom_manager->empty())
+  {
+    ROS_WARN_STREAM("Problem - collide2 required for body collision for " << cn->name);
+    return;
+  }
+
   cn->env_geom_manager->setup();
   for(int i = model_geom_.link_geom.size() - 1; i >= 0; --i)
   {
@@ -799,6 +852,7 @@ void EnvironmentModelFCL::testObjectCollision(CollisionNamespace *cn, CollisionD
     //have to test collisions with link
     if(!allowed)
     {
+      ROS_DEBUG_STREAM("Will test for collision between object " << cn->name << " and link " << lg->link->getName());
       for(unsigned int j = 0; j < lg->padded_geom.size(); j++)
       {
         //have to figure
@@ -836,6 +890,11 @@ void EnvironmentModelFCL::testObjectCollision(CollisionNamespace *cn, CollisionD
         }
       }
     }
+    else
+    {
+      ROS_DEBUG_STREAM("Will not test for allowed collision between object " << cn->name << " and link " << lg->link->getName());
+    }
+
     //now we need to do the attached bodies
     for(unsigned int j = 0; j < lg->att_bodies.size(); j++)
     {
@@ -851,6 +910,7 @@ void EnvironmentModelFCL::testObjectCollision(CollisionNamespace *cn, CollisionD
       }
       if(!allowed)
       {
+        ROS_DEBUG_STREAM("Will test for collision between object " << cn->name << " and attached object " << att_name);
         for(unsigned int k = 0; k < lg->att_bodies[j]->padded_geom.size(); k++)
         {
           //have to figure
@@ -887,7 +947,11 @@ void EnvironmentModelFCL::testObjectCollision(CollisionNamespace *cn, CollisionD
             return;
           }
         }
-      } 
+      }
+      else
+      {
+        ROS_DEBUG_STREAM("Will not test for allowed collision between object " << cn->name << " and attached object " << att_name);
+      }
     }
   }
 }

trunk/collision_space_fcl_test/test/test_collision_space_fcl.cpp

@@ -337,10 +337,9 @@ TEST_F(TestCollisionSpaceFCL, TestACM) {
     ASSERT_TRUE(coll_space_->isCollision());
 
     //now we get the full set of collisions in the default state
-    std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
     std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
       
-    coll_space_->getAllCollisionContacts(ac, contacts, 1);
+    coll_space_->getAllCollisionContacts(contacts, 1);
 
     ASSERT_TRUE(contacts.size() > 1);
     //now we change all these pairwise to true
@@ -377,10 +376,9 @@ TEST_F(TestCollisionSpaceFCL, TestAttachedObjects)
     coll_space_->updateRobotModel(&state);
 
     //now we get the full set of collisions in the default state
-    std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
     std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
       
-    coll_space_->getAllCollisionContacts(ac, contacts, 1);
+    coll_space_->getAllCollisionContacts(contacts, 1);
 
     //now we change all these pairwise to true
     for(unsigned int i = 0; i < contacts.size(); i++) {
@@ -568,10 +566,9 @@ TEST_F(TestCollisionSpaceFCL, TestStaticObjects)
   ASSERT_TRUE(coll_space_->isEnvironmentCollision());
 
   //now we get the full set of collisions in the default state
-  std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
   std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
   
-  coll_space_->getAllCollisionContacts(ac, contacts, 1);
+  coll_space_->getAllCollisionContacts(contacts, 1);
 
   //now we change all these pairwise to true
   for(unsigned int i = 0; i < contacts.size(); i++) {
@@ -625,7 +622,7 @@ TEST_F(TestCollisionSpaceFCL, TestStaticObjects)
   EXPECT_FALSE(coll_space_->isEnvironmentCollision());
   contacts.clear();
 
-  coll_space_->getAllCollisionContacts(ac, contacts, 1);
+  coll_space_->getAllCollisionContacts(contacts, 1);
 
   //now we change all these pairwise to true
   for(unsigned int i = 0; i < contacts.size(); i++) {

trunk/fcl/manifest.xml

@@ -8,8 +8,8 @@
   <license>BSD</license>
   <review status="unreviewed" notes=""/>
   <url>http://ros.org/wiki/fcl</url>
-  <depend package="ann"/>
   <depend package="libccd"/>
+  <depend package="ann"/>
   <!-- <depend package="PQP"/> -->
   <!-- <depend package="svm_light"/> -->
   <export>