collision_common.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Jia Pan */
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   // If active components are specified
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     // If neither of the involved components is active
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   // use the collision matrix (if any) to avoid certain collision checks
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       // if we have an entry in the collision matrix, we read it
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   // check if a link is touching an attached object
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   // bodies attached to the same link should not collide
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   // if collisions are always allowed, we are done
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   // see if we need to compute a contact
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     // if we have a decider for allowed contacts, we need to look at all the contacts
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         // if the contact is  not allowed, we have a collision
00174         if (dcf(c) == false)
00175         {
00176           // store the contact, if it is needed
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       // otherwise, we need to compute more things
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         // make sure we don't get more contacts than we want
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     // clean-up for cache (we don't want to keep infinitely large number of weak ptrs stored)
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       //      logDebug("Cleaning up cache for FCL objects that correspond to static shapes. Cache size reduced from %u to %u", from, (unsigned int)map_.size());
00346     }
00347   }
00348 
00349   static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is executed (this is only removal of expired entries)
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   // If active components are specified
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     // If neither of the involved components is active
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   // use the collision matrix (if any) to avoid certain distance checks
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       // if we have an entry in the collision matrix, we read it
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   // check if a link is touching an attached object
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); // can be faster
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 /* We template the function so we get a different cache for each of the template arguments combinations */
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         //        logDebug("Collision data structures for object %s retrieved from cache.", cache_it->second->collision_geometry_data_->getID().c_str());
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           //          logDebug("Collision data structures for object %s retrieved from cache after updating the source object.", cache_it->second->collision_geometry_data_->getID().c_str());
00495           return cache_it->second;
00496         }
00497     }
00498   }
00499 
00500   // attached objects could have previously been World::Object; we try to move them
00501   // from their old cache to the new one, if possible. the code is not pretty, but should help
00502   // when we attach/detach objects that are in the world
00503   if (IfSameType<T, robot_state::AttachedBody>::value == 1)
00504   {
00505     // get the cache that corresponds to objects; maybe this attached object used to be a world object
00506     FCLShapeCache &othercache = GetShapeCache<BV, World::Object>();
00507 
00508     // attached bodies could be just moved from the environment.
00509     othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
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         // remove from old cache
00516         FCLGeometryConstPtr obj_cache = cache_it->second;
00517         othercache.map_.erase(cache_it);
00518         othercache.lock_.unlock();
00519 
00520         // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
00521         const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, true);
00522 
00523         //        logDebug("Collision data structures for attached body %s retrieved from the cache for world objects.", obj_cache->collision_geometry_data_->getID().c_str());
00524 
00525         // add to the new cache
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     // world objects could have previously been attached objects; we try to move them
00536     // from their old cache to the new one, if possible. the code is not pretty, but should help
00537     // when we attach/detach objects that are in the world
00538     if (IfSameType<T, World::Object>::value == 1)
00539     {
00540       // get the cache that corresponds to objects; maybe this attached object used to be a world object
00541       FCLShapeCache &othercache = GetShapeCache<BV, robot_state::AttachedBody>();
00542 
00543       // attached bodies could be just moved from the environment.
00544       othercache.lock_.lock(); // lock manually to avoid having 2 simultaneous locks active (avoids possible deadlock)
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           // remove from old cache
00551           FCLGeometryConstPtr obj_cache = cache_it->second;
00552           othercache.map_.erase(cache_it);
00553           othercache.lock_.unlock();
00554 
00555           // update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
00556           const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, true);
00557 
00558           //          logDebug("Collision data structures for world object %s retrieved from the cache for attached bodies.",
00559           //                   obj_cache->collision_geometry_data_->getID().c_str());
00560 
00561           // add to the new cache
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) // shapes that directly produce CollisionGeometry
00573   {
00574     // handle cases individually
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:46