mapped_ring_buffer.hpp

Go to the documentation of this file.

// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP__MAPPED_RING_BUFFER_HPP_
#define RCLCPP__MAPPED_RING_BUFFER_HPP_

#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <utility>
#include <vector>

#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"

namespace rclcpp
{
namespace mapped_ring_buffer
{

class RCLCPP_PUBLIC MappedRingBufferBase
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase)
};


template<typename T, typename Alloc = std::allocator<void>>
class MappedRingBuffer : public MappedRingBufferBase
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>)
  using ElemAllocTraits = allocator::AllocRebind<T, Alloc>;
  using ElemAlloc = typename ElemAllocTraits::allocator_type;
  using ElemDeleter = allocator::Deleter<ElemAlloc, T>;

  using ConstElemSharedPtr = std::shared_ptr<const T>;
  using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;


  explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
  : elements_(size), head_(0)
  {
    if (size == 0) {
      throw std::invalid_argument("size must be a positive, non-zero value");
    }
    if (!allocator) {
      allocator_ = std::make_shared<ElemAlloc>();
    } else {
      allocator_ = std::make_shared<ElemAlloc>(*allocator.get());
    }
  }

  virtual ~MappedRingBuffer() {}


  void
  get(uint64_t key, ElemUniquePtr & value)
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    auto it = get_iterator_of_key(key);
    value = nullptr;
    if (it != elements_.end() && it->in_use) {
      if (it->unique_value) {
        ElemDeleter deleter = it->unique_value.get_deleter();
        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->unique_value);
        value = ElemUniquePtr(ptr, deleter);
      } else if (it->shared_value) {
        ElemDeleter * deleter = std::get_deleter<ElemDeleter, const T>(it->shared_value);
        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value);
        if (deleter) {
          value = ElemUniquePtr(ptr, *deleter);
        } else {
          value = ElemUniquePtr(ptr);
        }
      } else {
        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
      }
    }
  }


  void
  get(uint64_t key, ConstElemSharedPtr & value)
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    auto it = get_iterator_of_key(key);
    value.reset();
    if (it != elements_.end() && it->in_use) {
      if (!it->shared_value) {
        // The stored unique_ptr is upgraded to a shared_ptr here.
        // All the remaining get and pop calls done with unique_ptr
        // signature will receive a copy.
        if (!it->unique_value) {
          throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
        }
        it->shared_value = std::move(it->unique_value);
      }
      value = it->shared_value;
    }
  }


  void
  pop(uint64_t key, ElemUniquePtr & value)
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    auto it = get_iterator_of_key(key);
    value = nullptr;
    if (it != elements_.end() && it->in_use) {
      if (it->unique_value) {
        value = std::move(it->unique_value);
      } else if (it->shared_value) {
        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value);
        auto deleter = std::get_deleter<ElemDeleter, const T>(it->shared_value);
        if (deleter) {
          value = ElemUniquePtr(ptr, *deleter);
        } else {
          value = ElemUniquePtr(ptr);
        }
        it->shared_value.reset();
      } else {
        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
      }
      it->in_use = false;
    }
  }


  void
  pop(uint64_t key, ConstElemSharedPtr & value)
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    auto it = get_iterator_of_key(key);
    if (it != elements_.end() && it->in_use) {
      if (it->shared_value) {
        value = std::move(it->shared_value);
      } else if (it->unique_value) {
        value = std::move(it->unique_value);
      } else {
        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
      }
      it->in_use = false;
    }
  }


  bool
  push_and_replace(uint64_t key, ConstElemSharedPtr value)
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    bool did_replace = elements_[head_].in_use;
    Element & element = elements_[head_];
    element.key = key;
    element.unique_value.reset();
    element.shared_value.reset();
    element.shared_value = value;
    element.in_use = true;
    head_ = (head_ + 1) % elements_.size();
    return did_replace;
  }


  bool
  push_and_replace(uint64_t key, ElemUniquePtr value)
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    bool did_replace = elements_[head_].in_use;
    Element & element = elements_[head_];
    element.key = key;
    element.unique_value.reset();
    element.shared_value.reset();
    element.unique_value = std::move(value);
    element.in_use = true;
    head_ = (head_ + 1) % elements_.size();
    return did_replace;
  }

  bool
  has_key(uint64_t key)
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    return elements_.end() != get_iterator_of_key(key);
  }

private:
  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>)

  struct Element
  {
    uint64_t key;
    ElemUniquePtr unique_value;
    ConstElemSharedPtr shared_value;
    bool in_use;
  };

  using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<Element>;

  typename std::vector<Element, VectorAlloc>::iterator
  get_iterator_of_key(uint64_t key)
  {
    auto it = std::find_if(
      elements_.begin(), elements_.end(),
      [key](Element & e) -> bool {
        return e.key == key && e.in_use;
      });
    return it;
  }

  std::vector<Element, VectorAlloc> elements_;
  size_t head_;
  std::shared_ptr<ElemAlloc> allocator_;
  std::mutex data_mutex_;
};

}  // namespace mapped_ring_buffer
}  // namespace rclcpp

#endif  // RCLCPP__MAPPED_RING_BUFFER_HPP_