39 #include <fcl/BVH/BVH_model.h> 40 #include <fcl/shape/geometric_shapes.h> 41 #include <fcl/octree.h> 42 #include <boost/thread/mutex.hpp> 79 bool always_allow_collision =
false;
89 always_allow_collision =
true;
92 "collision_detection.fcl",
"Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. " 93 "No contacts are computed.",
100 ROS_DEBUG_NAMED(
"collision_detection.fcl",
"Collision between '%s' and '%s' is conditionally allowed",
110 if (tl.find(cd1->
getID()) != tl.end())
112 always_allow_collision =
true;
115 "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
122 if (tl.find(cd2->
getID()) != tl.end())
124 always_allow_collision =
true;
127 "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
135 always_allow_collision =
true;
139 if (always_allow_collision)
143 ROS_DEBUG_NAMED(
"collision_detection.fcl",
"Actually checking collisions between %s and %s", cd1->
getID().c_str(),
144 cd2->
getID().c_str());
147 std::size_t want_contact_count = 0;
154 std::pair<std::string, std::string> cp(cd1->
getID(), cd2->
getID());
159 std::pair<std::string, std::string> cp(cd2->
getID(), cd1->
getID());
162 if (have < cdata->req_->max_contacts_per_pair)
170 bool enable_cost = cdata->
req_->
cost;
172 bool enable_contact =
true;
173 fcl::CollisionResult col_result;
174 int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequest(std::numeric_limits<size_t>::max(), enable_contact,
175 num_max_cost_sources, enable_cost),
177 if (num_contacts > 0)
181 "Found %d contacts between '%s' and '%s'. " 182 "These contacts will be evaluated to check if they are accepted or not",
183 num_contacts, cd1->
getID().c_str(), cd2->
getID().c_str());
185 const std::pair<std::string, std::string>& pc = cd1->
getID() < cd2->
getID() ?
188 for (
int i = 0; i < num_contacts; ++i)
195 if (want_contact_count > 0)
197 --want_contact_count;
202 "Found unacceptable contact between '%s' and '%s'. Contact was stored.",
206 ROS_INFO_NAMED(
"collision_detection.fcl",
"Found unacceptable contact between '%s' (type '%s') and '%s' " 207 "(type '%s'). Contact was stored.",
211 if (want_contact_count == 0)
219 std::vector<fcl::CostSource> cost_sources;
220 col_result.getCostSources(cost_sources);
223 for (
auto& cost_source : cost_sources)
234 if (want_contact_count > 0)
237 bool enable_cost = cdata->
req_->
cost;
239 bool enable_contact =
true;
241 fcl::CollisionResult col_result;
242 int num_contacts = fcl::collide(
243 o1, o2, fcl::CollisionRequest(want_contact_count, enable_contact, num_max_cost_sources, enable_cost),
245 if (num_contacts > 0)
247 int num_contacts_initial = num_contacts;
250 if (want_contact_count >= (std::size_t)num_contacts)
251 want_contact_count -= num_contacts;
254 num_contacts = want_contact_count;
255 want_contact_count = 0;
259 ROS_INFO_NAMED(
"collision_detection.fcl",
"Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), " 260 "which constitute a collision. %d contacts will be stored",
264 const std::pair<std::string, std::string>& pc = cd1->
getID() < cd2->
getID() ?
268 for (
int i = 0; i < num_contacts; ++i)
279 std::vector<fcl::CostSource> cost_sources;
280 col_result.getCostSources(cost_sources);
283 for (
auto& cost_source : cost_sources)
294 bool enable_cost = cdata->
req_->
cost;
296 bool enable_contact =
false;
297 fcl::CollisionResult col_result;
299 fcl::collide(o1, o2, fcl::CollisionRequest(1, enable_contact, num_max_cost_sources, enable_cost), col_result);
300 if (num_contacts > 0)
304 ROS_INFO_NAMED(
"collision_detection.fcl",
"Found a contact between '%s' (type '%s') and '%s' (type '%s'), " 305 "which constitutes a collision. " 306 "Contact information is not stored.",
313 std::vector<fcl::CostSource> cost_sources;
314 col_result.getCostSources(cost_sources);
317 for (
auto& cost_source : cost_sources)
335 "Collision checking is considered complete (collision was found and %u contacts are stored)",
343 ROS_INFO_NAMED(
"collision_detection.fcl",
"Collision checking is considered complete due to external callback. " 344 "%s was found. %u contacts are stored.",
353 using ShapeKey = std::weak_ptr<const shapes::Shape>;
354 using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
368 for (
auto it =
map_.begin(); it !=
map_.end();)
372 if (it->first.expired())
390 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2,
void* data,
double& min_dist)
422 bool always_allow_collision =
false;
433 always_allow_collision =
true;
436 "Collision between '%s' and '%s' is always allowed. No distances are computed.",
446 if (tl.find(cd1->
getID()) != tl.end())
448 always_allow_collision =
true;
451 "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
460 if (tl.find(cd2->
getID()) != tl.end())
462 always_allow_collision =
true;
465 "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
471 if (always_allow_collision)
476 ROS_DEBUG_NAMED(
"collision_detection.fcl",
"Actually checking collisions between %s and %s", cd1->
getID().c_str(),
477 cd2->
getID().c_str());
479 fcl::DistanceResult fcl_result;
483 const std::pair<std::string, std::string>& pc = cd1->
getID() < cd2->
getID() ?
487 DistanceMap::iterator it = cdata->
res->
distances.find(pc);
505 dist_threshold = it->second[0].distance;
509 fcl_result.min_distance = dist_threshold;
515 if (d < dist_threshold)
517 dist_result.
distance = fcl_result.min_distance;
518 dist_result.
nearest_points[0] = Eigen::Vector3d(fcl_result.nearest_points[0].data.vs);
519 dist_result.
nearest_points[1] = Eigen::Vector3d(fcl_result.nearest_points[1].data.vs);
529 if (d <= 0 && cdata->req->enable_signed_distance)
533 dist_result.
normal.setZero();
535 fcl::CollisionRequest coll_req;
536 fcl::CollisionResult coll_res;
537 coll_req.enable_contact =
true;
538 coll_req.num_max_contacts = 200;
539 std::size_t contacts = fcl::collide(o1, o2, coll_req, coll_res);
544 for (std::size_t i = 0; i < contacts; ++i)
546 const fcl::Contact& contact = coll_res.getContact(i);
547 if (contact.penetration_depth > max_dist)
549 max_dist = contact.penetration_depth;
554 const fcl::Contact& contact = coll_res.getContact(max_index);
555 dist_result.
distance = -contact.penetration_depth;
556 dist_result.
nearest_points[0] = Eigen::Vector3d(contact.pos.data.vs);
557 dist_result.
nearest_points[1] = Eigen::Vector3d(contact.pos.data.vs);
558 dist_result.
normal = Eigen::Vector3d(contact.normal.data.vs);
576 std::vector<DistanceResultsData> data;
578 data.push_back(dist_result);
585 it->second.push_back(dist_result);
589 if (it->second[0].distance < dist_result.
distance)
590 it->second[0] = dist_result;
595 it->second.push_back(dist_result);
610 template <
typename BV,
typename T>
617 template <
typename T1,
typename T2>
626 template <
typename T>
635 template <
typename BV,
typename T>
638 using ShapeKey = std::weak_ptr<const shapes::Shape>;
639 using ShapeMap = std::map<ShapeKey, FCLGeometryConstPtr, std::owner_less<ShapeKey>>;
643 std::weak_ptr<const shapes::Shape> wptr(shape);
645 boost::mutex::scoped_lock slock(cache.
lock_);
646 ShapeMap::const_iterator cache_it = cache.
map_.find(wptr);
647 if (cache_it != cache.
map_.end())
649 if (cache_it->second->collision_geometry_data_->ptr.raw == data)
654 return cache_it->second;
656 else if (cache_it->second.unique())
658 const_cast<FCLGeometry*
>(cache_it->second.get())->updateCollisionGeometryData(data, shape_index,
false);
663 return cache_it->second;
674 FCLShapeCache& othercache = GetShapeCache<BV, World::Object>();
677 othercache.
lock_.lock();
678 auto cache_it = othercache.
map_.find(wptr);
679 if (cache_it != othercache.
map_.end())
681 if (cache_it->second.unique())
684 FCLGeometryConstPtr obj_cache = cache_it->second;
685 othercache.
map_.erase(cache_it);
686 othercache.
lock_.unlock();
689 const_cast<FCLGeometry*
>(obj_cache.get())->updateCollisionGeometryData(data, shape_index,
true);
697 boost::mutex::scoped_lock slock(cache.
lock_);
698 cache.
map_[wptr] = obj_cache;
703 othercache.
lock_.unlock();
712 FCLShapeCache& othercache = GetShapeCache<BV, robot_state::AttachedBody>();
715 othercache.
lock_.lock();
716 auto cache_it = othercache.
map_.find(wptr);
717 if (cache_it != othercache.
map_.end())
719 if (cache_it->second.unique())
722 FCLGeometryConstPtr obj_cache = cache_it->second;
723 othercache.
map_.erase(cache_it);
724 othercache.
lock_.unlock();
727 const_cast<FCLGeometry*
>(obj_cache.get())->updateCollisionGeometryData(data, shape_index,
true);
736 boost::mutex::scoped_lock slock(cache.
lock_);
737 cache.
map_[wptr] = obj_cache;
742 othercache.
lock_.unlock();
745 fcl::CollisionGeometry* cg_g =
nullptr;
754 cg_g =
new fcl::Plane(p->
a, p->
b, p->
c, p->
d);
768 cg_g =
new fcl::Sphere(s->
radius);
774 const double* size = s->
size;
775 cg_g =
new fcl::Box(size[0], size[1], size[2]);
792 auto g =
new fcl::BVHModel<BV>();
794 if (mesh->vertex_count > 0 && mesh->triangle_count > 0)
796 std::vector<fcl::Triangle> tri_indices(mesh->triangle_count);
797 for (
unsigned int i = 0; i < mesh->triangle_count; ++i)
799 fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
801 std::vector<fcl::Vec3f> points(mesh->vertex_count);
802 for (
unsigned int i = 0; i < mesh->vertex_count; ++i)
803 points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
806 g->addSubModel(points, tri_indices);
815 cg_g =
new fcl::OcTree(g->
octree);
819 ROS_ERROR_NAMED(
"collision_detection.fcl",
"This shape type (%d) is not supported using FCL yet",
826 cg_g->computeLocalAABB();
827 FCLGeometryConstPtr res(
new FCLGeometry(cg_g, data, shape_index));
828 boost::mutex::scoped_lock slock(cache.
lock_);
829 cache.
map_[wptr] = res;
833 return FCLGeometryConstPtr();
840 return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, link, shape_index);
846 return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, ab, shape_index);
851 return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, obj, 0);
854 template <
typename BV,
typename T>
856 const T* data,
int shape_index)
858 if (fabs(scale - 1.0) <= std::numeric_limits<double>::epsilon() &&
859 fabs(padding) <= std::numeric_limits<double>::epsilon())
860 return createCollisionGeometry<BV, T>(shape, data, shape_index);
864 scaled_shape->scaleAndPadd(scale, padding);
865 return createCollisionGeometry<BV, T>(scaled_shape, data, shape_index);
872 return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, scale, padding, link, shape_index);
878 return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, scale, padding, ab, shape_index);
884 return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, scale, padding, obj, 0);
889 FCLShapeCache& cache1 = GetShapeCache<fcl::OBBRSS, World::Object>();
891 boost::mutex::scoped_lock slock(cache1.
lock_);
894 FCLShapeCache& cache2 = GetShapeCache<fcl::OBBRSS, robot_state::AttachedBody>();
896 boost::mutex::scoped_lock slock(cache2.
lock_);
904 if (kmodel->hasJointModelGroup(req_->group_name))
905 active_components_only_ = &kmodel->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet();
907 active_components_only_ =
nullptr;
912 std::vector<fcl::CollisionObject*> collision_objects(collision_objects_.size());
913 for (std::size_t i = 0; i < collision_objects_.size(); ++i)
914 collision_objects[i] = collision_objects_[i].
get();
915 if (!collision_objects.empty())
916 manager->registerObjects(collision_objects);
921 for (
auto& collision_object : collision_objects_)
922 manager->unregisterObject(collision_object.get());
927 collision_objects_.clear();
928 collision_geometry_.clear();
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
BodyType body_types[2]
The object body type.
const CollisionRequest * req_
The collision request passed by the user.
CollisionResult * res_
The user specified response location.
#define ROS_INFO_NAMED(name,...)
bool done_
Flag indicating whether collision checking is complete.
FCLShapeCache & GetShapeCache()
std::map< ShapeKey, FCLGeometryConstPtr, std::owner_less< ShapeKey >> ShapeMap
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
union collision_detection::CollisionGeometryData::@0 ptr
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
bool collisionCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data)
A body attached to a robot link.
bool enable_nearest_points
Indicate if nearest point information should be calculated.
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Eigen::Vector3d nearest_points[2]
The nearest points.
bool getAllowedCollision(const std::string &name1, const std::string &name2, DecideContactFn &fn) const
Get the allowed collision predicate between two elements. Return true if a predicate for entry is inc...
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
std::shared_ptr< Shape > ShapePtr
Generic interface to collision detection.
unsigned int clean_count_
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
void unregisterFrom(fcl::BroadPhaseCollisionManager *manager)
std::size_t contact_count
Number of contacts returned.
bool collision
True if collision was found, false otherwise.
const std::set< const robot_model::LinkModel * > * active_components_only
The set of active components to check.
std::set< CostSource > cost_sources
When costs are computed, the individual cost sources are.
boost::function< bool(const CollisionResult &)> is_done
Function call that decides whether collision detection should stop.
std::weak_ptr< const shapes::Shape > ShapeKey
#define ROS_DEBUG_NAMED(name,...)
void fcl2contact(const fcl::Contact &fc, Contact &c)
std::shared_ptr< const octomap::OcTree > octree
The collision is allowed depending on a predicate evaluated on the produced contact. If the predicate returns true, this particular contact is deemed ok (or allowed), i.e., the contact does not imply that the two bodies are in collision.
std::size_t max_contacts
Overall maximum number of contacts to compute.
A representation of an object.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
void bumpUseCount(bool force=false)
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
void cleanCollisionGeometryCache()
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
std::string getTypeString() const
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
Find a limited(max_contacts_per_body) set of contacts for a given pair.
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
bool contacts
If true, compute contacts.
bool collision
Indicates if two objects were in collision.
const AllowedCollisionMatrix * acm_
The user specified collision matrix (may be NULL)
void fcl2costsource(const fcl::CostSource &fcs, CostSource &cs)
DistanceResult * res
Distance query results information.
const std::string & getID() const
std::string link_names[2]
The object link names.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) ...
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned...
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
#define ROS_ERROR_NAMED(name,...)
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
static const double max_dist
Find the global minimum for each pair.
A link from the robot. Contains the constant transform applied to the link and its geometry...
bool done
Indicates if distance query is finished.
When collision costs are computed, this structure contains information about the partial cost incurre...
bool distanceCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist)
double distance_threshold
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
void registerTo(fcl::BroadPhaseCollisionManager *manager)
double distance(const urdf::Pose &transform)
const robot_state::AttachedBody * ab
static const unsigned int MAX_CLEAN_COUNT
const DistanceRequest * req
Distance query request information.
bool verbose
Log debug information.
bool cost
If true, a collision cost is computed.
const std::set< const robot_model::LinkModel * > * active_components_only_
const robot_model::LinkModel * link
std::shared_ptr< const Shape > ShapeConstPtr