feat(behavior_path_planner): run keep last approved modules after can…

…didate modules (#5970) Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Dec 28, 2023 · 70b88d8 · 70b88d8
1 parent 1c42fbe
commit 70b88d8
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 9 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -435,6 +435,15 @@ class PlannerManager
     const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
     const BehaviorModuleOutput & previous_module_output);
 
+  /**
+   * @brief run keep last approved modules
+   * @param planner data.
+   * @param previous module output.
+   * @return planning result.
+   */
+  BehaviorModuleOutput runKeepLastModules(
+    const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & previous_output) const;
+
   static std::string getNames(const std::vector<SceneModulePtr> & modules);
 
   std::optional<lanelet::ConstLanelet> root_lanelet_{std::nullopt};

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -141,33 +141,42 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
        * STEP3: if there is no module that need to be launched, return approved modules' output
        */
       if (request_modules.empty()) {
+        const auto output = runKeepLastModules(data, approved_modules_output);
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return approved_modules_output;
+        return output;
       }
 
       /**
        * STEP4: if there is module that should be launched, execute the module
        */
       const auto [highest_priority_module, candidate_modules_output] =
         runRequestModules(request_modules, data, approved_modules_output);
+
+      /**
+       * STEP5: run keep last approved modules after running candidate modules.
+       * NOTE: if no candidate module is launched, approved_modules_output used as input for keep
+       * last modules and return the result immediately.
+       */
+      const auto output = runKeepLastModules(
+        data, highest_priority_module ? candidate_modules_output : approved_modules_output);
       if (!highest_priority_module) {
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return approved_modules_output;
+        return output;
       }
 
       /**
-       * STEP5: if the candidate module's modification is NOT approved yet, return the result.
+       * STEP6: if the candidate module's modification is NOT approved yet, return the result.
        * NOTE: the result is output of the candidate module, but the output path don't contains path
        * shape modification that needs approval. On the other hand, it could include velocity
        * profile modification.
        */
       if (highest_priority_module->isWaitingApproval()) {
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return candidate_modules_output;
+        return output;
       }
 
       /**
-       * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_
+       * STEP7: if the candidate module is approved, push the module into approved_module_ptrs_
        */
       addApprovedModule(highest_priority_module);
       clearCandidateModules();
@@ -178,7 +187,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
           logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.",
           max_iteration_num_);
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return candidate_modules_output;
+        return output;
       }
     }
 
@@ -394,6 +403,19 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
   return request_modules;
 }
 
+BehaviorModuleOutput PlannerManager::runKeepLastModules(
+  const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & previous_output) const
+{
+  auto output = previous_output;
+  std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
+    if (getManager(m)->isKeepLast()) {
+      output = run(m, data, output);
+    }
+  });
+
+  return output;
+}
+
 BehaviorModuleOutput PlannerManager::getReferencePath(
   const std::shared_ptr<PlannerData> & data) const
 {
@@ -652,11 +674,13 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   }
 
   /**
-   * execute all approved modules.
+   * execute approved modules except keep last modules.
    */
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
-    output = run(m, data, output);
-    results.emplace(m->name(), output);
+    if (!getManager(m)->isKeepLast()) {
+      output = run(m, data, output);
+      results.emplace(m->name(), output);
+    }
   });
 
   /**