environment_cache.cpp

Go to the documentation of this file.

 
#include <tesseract_environment/environment_cache.h>
#include <tesseract_environment/environment.h>
#include <tesseract_kinematics/core/joint_group.h>
#include <tesseract_kinematics/core/kinematic_group.h>
 
#include <mutex>
 
namespace tesseract_environment
{
DefaultEnvironmentCache::DefaultEnvironmentCache(std::shared_ptr<const Environment> env, std::size_t cache_size)
  : env_(std::move(env)), cache_size_(cache_size)
{
}
 
void DefaultEnvironmentCache::setCacheSize(long size)
{
  std::unique_lock<std::shared_mutex> lock(cache_mutex_);
  cache_size_ = static_cast<std::size_t>(size);
}
 
long DefaultEnvironmentCache::getCacheSize() const { return static_cast<long>(cache_size_); }
 
void DefaultEnvironmentCache::refreshCache() const
{
  std::unique_lock<std::shared_mutex> lock(cache_mutex_);
  refreshCacheHelper();
}
 
std::unique_ptr<Environment> DefaultEnvironmentCache::getCachedEnvironment() const
{
  tesseract_scene_graph::SceneState current_state = env_->getState();
 
  std::unique_lock<std::shared_mutex> lock(cache_mutex_);
  refreshCacheHelper();  // This is to make sure the cached items are updated if needed
  assert(!cache_.empty());
  std::unique_ptr<Environment> t = std::move(cache_.back());
  // Update to the current joint values
  t->setState(current_state.joints);
 
  cache_.pop_back();
 
  return t;
}
 
void DefaultEnvironmentCache::refreshCacheHelper() const
{
  std::unique_ptr<Environment> env;
  auto lock_read = env_->lockRead();
  int rev = env_->getRevision();
  if (rev != cache_env_revision_ || cache_.empty())
  {
    env = env_->clone();
    cache_env_revision_ = rev;
  }
 
  if (env != nullptr)
  {
    cache_.clear();
    for (std::size_t i = 0; i < cache_size_; ++i)
      cache_.push_back(env->clone());
  }
  else if (cache_.size() <= 2)
  {
    for (std::size_t i = (cache_.size() - 1); i < cache_size_; ++i)
      cache_.push_back(cache_.front()->clone());
  }
}
 
}  // namespace tesseract_environment