Add clear() function to priority_queue

Change-Id: I9c24cd6c39aa4b76680c87a6239416239d7c56eb
diff --git a/aos/containers/priority_queue.h b/aos/containers/priority_queue.h
index 0bc94e7..8fb5e5c 100644
--- a/aos/containers/priority_queue.h
+++ b/aos/containers/priority_queue.h
@@ -78,8 +78,17 @@
   bool empty() const { return size_ == 0; }
   bool full() const { return size_ == buffer_size; }
 
+  // Removes all the elements from the queue:
+  void clear() {
+    size_ = 0;
+    bottom_ = buffer_size;
+    top_ = buffer_size;
+  }
+
   Data &top() { return list_[top_].data; }
+  const Data &top() const { return list_[top_].data; }
   Data &get(size_t idx) { return list_[idx].data; }
+  const Data &get(size_t idx) const { return list_[idx].data; }
   iterator begin() { return iterator(this, bottom_); }
   iterator end() { return iterator(this, buffer_size); }
 
diff --git a/aos/containers/priority_queue_test.cc b/aos/containers/priority_queue_test.cc
index 2c45990..c05d827 100644
--- a/aos/containers/priority_queue_test.cc
+++ b/aos/containers/priority_queue_test.cc
@@ -52,7 +52,7 @@
   ASSERT_EQ(4u, queue_.size());
   ASSERT_FALSE(queue_.full());
   ::std::vector<int> reverse_expected{20, 15, 11};
-  EXPECT_EQ(20, queue_.top());
+  EXPECT_EQ(10, *queue_.begin());
   for (; it != queue_.end(); ++it) {
     EXPECT_EQ(reverse_expected.back(), *it);
     reverse_expected.pop_back();
@@ -60,6 +60,30 @@
   ASSERT_TRUE(reverse_expected.empty());
 }
 
+// Tests that the clear() method works properly.
+TEST_F(PriorityQueueTest, ClearQueue) {
+  queue_.PushFromBottom(10);
+  queue_.PushFromBottom(20);
+  queue_.PushFromBottom(15);
+  queue_.PushFromBottom(11);
+  ASSERT_EQ(4u, queue_.size());
+  ASSERT_FALSE(queue_.full());
+  queue_.clear();
+  ASSERT_TRUE(queue_.empty());
+  ASSERT_EQ(0u, queue_.size());
+
+  queue_.PushFromBottom(1);
+  queue_.PushFromBottom(3);
+  queue_.PushFromBottom(2);
+  ASSERT_EQ(3u, queue_.size());
+  ::std::vector<int> reverse_expected{3, 2, 1};
+  for (const int val : queue_) {
+    EXPECT_EQ(reverse_expected.back(), val);
+    reverse_expected.pop_back();
+  }
+  ASSERT_TRUE(reverse_expected.empty());
+}
+
 TEST_F(PriorityQueueTest, FullBufferInsertTop) {
   for (int ii = 0; ii < 10; ++ii) {
     queue_.PushFromBottom(ii);