diff --git a/modules/planning/common/BUILD b/modules/planning/common/BUILD
index ff27b30c327118d8f6203bf20f68a2339ec13e16..4d893a3f41697b62c32dfc0c058ddfce4756ed2c 100644
--- a/modules/planning/common/BUILD
+++ b/modules/planning/common/BUILD
@@ -11,6 +11,7 @@ cc_library(
 
 cc_test(
     name = "indexed_list_test",
+    size = "small",
     srcs = [
         "indexed_list_test.cc",
     ],
@@ -28,6 +29,19 @@ cc_library(
     ],
 )
 
+cc_test(
+    name = "indexed_queue_test",
+    size = "small",
+    srcs = [
+        "indexed_queue_test.cc",
+    ],
+    deps = [
+        ":indexed_queue",
+        "//modules/common/util",
+        "@gtest//:main",
+    ],
+)
+
 cc_library(
     name = "obstacle",
     srcs = [
diff --git a/modules/planning/common/indexed_list_test.cc b/modules/planning/common/indexed_list_test.cc
index 0c176c0e99809770b2f760cac45b33ebe145617c..e9c613feec14bea5def2ad5a96650fc0723f1882 100644
--- a/modules/planning/common/indexed_list_test.cc
+++ b/modules/planning/common/indexed_list_test.cc
@@ -22,11 +22,8 @@
 #include <unordered_map>
 #include <vector>
 
-#include "gmock/gmock.h"
 #include "gtest/gtest.h"
 
-#include "modules/common/proto/pnc_point.pb.h"
-
 #include "modules/common/util/util.h"
 #include "modules/planning/common/indexed_list.h"
 
diff --git a/modules/planning/common/indexed_queue.h b/modules/planning/common/indexed_queue.h
index 93aa50b2674d6d2b73601ab847a43953b2fae0d5..563fb860fd6754f61749bbd3cdd70cb8ae9200a9 100644
--- a/modules/planning/common/indexed_queue.h
+++ b/modules/planning/common/indexed_queue.h
@@ -53,11 +53,11 @@ class IndexedQueue {
       return false;
     }
     if (!queue_.empty() && queue_.size() == max_queue_size_) {
-      const auto &front = queue_.top();
+      const auto &front = queue_.front();
       map_.erase(front.first);
-      queue_.pop_front();
+      queue_.pop();
     }
-    queue_.push_back(std::make_pair<I, ptr>(id, ptr.get()));
+    queue_.push(std::make_pair(id, ptr.get()));
     map_[id] = std::move(ptr);
     return true;
   }
diff --git a/modules/planning/common/indexed_queue_test.cc b/modules/planning/common/indexed_queue_test.cc
new file mode 100644
index 0000000000000000000000000000000000000000..ba73cf506812d579d501f00305c98c648a2be760
--- /dev/null
+++ b/modules/planning/common/indexed_queue_test.cc
@@ -0,0 +1,67 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "gtest/gtest.h"
+
+#include "modules/common/util/util.h"
+#include "modules/planning/common/indexed_queue.h"
+
+namespace apollo {
+namespace planning {
+
+using StringIndexedQueue = IndexedQueue<int, std::string>;
+TEST(IndexedQueue, QueueSize1) {
+  StringIndexedQueue object(1);
+  ASSERT_TRUE(object.Add(1, common::util::make_unique<std::string>("one")));
+  ASSERT_TRUE(object.Find(1) != nullptr);
+  ASSERT_TRUE(object.Find(2) == nullptr);
+  ASSERT_FALSE(object.Add(1, common::util::make_unique<std::string>("one")));
+  ASSERT_EQ("one", *object.Latest());
+  ASSERT_TRUE(object.Add(2, common::util::make_unique<std::string>("two")));
+  ASSERT_TRUE(object.Find(1) == nullptr);
+  ASSERT_TRUE(object.Find(2) != nullptr);
+  ASSERT_EQ("two", *object.Latest());
+}
+
+TEST(IndexedQueue, QueueSize2) {
+  StringIndexedQueue object(2);
+  ASSERT_TRUE(object.Add(1, common::util::make_unique<std::string>("one")));
+  ASSERT_TRUE(object.Find(1) != nullptr);
+  ASSERT_TRUE(object.Find(2) == nullptr);
+  ASSERT_FALSE(object.Add(1, common::util::make_unique<std::string>("one")));
+  ASSERT_EQ("one", *object.Latest());
+  ASSERT_TRUE(object.Add(2, common::util::make_unique<std::string>("two")));
+  ASSERT_TRUE(object.Find(1) != nullptr);
+  ASSERT_TRUE(object.Find(2) != nullptr);
+  ASSERT_EQ("two", *object.Latest());
+  ASSERT_TRUE(object.Add(3, common::util::make_unique<std::string>("three")));
+  ASSERT_TRUE(object.Find(1) == nullptr);
+  ASSERT_TRUE(object.Find(2) != nullptr);
+  ASSERT_TRUE(object.Find(3) != nullptr);
+  ASSERT_TRUE(object.Find(4) == nullptr);
+  ASSERT_EQ("three", *object.Latest());
+}
+
+}  // namespace planning
+}  // namespace apollo