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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:43