feat(interface): add new option last_keep (autowarefoundation#4811)

* feat(interface): add new option last_keep Signed-off-by: satoshi-ota <[email protected]> * feat(planner_manager): keep last module Signed-off-by: satoshi-ota <[email protected]> * update param config Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]> Signed-off-by: kosuke55 <[email protected]> Co-authored-by: kosuke55 <[email protected]>
tier4 · Sep 16, 2023 · 63dd7d5 · 63dd7d5
1 parent 5a8c980
commit 63dd7d5
Show file tree

Hide file tree

Showing 5 changed files with 62 additions and 14 deletions.
diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml
@@ -8,6 +8,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -16,6 +17,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -24,6 +26,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -32,6 +35,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -40,6 +44,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 0
       max_module_size: 1
 
@@ -48,6 +53,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 2
       max_module_size: 1
 
@@ -56,6 +62,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: true
       priority: 1
       max_module_size: 1
 
@@ -64,6 +71,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 4
       max_module_size: 1
 
@@ -72,6 +80,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 3
       max_module_size: 1
 
@@ -80,5 +89,6 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 7
       max_module_size: 1
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
@@ -28,6 +28,7 @@ struct ModuleConfigParameters
   bool enable_rtc{false};
   bool enable_simultaneous_execution_as_approved_module{false};
   bool enable_simultaneous_execution_as_candidate_module{false};
+  bool keep_last{false};
   uint8_t priority{0};
   uint8_t max_module_size{0};
 };

diff --git a/...ath_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/...ath_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp
@@ -54,6 +54,7 @@ class SceneModuleManagerInterface
     enable_simultaneous_execution_as_candidate_module_(
       config.enable_simultaneous_execution_as_candidate_module),
     enable_rtc_(config.enable_rtc),
+    keep_last_(config.keep_last),
     max_module_num_(config.max_module_size),
     priority_(config.priority)
   {
@@ -220,6 +221,8 @@ class SceneModuleManagerInterface
     return enable_simultaneous_execution_as_candidate_module_;
   }
 
+  bool isKeepLast() const { return keep_last_; }
+
   void setData(const std::shared_ptr<PlannerData> & planner_data) { planner_data_ = planner_data; }
 
   void reset()
@@ -281,6 +284,8 @@ class SceneModuleManagerInterface
 private:
   bool enable_rtc_;
 
+  bool keep_last_;
+
   size_t max_module_num_;
 
   size_t priority_;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -282,6 +282,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
     config.enable_simultaneous_execution_as_candidate_module =
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
+    config.keep_last = declare_parameter<bool>(ns + "keep_last");
     config.priority = declare_parameter<int>(ns + "priority");
     config.max_module_size = declare_parameter<int>(ns + "max_module_size");
     return config;

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -445,6 +445,17 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   BehaviorModuleOutput output = getReferencePath(data);
   results.emplace("root", output);
 
+  if (approved_module_ptrs_.empty()) {
+    return output;
+  }
+
+  // move modules whose keep last flag is true to end of the approved_module_ptrs_.
+  {
+    std::sort(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [this](auto a, auto b) {
+      return !getManager(a)->isKeepLast() && getManager(b)->isKeepLast();
+    });
+  }
+
   /**
    * execute all approved modules.
    */
@@ -453,6 +464,13 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
     results.emplace(m->name(), output);
   });
 
+  /**
+   * pop waiting approve module. push it back candidate modules. if waiting approve module is found
+   * in iteration std::find_if, not only the module but also the next one are removed from
+   * approved_module_ptrs_ since the next module's input (= waiting approve module's output) maybe
+   * change significantly. And, only the waiting approve module is pushed back to
+   * candidate_module_ptrs_.
+   */
   /**
    * pop waiting approve module. push it back candidate modules. if waiting approve module is found
    * in iteration std::find_if, not only the module but also the next one are removed from
@@ -461,22 +479,32 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
    * candidate_module_ptrs_.
    */
   {
-    const auto waiting_approval_modules = [](const auto & m) { return m->isWaitingApproval(); };
+    const auto not_keep_last_module = std::find_if(
+      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
+      [this](const auto & m) { return !getManager(m)->isKeepLast(); });
 
-    const auto itr = std::find_if(
-      approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules);
+    // convert reverse iterator -> iterator
+    const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend()
+                             ? std::next(not_keep_last_module).base()
+                             : approved_module_ptrs_.begin();
 
-    if (itr != approved_module_ptrs_.end()) {
+    const auto waiting_approval_modules_itr = std::find_if(
+      begin_itr, approved_module_ptrs_.end(),
+      [](const auto & m) { return m->isWaitingApproval(); });
+
+    if (waiting_approval_modules_itr != approved_module_ptrs_.end()) {
       clearCandidateModules();
-      candidate_module_ptrs_.push_back(*itr);
+      candidate_module_ptrs_.push_back(*waiting_approval_modules_itr);
+
+      debug_info_.emplace_back(
+        *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval");
 
       std::for_each(
-        std::next(itr), approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); });
+        waiting_approval_modules_itr, approved_module_ptrs_.end(),
+        [&results](const auto & m) { results.erase(m->name()); });
 
-      debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval");
+      approved_module_ptrs_.erase(waiting_approval_modules_itr);
     }
-
-    approved_module_ptrs_.erase(itr, approved_module_ptrs_.end());
   }
 
   /**
@@ -506,12 +534,15 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   /**
    * use the last module's output as approved modules planning result.
    */
-  const auto output_module_name = approved_module_ptrs_.back()->name();
-  const auto approved_modules_output = [&output_module_name, &results]() {
-    if (results.count(output_module_name) == 0) {
-      return results.at("root");
+  const auto approved_modules_output = [&results, this]() {
+    const auto itr = std::find_if(
+      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
+      [&results](const auto & m) { return results.count(m->name()) != 0; });
+
+    if (itr != approved_module_ptrs_.rend()) {
+      return results.at((*itr)->name());
     }
-    return results.at(output_module_name);
+    return results.at("root");
   }();
 
   const auto not_success_itr = std::find_if(