fcl_collision_geometry_cache.cpp

Go to the documentation of this file.

 
#include <tesseract_collision/fcl/fcl_collision_geometry_cache.h>
#include <tesseract_geometry/geometry.h>
#include <mutex>
 
namespace tesseract_collision::tesseract_collision_fcl
{
// Static member definitions
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
std::map<boost::uuids::uuid, std::weak_ptr<fcl::CollisionGeometryd>> FCLCollisionGeometryCache::cache_;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
std::mutex FCLCollisionGeometryCache::mutex_;
 
void FCLCollisionGeometryCache::insert(const std::shared_ptr<const tesseract_geometry::Geometry>& key,
                                       const std::shared_ptr<fcl::CollisionGeometryd>& value)
{
  assert(!key->getUUID().is_nil());
  std::scoped_lock lock(mutex_);
  cache_[key->getUUID()] = value;
}
 
std::shared_ptr<fcl::CollisionGeometryd>
FCLCollisionGeometryCache::get(const std::shared_ptr<const tesseract_geometry::Geometry>& key)
{
  assert(!key->getUUID().is_nil());
  std::scoped_lock lock(mutex_);
  auto it = cache_.find(key->getUUID());
  if (it != cache_.end())
  {
    std::shared_ptr<fcl::CollisionGeometryd> collision_shape = it->second.lock();
    if (collision_shape != nullptr)
      return collision_shape;
 
    cache_.erase(key->getUUID());
  }
  return nullptr;
}
 
void FCLCollisionGeometryCache::prune()
{
  std::scoped_lock lock(mutex_);
  for (auto it = cache_.begin(); it != cache_.end();)
  {
    if (it->second.expired())
      it = cache_.erase(it);
    else
      ++it;
  }
}
 
}  // namespace tesseract_collision::tesseract_collision_fcl