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 · Aug 31, 2023 · 58db04e · 58db04e
1 parent b2e4582
commit 58db04e
Show file tree

Hide file tree

Showing 5 changed files with 64 additions and 17 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
@@ -55,6 +55,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)
   {
@@ -224,6 +225,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()
@@ -290,6 +293,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
@@ -284,6 +284,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
@@ -456,11 +456,28 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
     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();
+    });
+  }
+
   // lock approved modules besides last one
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
     m->lockOutputPath();
   });
-  approved_module_ptrs_.back()->unlockOutputPath();
+
+  // unlock only last approved module except keep last module.
+  {
+    const auto not_keep_last_modules = std::find_if(
+      approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
+      [this](const auto & m) { return !getManager(m)->isKeepLast(); });
+
+    if (not_keep_last_modules != approved_module_ptrs_.rend()) {
+      (*not_keep_last_modules)->unlockOutputPath();
+    }
+  }
 
   /**
    * execute all approved modules.
@@ -478,21 +495,31 @@ 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_.rbegin(), approved_module_ptrs_.rend(), 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_.rend()) {
-      const auto is_last_module = std::distance(approved_module_ptrs_.rbegin(), itr) == 0;
-      if (is_last_module) {
-        clearCandidateModules();
-        candidate_module_ptrs_.push_back(*itr);
+    const auto waiting_approval_modules_itr = std::find_if(
+      begin_itr, approved_module_ptrs_.end(),
+      [](const auto & m) { return m->isWaitingApproval(); });
 
-        debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval");
+    if (waiting_approval_modules_itr != approved_module_ptrs_.end()) {
+      clearCandidateModules();
+      candidate_module_ptrs_.push_back(*waiting_approval_modules_itr);
 
-        approved_module_ptrs_.pop_back();
-      }
+      debug_info_.emplace_back(
+        *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval");
+
+      std::for_each(
+        waiting_approval_modules_itr, approved_module_ptrs_.end(),
+        [&results](const auto & m) { results.erase(m->name()); });
+
+      approved_module_ptrs_.erase(waiting_approval_modules_itr);
 
       std::for_each(
         manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
@@ -529,12 +556,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");
   }();
 
   /**