fix(planner_manager): remove succeeded candidate module (#3346)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Apr 11, 2023 · 86b773a · 86b773a
1 parent 334e066
commit 86b773a
Showing 1 changed file with 7 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -294,17 +294,22 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runCandidateModu
     processing_time_.at(name) += stop_watch_.toc(name, true);
   }
 
-  const auto remove_failure_modules = [this](auto & m) {
+  const auto remove_expired_modules = [this](auto & m) {
     if (m->getCurrentStatus() == ModuleStatus::FAILURE) {
       deleteExpiredModules(m);
       return true;
     }
 
+    if (m->getCurrentStatus() == ModuleStatus::SUCCESS) {
+      deleteExpiredModules(m);
+      return true;
+    }
+
     return false;
   };
 
   executable_modules.erase(
-    std::remove_if(executable_modules.begin(), executable_modules.end(), remove_failure_modules),
+    std::remove_if(executable_modules.begin(), executable_modules.end(), remove_expired_modules),
     executable_modules.end());
 
   if (executable_modules.empty()) {