00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/collision_detection_fcl/collision_common.h>
00038 #include <fcl/BVH/BVH_model.h>
00039 #include <fcl/shape/geometric_shapes.h>
00040 #include <fcl/octree.h>
00041 #include <boost/thread/mutex.hpp>
00042
00043 namespace collision_detection
00044 {
00045
00046 bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void *data)
00047 {
00048 CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
00049 if (cdata->done_)
00050 return true;
00051 const CollisionGeometryData *cd1 = static_cast<const CollisionGeometryData*>(o1->getCollisionGeometry()->getUserData());
00052 const CollisionGeometryData *cd2 = static_cast<const CollisionGeometryData*>(o2->getCollisionGeometry()->getUserData());
00053
00054
00055 if (cdata->active_components_only_)
00056 {
00057 const robot_model::LinkModel *l1 = cd1->type == BodyTypes::ROBOT_LINK ? cd1->ptr.link : (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : NULL);
00058 const robot_model::LinkModel *l2 = cd2->type == BodyTypes::ROBOT_LINK ? cd2->ptr.link : (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : NULL);
00059
00060
00061 if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
00062 (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
00063 return false;
00064 }
00065
00066
00067 DecideContactFn dcf;
00068 bool always_allow_collision = false;
00069 if (cdata->acm_)
00070 {
00071 AllowedCollision::Type type;
00072 bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
00073 if (found)
00074 {
00075
00076 if (type == AllowedCollision::ALWAYS)
00077 {
00078 always_allow_collision = true;
00079 if (cdata->req_->verbose)
00080 logDebug("Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. No contacts are computed.",
00081 cd1->getID().c_str(),
00082 cd1->getTypeString().c_str(),
00083 cd2->getID().c_str(),
00084 cd2->getTypeString().c_str());
00085 }
00086 else
00087 if (type == AllowedCollision::CONDITIONAL)
00088 {
00089 cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), dcf);
00090 if (cdata->req_->verbose)
00091 logDebug("Collision between '%s' and '%s' is conditionally allowed", cd1->getID().c_str(), cd2->getID().c_str());
00092 }
00093 }
00094 }
00095
00096
00097 if (cd1->type == BodyTypes::ROBOT_LINK && cd2->type == BodyTypes::ROBOT_ATTACHED)
00098 {
00099 const std::set<std::string> &tl = cd2->ptr.ab->getTouchLinks();
00100 if (tl.find(cd1->getID()) != tl.end())
00101 {
00102 always_allow_collision = true;
00103 if (cdata->req_->verbose)
00104 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00105 cd1->getID().c_str(), cd2->getID().c_str());
00106 }
00107 }
00108 else
00109 if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
00110 {
00111 const std::set<std::string> &tl = cd1->ptr.ab->getTouchLinks();
00112 if (tl.find(cd2->getID()) != tl.end())
00113 {
00114 always_allow_collision = true;
00115 if (cdata->req_->verbose)
00116 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00117 cd2->getID().c_str(), cd1->getID().c_str());
00118 }
00119 }
00120
00121 if (cd1->type == BodyTypes::ROBOT_ATTACHED && cd2->type == BodyTypes::ROBOT_ATTACHED)
00122 {
00123 if (cd1->ptr.ab->getAttachedLink() == cd2->ptr.ab->getAttachedLink())
00124 always_allow_collision = true;
00125 }
00126
00127
00128 if (always_allow_collision)
00129 return false;
00130
00131 if (cdata->req_->verbose)
00132 logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());
00133
00134
00135 std::size_t want_contact_count = 0;
00136 if (cdata->req_->contacts)
00137 if (cdata->res_->contact_count < cdata->req_->max_contacts)
00138 {
00139 std::size_t have;
00140 if (cd1->getID() < cd2->getID())
00141 {
00142 std::pair<std::string, std::string> cp(cd1->getID(), cd2->getID());
00143 have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
00144 }
00145 else
00146 {
00147 std::pair<std::string, std::string> cp(cd2->getID(), cd1->getID());
00148 have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
00149 }
00150 if (have < cdata->req_->max_contacts_per_pair)
00151 want_contact_count = std::min(cdata->req_->max_contacts_per_pair - have, cdata->req_->max_contacts - cdata->res_->contact_count);
00152 }
00153
00154 if (dcf)
00155 {
00156
00157 bool enable_cost = cdata->req_->cost;
00158 std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
00159 bool enable_contact = true;
00160 fcl::CollisionResult col_result;
00161 int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequest(std::numeric_limits<size_t>::max(), enable_contact, num_max_cost_sources, enable_cost), col_result);
00162 if (num_contacts > 0)
00163 {
00164 if (cdata->req_->verbose)
00165 logInform("Found %d contacts between '%s' and '%s'. These contacts will be evaluated to check if they are accepted or not",
00166 num_contacts, cd1->getID().c_str(), cd2->getID().c_str());
00167 Contact c;
00168 const std::pair<std::string, std::string> &pc = cd1->getID() < cd2->getID() ?
00169 std::make_pair(cd1->getID(), cd2->getID()) : std::make_pair(cd2->getID(), cd1->getID());
00170 for (int i = 0 ; i < num_contacts ; ++i)
00171 {
00172 fcl2contact(col_result.getContact(i), c);
00173
00174 if (dcf(c) == false)
00175 {
00176
00177 if (want_contact_count > 0)
00178 {
00179 --want_contact_count;
00180 cdata->res_->contacts[pc].push_back(c);
00181 cdata->res_->contact_count++;
00182 if (cdata->req_->verbose)
00183 logInform("Found unacceptable contact between '%s' and '%s'. Contact was stored.",
00184 cd1->getID().c_str(), cd2->getID().c_str());
00185 }
00186 else
00187 if (cdata->req_->verbose)
00188 logInform("Found unacceptable contact between '%s' (type '%s') and '%s' (type '%s'). Contact was stored.",
00189 cd1->getID().c_str(), cd1->getTypeString().c_str(),
00190 cd2->getID().c_str(), cd2->getTypeString().c_str());
00191 cdata->res_->collision = true;
00192 if (want_contact_count == 0)
00193 break;
00194 }
00195 }
00196 }
00197
00198 if (enable_cost)
00199 {
00200 std::vector<fcl::CostSource> cost_sources;
00201 col_result.getCostSources(cost_sources);
00202
00203 CostSource cs;
00204 for (std::size_t i = 0; i < cost_sources.size(); ++i)
00205 {
00206 fcl2costsource(cost_sources[i], cs);
00207 cdata->res_->cost_sources.insert(cs);
00208 while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
00209 cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
00210 }
00211 }
00212 }
00213 else
00214 {
00215 if (want_contact_count > 0)
00216 {
00217
00218 bool enable_cost = cdata->req_->cost;
00219 std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
00220 bool enable_contact = true;
00221
00222 fcl::CollisionResult col_result;
00223 int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequest(want_contact_count, enable_contact, num_max_cost_sources, enable_cost), col_result);
00224 if (num_contacts > 0)
00225 {
00226 int num_contacts_initial = num_contacts;
00227
00228
00229 if (want_contact_count >= (std::size_t)num_contacts)
00230 want_contact_count -= num_contacts;
00231 else
00232 {
00233 num_contacts = want_contact_count;
00234 want_contact_count = 0;
00235 }
00236
00237 if (cdata->req_->verbose)
00238 logInform("Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), which constitute a collision. %d contacts will be stored",
00239 num_contacts_initial,
00240 cd1->getID().c_str(), cd1->getTypeString().c_str(),
00241 cd2->getID().c_str(), cd2->getTypeString().c_str(),
00242 num_contacts);
00243
00244 const std::pair<std::string, std::string> &pc = cd1->getID() < cd2->getID() ?
00245 std::make_pair(cd1->getID(), cd2->getID()) : std::make_pair(cd2->getID(), cd1->getID());
00246 cdata->res_->collision = true;
00247 for (int i = 0 ; i < num_contacts ; ++i)
00248 {
00249 Contact c;
00250 fcl2contact(col_result.getContact(i), c);
00251 cdata->res_->contacts[pc].push_back(c);
00252 cdata->res_->contact_count++;
00253 }
00254 }
00255
00256 if (enable_cost)
00257 {
00258 std::vector<fcl::CostSource> cost_sources;
00259 col_result.getCostSources(cost_sources);
00260
00261 CostSource cs;
00262 for (std::size_t i = 0; i < cost_sources.size(); ++i)
00263 {
00264 fcl2costsource(cost_sources[i], cs);
00265 cdata->res_->cost_sources.insert(cs);
00266 while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
00267 cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
00268 }
00269 }
00270 }
00271 else
00272 {
00273 bool enable_cost = cdata->req_->cost;
00274 std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
00275 bool enable_contact = false;
00276 fcl::CollisionResult col_result;
00277 int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequest(1, enable_contact, num_max_cost_sources, enable_cost), col_result);
00278 if (num_contacts > 0)
00279 {
00280 cdata->res_->collision = true;
00281 if (cdata->req_->verbose)
00282 logInform("Found a contact between '%s' (type '%s') and '%s' (type '%s'), which constitutes a collision. Contact information is not stored.",
00283 cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str());
00284 }
00285
00286 if (enable_cost)
00287 {
00288 std::vector<fcl::CostSource> cost_sources;
00289 col_result.getCostSources(cost_sources);
00290
00291 CostSource cs;
00292 for (std::size_t i = 0; i < cost_sources.size(); ++i)
00293 {
00294 fcl2costsource(cost_sources[i], cs);
00295 cdata->res_->cost_sources.insert(cs);
00296 while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
00297 cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
00298 }
00299 }
00300 }
00301 }
00302
00303
00304 if (cdata->res_->collision)
00305 if (!cdata->req_->contacts || cdata->res_->contact_count >= cdata->req_->max_contacts)
00306 {
00307 if (!cdata->req_->cost)
00308 cdata->done_ = true;
00309 if (cdata->req_->verbose)
00310 logInform("Collision checking is considered complete (collision was found and %u contacts are stored)",
00311 (unsigned int)cdata->res_->contact_count);
00312 }
00313
00314 if (!cdata->done_ && cdata->req_->is_done)
00315 {
00316 cdata->done_ = cdata->req_->is_done(*cdata->res_);
00317 if (cdata->done_ && cdata->req_->verbose)
00318 logInform("Collision checking is considered complete due to external callback. %s was found. %u contacts are stored.",
00319 cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count);
00320 }
00321
00322 return cdata->done_;
00323 }
00324
00325 struct FCLShapeCache
00326 {
00327 FCLShapeCache() : clean_count_(0) {}
00328
00329 void bumpUseCount(bool force = false)
00330 {
00331 clean_count_++;
00332
00333
00334 if (clean_count_ > MAX_CLEAN_COUNT || force)
00335 {
00336 clean_count_ = 0;
00337 unsigned int from = map_.size();
00338 for (std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator it = map_.begin() ; it != map_.end() ; )
00339 {
00340 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator nit = it; ++nit;
00341 if (it->first.expired())
00342 map_.erase(it);
00343 it = nit;
00344 }
00345
00346 }
00347 }
00348
00349 static const unsigned int MAX_CLEAN_COUNT = 100;
00350 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr> map_;
00351 unsigned int clean_count_;
00352 boost::mutex lock_;
00353 };
00354
00355
00356 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& min_dist)
00357 {
00358 CollisionData* cdata = reinterpret_cast<CollisionData*>(data);
00359
00360 const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->getCollisionGeometry()->getUserData());
00361 const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->getCollisionGeometry()->getUserData());
00362
00363
00364 if (cdata->active_components_only_)
00365 {
00366 const robot_model::LinkModel *l1 = cd1->type == BodyTypes::ROBOT_LINK ? cd1->ptr.link : (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : NULL);
00367 const robot_model::LinkModel *l2 = cd2->type == BodyTypes::ROBOT_LINK ? cd2->ptr.link : (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : NULL);
00368
00369
00370 if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
00371 (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
00372 {
00373 min_dist = cdata->res_->distance;
00374 return cdata->done_;
00375 }
00376 }
00377
00378
00379 bool always_allow_collision = false;
00380 if (cdata->acm_)
00381 {
00382 AllowedCollision::Type type;
00383
00384 bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
00385 if (found)
00386 {
00387
00388 if (type == AllowedCollision::ALWAYS)
00389 {
00390 always_allow_collision = true;
00391 if (cdata->req_->verbose)
00392 logDebug("Collision between '%s' and '%s' is always allowed. No contacts are computed.",
00393 cd1->getID().c_str(), cd2->getID().c_str());
00394 }
00395 }
00396 }
00397
00398
00399 if (cd1->type == BodyTypes::ROBOT_LINK && cd2->type == BodyTypes::ROBOT_ATTACHED)
00400 {
00401 const std::set<std::string> &tl = cd2->ptr.ab->getTouchLinks();
00402 if (tl.find(cd1->getID()) != tl.end())
00403 {
00404 always_allow_collision = true;
00405 if (cdata->req_->verbose)
00406 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00407 cd1->getID().c_str(), cd2->getID().c_str());
00408 }
00409 }
00410 else
00411 {
00412 if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
00413 {
00414 const std::set<std::string> &tl = cd1->ptr.ab->getTouchLinks();
00415 if (tl.find(cd2->getID()) != tl.end())
00416 {
00417 always_allow_collision = true;
00418 if (cdata->req_->verbose)
00419 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00420 cd2->getID().c_str(), cd1->getID().c_str());
00421 }
00422 }
00423 }
00424
00425 if(always_allow_collision)
00426 {
00427 min_dist = cdata->res_->distance;
00428 return cdata->done_;
00429 }
00430
00431 if (cdata->req_->verbose)
00432 logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());
00433
00434 fcl::DistanceResult dist_result;
00435 dist_result.update(cdata->res_->distance, NULL, NULL, fcl::DistanceResult::NONE, fcl::DistanceResult::NONE);
00436 double d = fcl::distance(o1, o2, fcl::DistanceRequest(), dist_result);
00437
00438 if(d < 0)
00439 {
00440 cdata->done_ = true;
00441 cdata->res_->distance = -1;
00442 }
00443 else
00444 {
00445 if(cdata->res_->distance > d)
00446 cdata->res_->distance = d;
00447 }
00448
00449 min_dist = cdata->res_->distance;
00450
00451 return cdata->done_;
00452 }
00453
00454
00455 template<typename BV, typename T>
00456 FCLShapeCache& GetShapeCache()
00457 {
00458 static FCLShapeCache cache;
00459 return cache;
00460 }
00461
00462 template<typename T1, typename T2>
00463 struct IfSameType
00464 {
00465 enum { value = 0 };
00466 };
00467
00468 template<typename T>
00469 struct IfSameType<T, T>
00470 {
00471 enum { value = 1 };
00472 };
00473
00474 template<typename BV, typename T>
00475 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const T *data)
00476 {
00477 FCLShapeCache &cache = GetShapeCache<BV, T>();
00478
00479 boost::weak_ptr<const shapes::Shape> wptr(shape);
00480 {
00481 boost::mutex::scoped_lock slock(cache.lock_);
00482 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::const_iterator cache_it = cache.map_.find(wptr);
00483 if (cache_it != cache.map_.end())
00484 {
00485 if (cache_it->second->collision_geometry_data_->ptr.raw == (void*)data)
00486 {
00487
00488 return cache_it->second;
00489 }
00490 else
00491 if (cache_it->second.unique())
00492 {
00493 const_cast<FCLGeometry*>(cache_it->second.get())->updateCollisionGeometryData(data, false);
00494
00495 return cache_it->second;
00496 }
00497 }
00498 }
00499
00500
00501
00502
00503 if (IfSameType<T, robot_state::AttachedBody>::value == 1)
00504 {
00505
00506 FCLShapeCache &othercache = GetShapeCache<BV, World::Object>();
00507
00508
00509 othercache.lock_.lock();
00510 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator cache_it = othercache.map_.find(wptr);
00511 if (cache_it != othercache.map_.end())
00512 {
00513 if (cache_it->second.unique())
00514 {
00515
00516 FCLGeometryConstPtr obj_cache = cache_it->second;
00517 othercache.map_.erase(cache_it);
00518 othercache.lock_.unlock();
00519
00520
00521 const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, true);
00522
00523
00524
00525
00526 boost::mutex::scoped_lock slock(cache.lock_);
00527 cache.map_[wptr] = obj_cache;
00528 cache.bumpUseCount();
00529 return obj_cache;
00530 }
00531 }
00532 othercache.lock_.unlock();
00533 }
00534 else
00535
00536
00537
00538 if (IfSameType<T, World::Object>::value == 1)
00539 {
00540
00541 FCLShapeCache &othercache = GetShapeCache<BV, robot_state::AttachedBody>();
00542
00543
00544 othercache.lock_.lock();
00545 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator cache_it = othercache.map_.find(wptr);
00546 if (cache_it != othercache.map_.end())
00547 {
00548 if (cache_it->second.unique())
00549 {
00550
00551 FCLGeometryConstPtr obj_cache = cache_it->second;
00552 othercache.map_.erase(cache_it);
00553 othercache.lock_.unlock();
00554
00555
00556 const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, true);
00557
00558
00559
00560
00561
00562 boost::mutex::scoped_lock slock(cache.lock_);
00563 cache.map_[wptr] = obj_cache;
00564 cache.bumpUseCount();
00565 return obj_cache;
00566 }
00567 }
00568 othercache.lock_.unlock();
00569 }
00570
00571 fcl::CollisionGeometry* cg_g = NULL;
00572 if (shape->type == shapes::PLANE)
00573 {
00574
00575 switch (shape->type)
00576 {
00577 case shapes::PLANE:
00578 {
00579 const shapes::Plane* p = static_cast<const shapes::Plane*>(shape.get());
00580 cg_g = new fcl::Plane(p->a, p->b, p->c, p->d);
00581 }
00582 break;
00583 default:
00584 break;
00585 }
00586 }
00587 else
00588 {
00589 switch (shape->type)
00590 {
00591 case shapes::SPHERE:
00592 {
00593 const shapes::Sphere* s = static_cast<const shapes::Sphere*>(shape.get());
00594 cg_g = new fcl::Sphere(s->radius);
00595 }
00596 break;
00597 case shapes::BOX:
00598 {
00599 const shapes::Box* s = static_cast<const shapes::Box*>(shape.get());
00600 const double* size = s->size;
00601 cg_g = new fcl::Box(size[0], size[1], size[2]);
00602 }
00603 break;
00604 case shapes::CYLINDER:
00605 {
00606 const shapes::Cylinder* s = static_cast<const shapes::Cylinder*>(shape.get());
00607 cg_g = new fcl::Cylinder(s->radius, s->length);
00608 }
00609 break;
00610 case shapes::CONE:
00611 {
00612 const shapes::Cone* s = static_cast<const shapes::Cone*>(shape.get());
00613 cg_g = new fcl::Cone(s->radius, s->length);
00614 }
00615 break;
00616 case shapes::MESH:
00617 {
00618 fcl::BVHModel<BV>* g = new fcl::BVHModel<BV>();
00619 const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape.get());
00620 if (mesh->vertex_count > 0 && mesh->triangle_count > 0)
00621 {
00622 std::vector<fcl::Triangle> tri_indices(mesh->triangle_count);
00623 for(unsigned int i = 0; i < mesh->triangle_count; ++i)
00624 tri_indices[i] = fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
00625
00626 std::vector<fcl::Vec3f> points(mesh->vertex_count);
00627 for (unsigned int i = 0; i < mesh->vertex_count; ++i)
00628 points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
00629
00630 g->beginModel();
00631 g->addSubModel(points, tri_indices);
00632 g->endModel();
00633 }
00634 cg_g = g;
00635 }
00636 break;
00637 case shapes::OCTREE:
00638 {
00639 const shapes::OcTree* g = static_cast<const shapes::OcTree*>(shape.get());
00640 cg_g = new fcl::OcTree(g->octree);
00641 }
00642 break;
00643 default:
00644 logError("This shape type (%d) is not supported using FCL yet", (int)shape->type);
00645 cg_g = NULL;
00646 }
00647 }
00648 if (cg_g)
00649 {
00650 cg_g->computeLocalAABB();
00651 FCLGeometryConstPtr res(new FCLGeometry(cg_g, data));
00652 boost::mutex::scoped_lock slock(cache.lock_);
00653 cache.map_[wptr] = res;
00654 cache.bumpUseCount();
00655 return res;
00656 }
00657 return FCLGeometryConstPtr();
00658 }
00659
00660
00662 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape,
00663 const robot_model::LinkModel *link)
00664 {
00665 return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, link);
00666 }
00667
00668 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape,
00669 const robot_state::AttachedBody *ab)
00670 {
00671 return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, ab);
00672 }
00673
00674 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape,
00675 const World::Object *obj)
00676 {
00677 return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, obj);
00678 }
00679
00680 template<typename BV, typename T>
00681 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, double scale, double padding, const T *data)
00682 {
00683 if (fabs(scale - 1.0) <= std::numeric_limits<double>::epsilon() && fabs(padding) <= std::numeric_limits<double>::epsilon())
00684 return createCollisionGeometry<BV, T>(shape, data);
00685 else
00686 {
00687 boost::shared_ptr<shapes::Shape> scaled_shape(shape->clone());
00688 scaled_shape->scaleAndPadd(scale, padding);
00689 return createCollisionGeometry<BV, T>(scaled_shape, data);
00690 }
00691 }
00692
00693 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, double scale, double padding,
00694 const robot_model::LinkModel *link)
00695 {
00696 return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, scale, padding, link);
00697 }
00698
00699 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, double scale, double padding,
00700 const robot_state::AttachedBody *ab)
00701 {
00702 return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, scale, padding, ab);
00703 }
00704
00705 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, double scale, double padding,
00706 const World::Object *obj)
00707 {
00708 return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, scale, padding, obj);
00709 }
00710
00711 void cleanCollisionGeometryCache()
00712 {
00713 FCLShapeCache &cache1 = GetShapeCache<fcl::OBBRSS, World::Object>();
00714 {
00715 boost::mutex::scoped_lock slock(cache1.lock_);
00716 cache1.bumpUseCount(true);
00717 }
00718 FCLShapeCache &cache2 = GetShapeCache<fcl::OBBRSS, robot_state::AttachedBody>();
00719 {
00720 boost::mutex::scoped_lock slock(cache2.lock_);
00721 cache2.bumpUseCount(true);
00722 }
00723 }
00724
00725 }
00726
00727 void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr &kmodel)
00728 {
00729 if (kmodel->hasJointModelGroup(req_->group_name))
00730 active_components_only_ = &kmodel->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsWithGeometrySet();
00731 else
00732 active_components_only_ = NULL;
00733 }
00734
00735 void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager *manager)
00736 {
00737 std::vector<fcl::CollisionObject*> collision_objects(collision_objects_.size());
00738 for(std::size_t i = 0; i < collision_objects_.size(); ++i)
00739 collision_objects[i] = collision_objects_[i].get();
00740 if (collision_objects.size() > 0)
00741 manager->registerObjects(collision_objects);
00742 }
00743
00744 void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManager *manager)
00745 {
00746 for (std::size_t i = 0 ; i < collision_objects_.size() ; ++i)
00747 manager->unregisterObject(collision_objects_[i].get());
00748 }
00749
00750 void collision_detection::FCLObject::clear()
00751 {
00752 collision_objects_.clear();
00753 collision_geometry_.clear();
00754 }