Add priority queue container to AOS.
Needed in order to handle out-of-order incoming EKF observations.
Also, rename ring_buffer folder to more generic containers/ and
buildify y2017/BUILD.
Change-Id: Idc343729628e24eceb024b80a2439170ead289de
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
new file mode 100644
index 0000000..ff0b600
--- /dev/null
+++ b/aos/containers/BUILD
@@ -0,0 +1,37 @@
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+ name = "ring_buffer",
+ hdrs = [
+ "ring_buffer.h",
+ ],
+)
+
+cc_test(
+ name = "ring_buffer_test",
+ srcs = [
+ "ring_buffer_test.cc",
+ ],
+ deps = [
+ ":ring_buffer",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "priority_queue",
+ hdrs = [
+ "priority_queue.h",
+ ],
+)
+
+cc_test(
+ name = "priority_queue_test",
+ srcs = [
+ "priority_queue_test.cc",
+ ],
+ deps = [
+ ":priority_queue",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/aos/containers/priority_queue.h b/aos/containers/priority_queue.h
new file mode 100644
index 0000000..0bc94e7
--- /dev/null
+++ b/aos/containers/priority_queue.h
@@ -0,0 +1,148 @@
+#ifndef AOS_CONTAINERS_PRIORITY_QUEUE_H_
+#define AOS_CONTAINERS_PRIORITY_QUEUE_H_
+
+#include <array>
+#include <iterator>
+
+namespace aos {
+
+// Creates a priority queue which will never exceed a particular size.
+// Data: The type of the data to store.
+// buffer_size: The max number of Data to store.
+// Compare: A comparison function. If std::less were used here, then the
+// smallest element would be discarded first, see
+// https://en.cppreference.com/w/cpp/named_req/Compare
+// The lowest priority elements will be discarded first to maintain buffer_size.
+// Note that as a "queue" this is a bit incomplete because there is no mechanism
+// to pop from the queue.
+template <typename Data, size_t buffer_size, typename Compare>
+class PriorityQueue {
+ public:
+ class iterator {
+ public:
+ explicit iterator(PriorityQueue *queue, size_t idx)
+ : queue_(queue), idx_(idx) {}
+ iterator &operator++() {
+ idx_ = queue_->next_idx(idx_);
+ return *this;
+ }
+ iterator operator++(int) {
+ iterator retval = *this;
+ ++(*this);
+ return retval;
+ }
+ iterator &operator--() {
+ idx_ = queue_->prev_idx(idx_);
+ return *this;
+ }
+ iterator operator--(int) {
+ iterator retval = *this;
+ --(*this);
+ return retval;
+ }
+ bool operator==(iterator other) const {
+ return queue_ == other.queue_ && idx_ == other.idx_;
+ }
+ bool operator!=(iterator other) const { return !(*this == other); }
+ Data &operator*() { return queue_->get(idx_); }
+ Data *operator->() { return &queue_->get(idx_); }
+
+ private:
+ PriorityQueue *queue_;
+ size_t idx_;
+ };
+
+ constexpr PriorityQueue() {}
+
+ // Inserts data into the queue and returns an iterator for the inserted
+ // element. If the queue was already full, then the lowest priority element
+ // will be discarded. If data is lower priority than all the current elements
+ // and the queue is full, then data is ignored and end() is returned.
+ // PushFromBottom starts the search from the bottom of the queue.
+ // TODO(james): If performance becomes an issue, improve search.
+ iterator PushFromBottom(const Data &data) {
+ size_t lower_idx = buffer_size;
+ size_t upper_idx = bottom_;
+ // Find our spot in the queue:
+ while (upper_idx != buffer_size && !cmp_(data, list_[upper_idx].data)) {
+ lower_idx = upper_idx;
+ upper_idx = list_[upper_idx].upper_idx;
+ if (upper_idx == buffer_size) {
+ break;
+ }
+ }
+ return InsertInList(data, lower_idx, upper_idx);
+ }
+
+ size_t size() const { return size_; }
+ bool empty() const { return size_ == 0; }
+ bool full() const { return size_ == buffer_size; }
+
+ Data &top() { return list_[top_].data; }
+ Data &get(size_t idx) { return list_[idx].data; }
+ iterator begin() { return iterator(this, bottom_); }
+ iterator end() { return iterator(this, buffer_size); }
+
+ // Gets the index of the next (higher valued) element in the queue.
+ size_t next_idx(size_t idx) const { return list_[idx].upper_idx; }
+ // Gets the index of the previous (lower valued) element in the queue.
+ size_t prev_idx(size_t idx) const { return list_[idx].lower_idx; }
+ private:
+ struct Datum {
+ // A list element with data and the indices of the next highest/lowest
+ // priority elements.
+ Data data;
+ // Values of buffer_size indicate that we are at the beginning or end of
+ // the queue.
+ size_t lower_idx = buffer_size;
+ size_t upper_idx = buffer_size;
+ };
+
+ // Insert an element above lower_idx and below upper_idx.
+ iterator InsertInList(const Data &data, size_t lower_idx, size_t upper_idx) {
+ // For inserting new elements, when we are initially filling the queue we
+ // will increment upwards in the array; once full, we just evict the
+ // lowest priority element.
+ size_t insertion_idx = size();
+ if (full()) {
+ if (upper_idx == bottom_) {
+ // this item is lower priority than everyone else, don't insert it.
+ return end();
+ }
+ // Eject the lowest priority element.
+ insertion_idx = bottom_;
+ if (lower_idx == insertion_idx) {
+ lower_idx = buffer_size;
+ }
+ --size_;
+ bottom_ = list_[bottom_].upper_idx;
+ list_[bottom_].lower_idx = buffer_size;
+ }
+ if (upper_idx != buffer_size) {
+ list_[upper_idx].lower_idx = insertion_idx;
+ }
+ if (lower_idx != buffer_size) {
+ list_[lower_idx].upper_idx = insertion_idx;
+ }
+ if (bottom_ == upper_idx) {
+ bottom_ = insertion_idx;
+ }
+ if (top_ == lower_idx) {
+ top_ = insertion_idx;
+ }
+ list_[insertion_idx].data = data;
+ list_[insertion_idx].upper_idx = upper_idx;
+ list_[insertion_idx].lower_idx = lower_idx;
+ ++size_;
+ return iterator(this, insertion_idx);
+ }
+ ::std::array<Datum, buffer_size> list_;
+ // Index of the bottom and top of the queue.
+ size_t bottom_ = buffer_size, top_ = buffer_size;
+ // Number of elements currently in the queue.
+ size_t size_ = 0;
+ Compare cmp_;
+};
+} // namespace aos
+
+#endif // AOS_CONTAINERS_PRIORITY_QUEUE_H_
diff --git a/aos/containers/priority_queue_test.cc b/aos/containers/priority_queue_test.cc
new file mode 100644
index 0000000..2c45990
--- /dev/null
+++ b/aos/containers/priority_queue_test.cc
@@ -0,0 +1,145 @@
+#include "aos/containers/priority_queue.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+// Effectively copies the implementation of ::std::less just to demonstrate how
+// things work.
+class ExampleCompare {
+ public:
+ constexpr bool operator()(const int &lhs, const int &rhs) const {
+ return lhs < rhs;
+ }
+};
+
+class PriorityQueueTest : public ::testing::Test {
+ public:
+ PriorityQueueTest() {}
+ protected:
+ PriorityQueue<int, 10, ExampleCompare> queue_;
+};
+
+TEST_F(PriorityQueueTest, DefaultIsEmpty) {
+ ASSERT_EQ(0u, queue_.size());
+ ASSERT_TRUE(queue_.empty());
+ ASSERT_FALSE(queue_.full());
+}
+
+TEST_F(PriorityQueueTest, CanAddData) {
+ auto it = queue_.PushFromBottom(5);
+ ASSERT_EQ(1u, queue_.size());
+ ASSERT_FALSE(queue_.empty());
+ ASSERT_FALSE(queue_.full());
+ EXPECT_EQ(5, *it);
+ EXPECT_EQ(5, queue_.get(0));
+ EXPECT_EQ(5, queue_.top());
+ EXPECT_EQ(queue_.begin(), it);
+ // Also check pre/post-fix increment/decrement operators
+ EXPECT_EQ(queue_.end(), ++it);
+ EXPECT_EQ(queue_.begin(), --it);
+ EXPECT_EQ(queue_.begin(), it++);
+ EXPECT_EQ(queue_.end(), it--);
+ EXPECT_EQ(queue_.begin(), it);
+}
+
+TEST_F(PriorityQueueTest, PriorityInsertion) {
+ queue_.PushFromBottom(10);
+ queue_.PushFromBottom(20);
+ queue_.PushFromBottom(15);
+ auto it = queue_.PushFromBottom(11);
+ ASSERT_EQ(4u, queue_.size());
+ ASSERT_FALSE(queue_.full());
+ ::std::vector<int> reverse_expected{20, 15, 11};
+ EXPECT_EQ(20, queue_.top());
+ for (; it != queue_.end(); ++it) {
+ EXPECT_EQ(reverse_expected.back(), *it);
+ reverse_expected.pop_back();
+ }
+ ASSERT_TRUE(reverse_expected.empty());
+}
+
+TEST_F(PriorityQueueTest, FullBufferInsertTop) {
+ for (int ii = 0; ii < 10; ++ii) {
+ queue_.PushFromBottom(ii);
+ }
+ ASSERT_EQ(10u, queue_.size());
+ ASSERT_TRUE(queue_.full());
+ // Check adding value at top.
+ queue_.PushFromBottom(100);
+ ASSERT_EQ(10u, queue_.size());
+ ASSERT_TRUE(queue_.full());
+ ::std::vector<int> reverse_expected{100, 9, 8, 7, 6, 5, 4, 3, 2, 1};
+ EXPECT_EQ(100, queue_.top());
+ for (int val : queue_) {
+ EXPECT_EQ(reverse_expected.back(), val);
+ reverse_expected.pop_back();
+ }
+ ASSERT_TRUE(reverse_expected.empty());
+}
+
+TEST_F(PriorityQueueTest, FullBufferInsertMiddle) {
+ for (int ii = 9; ii >= 0; --ii) {
+ queue_.PushFromBottom(ii);
+ }
+ // Check adding value in the middle.
+ queue_.PushFromBottom(5);
+
+ ::std::vector<int> reverse_expected{9, 8, 7, 6, 5, 5, 4, 3, 2, 1};
+ EXPECT_EQ(9, queue_.top());
+ for (int val : queue_) {
+ EXPECT_EQ(reverse_expected.back(), val);
+ reverse_expected.pop_back();
+ }
+ ASSERT_TRUE(reverse_expected.empty());
+}
+
+TEST_F(PriorityQueueTest, FullBufferInsertBelowBottom) {
+ for (int ii = 9; ii >= 0; --ii) {
+ queue_.PushFromBottom(ii);
+ }
+ // Check adding value at the bottom where it will be dropped.
+ queue_.PushFromBottom(-1);
+
+ ::std::vector<int> reverse_expected{9, 8, 7, 6, 5, 4, 3, 2, 1, 0};
+ EXPECT_EQ(9, queue_.top());
+ for (int val : queue_) {
+ EXPECT_EQ(reverse_expected.back(), val);
+ reverse_expected.pop_back();
+ }
+ ASSERT_TRUE(reverse_expected.empty());
+}
+
+TEST_F(PriorityQueueTest, FullBufferInsertBottom) {
+ for (int ii = 18; ii >= 0; ii -= 2) {
+ queue_.PushFromBottom(ii);
+ }
+ ASSERT_TRUE(queue_.full());
+ // Check adding value at the bottom where it displaces the bottom value.
+ queue_.PushFromBottom(1);
+
+ ::std::vector<int> reverse_expected{18, 16, 14, 12, 10, 8, 6, 4, 2, 1};
+ EXPECT_EQ(18, queue_.top());
+ for (int val : queue_) {
+ EXPECT_EQ(reverse_expected.back(), val);
+ reverse_expected.pop_back();
+ }
+ ASSERT_TRUE(reverse_expected.empty());
+}
+
+// Check that operator-> works as expected on the iterator.
+struct TestStruct {
+ int a;
+ friend bool operator<(const TestStruct &lhs, const TestStruct &rhs) {
+ return lhs.a < rhs.a;
+ }
+};
+TEST(PriorirtyQueueTest, MemberAccess) {
+ PriorityQueue<TestStruct, 10, ::std::less<TestStruct>> q;
+ auto it = q.PushFromBottom({11});
+ EXPECT_EQ(11, it->a);
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/ring_buffer/ring_buffer.h b/aos/containers/ring_buffer.h
similarity index 98%
rename from aos/ring_buffer/ring_buffer.h
rename to aos/containers/ring_buffer.h
index 9f93a8c..d4cd92a 100644
--- a/aos/ring_buffer/ring_buffer.h
+++ b/aos/containers/ring_buffer.h
@@ -13,7 +13,7 @@
public:
static constexpr size_t kBufferSize = buffer_size;
- RingBuffer() {}
+ constexpr RingBuffer() {}
// Add an item to the RingBuffer, overwriting the oldest element if necessary
void Push(const Data &data) {
diff --git a/aos/ring_buffer/ring_buffer_test.cc b/aos/containers/ring_buffer_test.cc
similarity index 98%
rename from aos/ring_buffer/ring_buffer_test.cc
rename to aos/containers/ring_buffer_test.cc
index a01b315..5fb2331 100644
--- a/aos/ring_buffer/ring_buffer_test.cc
+++ b/aos/containers/ring_buffer_test.cc
@@ -1,4 +1,4 @@
-#include "aos/ring_buffer/ring_buffer.h"
+#include "aos/containers/ring_buffer.h"
#include "gtest/gtest.h"
diff --git a/aos/ring_buffer/BUILD b/aos/ring_buffer/BUILD
deleted file mode 100644
index 9f6ab06..0000000
--- a/aos/ring_buffer/BUILD
+++ /dev/null
@@ -1,19 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-cc_library(
- name = "ring_buffer",
- hdrs = [
- "ring_buffer.h",
- ],
-)
-
-cc_test(
- name = "ring_buffer_test",
- srcs = [
- "ring_buffer_test.cc",
- ],
- deps = [
- ":ring_buffer",
- "//aos/testing:googletest",
- ],
-)
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 3a3d17e..732c6dc 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -1,123 +1,123 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
queue_library(
- name = 'superstructure_queue',
- srcs = [
- 'superstructure.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- '//frc971/control_loops:profiled_subsystem_queue',
- '//frc971/control_loops:queues',
- ],
+ name = "superstructure_queue",
+ srcs = [
+ "superstructure.q",
+ ],
+ deps = [
+ "//aos/controls:control_loop_queues",
+ "//frc971/control_loops:profiled_subsystem_queue",
+ "//frc971/control_loops:queues",
+ ],
)
cc_library(
- name = 'superstructure_lib',
- srcs = [
- 'superstructure.cc',
- ],
- hdrs = [
- 'superstructure.h',
- ],
- deps = [
- ':vision_distance_average',
- ':superstructure_queue',
- '//aos/controls:control_loop',
- '//y2017/control_loops/superstructure/column',
- '//y2017/control_loops/superstructure/hood',
- '//y2017/control_loops/superstructure/intake',
- '//y2017/control_loops/superstructure/shooter',
- '//y2017:constants',
- ],
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ ":vision_distance_average",
+ "//aos/controls:control_loop",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure/column",
+ "//y2017/control_loops/superstructure/hood",
+ "//y2017/control_loops/superstructure/intake",
+ "//y2017/control_loops/superstructure/shooter",
+ ],
)
cc_test(
- name = 'superstructure_lib_test',
- srcs = [
- 'superstructure_lib_test.cc',
- ],
- deps = [
- ':superstructure_queue',
- ':superstructure_lib',
- '//aos/controls:control_loop_test',
- '//aos:math',
- '//aos:queues',
- '//aos/time:time',
- '//aos/testing:googletest',
- '//frc971/control_loops:position_sensor_sim',
- '//frc971/control_loops:team_number_test_environment',
- '//y2017/control_loops/superstructure/column:column_plants',
- '//y2017/control_loops/superstructure/hood:hood_plants',
- '//y2017/control_loops/superstructure/intake:intake_plants',
- '//y2017/control_loops/superstructure/shooter:shooter_plants',
- ],
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos:queues",
+ "//aos/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//y2017/control_loops/superstructure/column:column_plants",
+ "//y2017/control_loops/superstructure/hood:hood_plants",
+ "//y2017/control_loops/superstructure/intake:intake_plants",
+ "//y2017/control_loops/superstructure/shooter:shooter_plants",
+ ],
)
cc_binary(
- name = 'superstructure',
- srcs = [
- 'superstructure_main.cc',
- ],
- deps = [
- '//aos:init',
- ':superstructure_lib',
- ':superstructure_queue',
- ],
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos:init",
+ ],
)
cc_library(
- name = 'vision_time_adjuster',
- hdrs = [
- 'vision_time_adjuster.h',
- ],
- srcs = [
- 'vision_time_adjuster.cc',
- ],
- deps = [
- ':superstructure_queue',
- '//aos/ring_buffer:ring_buffer',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//y2017/control_loops/drivetrain:polydrivetrain_plants',
- '//y2017/vision:vision_queue',
- ],
+ name = "vision_time_adjuster",
+ srcs = [
+ "vision_time_adjuster.cc",
+ ],
+ hdrs = [
+ "vision_time_adjuster.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/containers:ring_buffer",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//y2017/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2017/vision:vision_queue",
+ ],
)
cc_test(
- name = 'vision_time_adjuster_test',
- srcs = [
- 'vision_time_adjuster_test.cc',
- ],
- deps = [
- ':vision_time_adjuster',
- '//aos/time:time',
- '//aos/testing:googletest',
- '//aos/testing:test_shm',
- ],
+ name = "vision_time_adjuster_test",
+ srcs = [
+ "vision_time_adjuster_test.cc",
+ ],
+ deps = [
+ ":vision_time_adjuster",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ "//aos/time",
+ ],
)
cc_library(
- name = 'vision_distance_average',
- hdrs = [
- 'vision_distance_average.h',
- ],
- deps = [
- '//aos/time:time',
- '//aos/ring_buffer:ring_buffer',
- '//y2017/vision:vision_queue',
- ],
+ name = "vision_distance_average",
+ hdrs = [
+ "vision_distance_average.h",
+ ],
+ deps = [
+ "//aos/containers:ring_buffer",
+ "//aos/time",
+ "//y2017/vision:vision_queue",
+ ],
)
cc_test(
- name = 'vision_distance_average_test',
- srcs = [
- 'vision_distance_average_test.cc',
- ],
- deps = [
- ':vision_distance_average',
- '//aos/time:time',
- '//aos/testing:googletest',
- ],
+ name = "vision_distance_average_test",
+ srcs = [
+ "vision_distance_average_test.cc",
+ ],
+ deps = [
+ ":vision_distance_average",
+ "//aos/testing:googletest",
+ "//aos/time",
+ ],
)
diff --git a/y2017/control_loops/superstructure/vision_distance_average.h b/y2017/control_loops/superstructure/vision_distance_average.h
index 22464b7..74e775d 100644
--- a/y2017/control_loops/superstructure/vision_distance_average.h
+++ b/y2017/control_loops/superstructure/vision_distance_average.h
@@ -4,7 +4,7 @@
#include <stdint.h>
#include <chrono>
-#include "aos/ring_buffer/ring_buffer.h"
+#include "aos/containers/ring_buffer.h"
#include "aos/time/time.h"
#include "y2017/vision/vision.q.h"
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.h b/y2017/control_loops/superstructure/vision_time_adjuster.h
index 52e7d70..b475a99 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.h
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.h
@@ -3,7 +3,7 @@
#include <stdint.h>
-#include "aos/ring_buffer/ring_buffer.h"
+#include "aos/containers/ring_buffer.h"
#include "aos/time/time.h"
#include "y2017/vision/vision.q.h"