Planning: clear planning state when stop. (#4001)

modules/planning/common/indexed_queue.h

@@ -62,6 +62,14 @@ class IndexedQueue {
     return true;
   }
 
+  void Clear() {
+    capacity_ = 0;
+    while (!queue_.empty()) {
+      queue_.pop();
+    }
+    map_.clear();
+  }
+
  public:
   std::size_t capacity_ = 0;
   std::queue<std::pair<I, const T *>> queue_;

modules/planning/planning.cc

@@ -47,6 +47,8 @@ using apollo::common::adapter::AdapterManager;
 using apollo::common::time::Clock;
 using apollo::hdmap::HDMapUtil;
 
+Planning::~Planning() { Stop(); }
+
 std::string Planning::Name() const { return "planning"; }
 
 #define CHECK_ADAPTER(NAME)                                              \
@@ -433,6 +435,7 @@ void Planning::Stop() {
   last_publishable_trajectory_.reset(nullptr);
   frame_.reset(nullptr);
   planner_.reset(nullptr);
+  FrameHistory::instance()->Clear();
 }
 
 void Planning::SetLastPublishableTrajectory(

modules/planning/planning.h

@@ -51,6 +51,8 @@ namespace planning {
  */
 class Planning : public apollo::common::ApolloApp {
  public:
+  Planning() = default;
+  virtual ~Planning();
   /**
    * @brief module name
    * @return module name