added the mapped ring buffer class for intra-pc

rclcpp/CMakeLists.txt

@@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(rclcpp)
 
 find_package(ament_cmake REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 
 ament_export_dependencies(rmw)
 ament_export_dependencies(rcl_interfaces)
@@ -12,6 +13,14 @@ ament_export_include_directories(include)
 if(AMENT_ENABLE_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  if(NOT WIN32)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
+  endif()
+
+  include_directories(include)
+
+  ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
 endif()
 
 ament_package(

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -0,0 +1,205 @@
+// 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_RCLCPP_RING_BUFFER_HPP_
+#define RCLCPP_RCLCPP_RING_BUFFER_HPP_
+
+#include <rclcpp/macros.hpp>
+
+#include <algorithm>
+#include <cstddef>
+#include <cstdint>
+#include <memory>
+#include <vector>
+
+namespace rclcpp
+{
+namespace mapped_ring_buffer
+{
+
+class MappedRingBufferBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase);
+};
+
+/// Ring buffer container of unique_ptr's of T, which can be accessed by a key.
+/* T must be a CopyConstructable and CopyAssignable.
+ * This class can be used in a container by using the base class MappedRingBufferBase.
+ * This class must have a positive, non-zero size.
+ * This class cannot be resized nor can it reserve additional space after construction.
+ * This class is not CopyConstructable nor CopyAssignable.
+ *
+ * The key's are not guaranteed to be unique because push_and_replace does not
+ * check for colliding keys.
+ * It is up to the user to only use unique keys.
+ * A side effect of this is that when get_copy_at_key or pop_at_key are called,
+ * they return the first encountered instance of the key.
+ * But iteration does not begin with the ring buffer's head, and therefore
+ * there is no guarantee on which value is returned if a key is used multiple
+ * times.
+ */
+template<typename T>
+class MappedRingBuffer : public MappedRingBufferBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T>);
+
+  /// Constructor.
+  /* The constructor will allocate memory while reserving space.
+   *
+   * /param size size of the ring buffer; must be positive and non-zero.
+   */
+  MappedRingBuffer(size_t size) : elements_(size), head_(0)
+  {
+    if (size == 0) {
+      throw std::invalid_argument("size must be a positive, non-zero value");
+    }
+  }
+  virtual ~MappedRingBuffer() {}
+
+  /// Return a copy of the value stored in the ring buffer at the given key.
+  /* The key is matched if an element in the ring buffer has a matching key.
+   * This method will allocate in order to return a copy.
+   *
+   * The key is not guaranteed to be unique, see the class docs for more.
+   *
+   * /param key the key associated with the stored value
+   * /param value if the key is found, the value is stored in this parameter
+   */
+  void
+  get_copy_at_key(uint64_t key, std::unique_ptr<T> & value)
+  {
+    auto it = get_iterator_of_key(key);
+    value = nullptr;
+    if (it != elements_.end() && it->in_use) {
+      value = std::unique_ptr<T>(new T(*it->value));
+    }
+  }
+
+  /// Return ownership of the value stored in the ring buffer, leaving a copy.
+  /* The key is matched if an element in the ring bufer has a matching key.
+   * This method will allocate in order to store a copy.
+   *
+   * The key is not guaranteed to be unique, see the class docs for more.
+   *
+   * The ownership of the currently stored object is returned, but a copy is
+   * made and stored in its place.
+   * This means that multiple calls to this function for a particular element
+   * will result in returning the copied and stored object not the original.
+   * This also means that later calls to pop_at_key will not return the
+   * originally stored object, since it was returned by the first call to this
+   * method.
+   *
+   * /param key the key associated with the stored value
+   * /param value if the key is found, the value is stored in this parameter
+   */
+  void
+  get_ownership_at_key(uint64_t key, std::unique_ptr<T> & value)
+  {
+    auto it = get_iterator_of_key(key);
+    value = nullptr;
+    if (it != elements_.end() && it->in_use) {
+      // Make a copy.
+      auto copy = std::unique_ptr<T>(new T(*it->value));
+      // Return the original.
+      value.swap(it->value);
+      // Store the copy.
+      it->value.swap(copy);
+    }
+  }
+
+  /// Return ownership of the value stored in the ring buffer at the given key.
+  /* The key is matched if an element in the ring buffer has a matching key.
+   *
+   * The key is not guaranteed to be unique, see the class docs for more.
+   *
+   * /param key the key associated with the stored value
+   * /param value if the key is found, the value is stored in this parameter
+   */
+  void
+  pop_at_key(uint64_t key, std::unique_ptr<T> & value)
+  {
+    auto it = get_iterator_of_key(key);
+    value = nullptr;
+    if (it != elements_.end() && it->in_use) {
+      value.swap(it->value);
+      it->in_use = false;
+    }
+  }
+
+  /// Insert a key-value pair, displacing an existing pair if necessary.
+  /* The key's uniqueness is not checked on insertion.
+   * It is up to the user to ensure the key is unique.
+   * This method should not allocate memory.
+   *
+   * After insertion, if a pair was replaced, then value will contain ownership
+   * of that displaced value. Otherwise it will be a nullptr.
+   *
+   * /param key the key associated with the value to be stored
+   * /param value the value to store, and optionally the value displaced
+   */
+  bool
+  push_and_replace(uint64_t key, std::unique_ptr<T> & value)
+  {
+    bool did_replace = elements_[head_].in_use;
+    elements_[head_].key = key;
+    elements_[head_].value.swap(value);
+    elements_[head_].in_use = true;
+    head_ = (head_ + 1) % elements_.size();
+    return did_replace;
+  }
+
+  bool
+  push_and_replace(uint64_t key, std::unique_ptr<T> && value)
+  {
+    std::unique_ptr<T> temp = std::move(value);
+    return push_and_replace(key, temp);
+  }
+
+  /// Return true if the key is found in the ring buffer, otherwise false.
+  bool
+  has_key(uint64_t key)
+  {
+    return elements_.end() != get_iterator_of_key(key);
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(MappedRingBuffer<T>);
+
+  struct element
+  {
+    uint64_t key;
+    std::unique_ptr<T> value;
+    bool in_use;
+  };
+
+  typename std::vector<element>::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> elements_;
+  size_t head_;
+
+};
+
+} /* namespace ring_buffer */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_RING_BUFFER_HPP_ */

rclcpp/package.xml

@@ -13,6 +13,7 @@
   <build_depend>rcl_interfaces</build_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

rclcpp/test/test_mapped_ring_buffer.cpp

@@ -0,0 +1,154 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/mapped_ring_buffer.hpp>
+
+/*
+Tests get_copy and pop on an empty mrb.
+*/
+TEST(test_mapped_ring_buffer, empty) {
+  // Cannot create a buffer of size zero.
+  EXPECT_THROW(rclcpp::mapped_ring_buffer::MappedRingBuffer<char> mrb(0), std::invalid_argument);
+  // Getting or popping an empty buffer should result in a nullptr.
+  rclcpp::mapped_ring_buffer::MappedRingBuffer<char> mrb(1);
+
+  std::unique_ptr<char> actual;
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_EQ(nullptr, actual);
+
+  mrb.pop_at_key(1, actual);
+  EXPECT_EQ(nullptr, actual);
+}
+
+/*
+Tests push_and_replace with a temporary object.
+*/
+TEST(test_mapped_ring_buffer, temporary_l_value) {
+  rclcpp::mapped_ring_buffer::MappedRingBuffer<char> mrb(2);
+  // Pass in value with temporary object
+  mrb.push_and_replace(1, std::unique_ptr<char>(new char('a')));
+
+  std::unique_ptr<char> actual;
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_EQ('a', *actual);
+
+  mrb.pop_at_key(1, actual);
+  EXPECT_EQ('a', *actual);
+
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_EQ(nullptr, actual);
+}
+
+/*
+Tests normal usage of the mrb.
+*/
+TEST(test_mapped_ring_buffer, nominal) {
+  rclcpp::mapped_ring_buffer::MappedRingBuffer<char> mrb(2);
+  std::unique_ptr<char> expected(new char('a'));
+  // Store expected value's address for later comparison.
+  char * expected_orig = expected.get();
+
+  EXPECT_FALSE(mrb.push_and_replace(1, expected));
+
+  std::unique_ptr<char> actual;
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('a', *actual);
+  EXPECT_NE(expected_orig, actual.get());
+
+  mrb.pop_at_key(1, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('a', *actual);
+  EXPECT_EQ(expected_orig, actual.get());
+
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_EQ(nullptr, actual);
+
+  expected.reset(new char('a'));
+  EXPECT_FALSE(mrb.push_and_replace(1, expected));
+
+  expected.reset(new char('b'));
+  EXPECT_FALSE(mrb.push_and_replace(2, expected));
+
+  expected.reset(new char('c'));
+  EXPECT_TRUE(mrb.push_and_replace(3, expected));
+
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_EQ(nullptr, actual);
+
+  mrb.get_copy_at_key(2, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('b', *actual);
+
+  mrb.get_copy_at_key(3, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('c', *actual);
+}
+
+/*
+Tests get_ownership on a normal mrb.
+*/
+TEST(test_mapped_ring_buffer, get_ownership) {
+  rclcpp::mapped_ring_buffer::MappedRingBuffer<char> mrb(2);
+  std::unique_ptr<char> expected(new char('a'));
+  // Store expected value's address for later comparison.
+  char * expected_orig = expected.get();
+
+  EXPECT_FALSE(mrb.push_and_replace(1, expected));
+
+  std::unique_ptr<char> actual;
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('a', *actual);
+  EXPECT_NE(expected_orig, actual.get());
+
+  mrb.get_ownership_at_key(1, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('a', *actual);
+  EXPECT_EQ(expected_orig, actual.get());
+
+  mrb.pop_at_key(1, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('a', *actual);  // The value should be the same.
+  EXPECT_NE(expected_orig, actual.get());  // Even though we pop'ed, we didn't get the original.
+
+  mrb.get_copy_at_key(1, actual);
+  EXPECT_EQ(nullptr, actual);
+}
+
+/*
+Tests the affect of reusing keys (non-unique keys) in a mrb.
+*/
+TEST(test_mapped_ring_buffer, non_unique_keys) {
+  rclcpp::mapped_ring_buffer::MappedRingBuffer<char> mrb(2);
+
+  std::unique_ptr<char> input(new char('a'));
+  mrb.push_and_replace(1, input);
+  input.reset(new char('b'));
+
+  // Different value, same key.
+  mrb.push_and_replace(1, input);
+
+  std::unique_ptr<char> actual;
+  mrb.pop_at_key(1, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('a', *actual);
+
+  actual = nullptr;
+  mrb.pop_at_key(1, actual);
+  EXPECT_NE(nullptr, actual);
+  if (actual) EXPECT_EQ('b', *actual);
+}