diff --git a/trunk/collision_space_fcl/include/collision_space_fcl/environment.h b/trunk/collision_space_fcl/include/collision_space_fcl/environment.h
index 2a812c9e10ea9dc892ed3e444b25a1a22d838430..5bf15885b40d9d2d50e5bce489b19c1366fe94c1 100644
--- a/trunk/collision_space_fcl/include/collision_space_fcl/environment.h
+++ b/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_;
};
}
diff --git a/trunk/collision_space_fcl/include/collision_space_fcl/environmentFCL.h b/trunk/collision_space_fcl/include/collision_space_fcl/environmentFCL.h
index 1a6b413bd4b3c612a1c3701ccab50ee6eed472e2..c772c034d1afb6ec8463422aa46f891076a5b9c0 100644
--- a/trunk/collision_space_fcl/include/collision_space_fcl/environmentFCL.h
+++ b/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
diff --git a/trunk/collision_space_fcl/include/collision_space_fcl/environment_objects.h b/trunk/collision_space_fcl/include/collision_space_fcl/environment_objects.h
index 7bc8e6814e8f49f99739778a77fb09d9ac44055f..dde9d7fea01b92a9df463f9c15d0ae7d28761e2f 100644
--- a/trunk/collision_space_fcl/include/collision_space_fcl/environment_objects.h
+++ b/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>
diff --git a/trunk/collision_space_fcl/src/environment.cpp b/trunk/collision_space_fcl/src/environment.cpp
index 23dd21262d8a6e5bcf67f40db9e39a5f401ec7bb..b7cd1f47aa35c12045d4346ee179c8fb878334e9 100644
--- a/trunk/collision_space_fcl/src/environment.cpp
+++ b/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_;
+}
diff --git a/trunk/collision_space_fcl/src/environmentFCL.cpp b/trunk/collision_space_fcl/src/environmentFCL.cpp
index e04c43bc52eea28e9247c17e70201792d1c91693..2d8e93abb4118fa37624fd9069f8064f3cf0dc63 100644
--- a/trunk/collision_space_fcl/src/environmentFCL.cpp
+++ b/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);
+ }
}
}
}
diff --git a/trunk/collision_space_fcl_test/test/test_collision_space_fcl.cpp b/trunk/collision_space_fcl_test/test/test_collision_space_fcl.cpp
index 95165cefccbfc96e16ff29a416dfc0ed8aa68b4a..83ce49d24724745177b86766bd7b2eeb577d93c0 100644
--- a/trunk/collision_space_fcl_test/test/test_collision_space_fcl.cpp
+++ b/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++) {
diff --git a/trunk/fcl/manifest.xml b/trunk/fcl/manifest.xml
index f71e1911c73533f67d2828b1e076454c90fea9c6..370fb5d3570138e9f5fd74029aec07b2a743f33f 100644
--- a/trunk/fcl/manifest.xml
+++ b/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>