feat(behavior_path_planner): add always executable module option (aut…

…owarefoundation#4785) * feat(behavior_path_planner): add always executable module option Signed-off-by: kosuke55 <[email protected]> * fix conditions Signed-off-by: kosuke55 <[email protected]> * rename itr Signed-off-by: kosuke55 <[email protected]> * Update planning/behavior_path_planner/src/planner_manager.cpp Co-authored-by: Satoshi OTA <[email protected]> * Update planning/behavior_path_planner/src/planner_manager.cpp Co-authored-by: Satoshi OTA <[email protected]> * Update planning/behavior_path_planner/src/planner_manager.cpp Co-authored-by: Satoshi OTA <[email protected]> * fix candidate break Signed-off-by: kosuke55 <[email protected]> * Revert "fix candidate break" This reverts commit 0071bbd. * remove continue Signed-off-by: kosuke55 <[email protected]> * check manager_ptr->isAlwaysExecutableModule first Signed-off-by: kosuke55 <[email protected]> * common similar process Signed-off-by: kosuke55 <[email protected]> * fix getRequestModules Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]> Co-authored-by: Satoshi OTA <[email protected]>
tier4 · Sep 19, 2023 · 81bf2af · 81bf2af
1 parent a9dfe08
commit 81bf2af
Show file tree

Hide file tree

Showing 4 changed files with 142 additions and 31 deletions.
diff --git a/...behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/...behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp
@@ -40,6 +40,8 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
+  bool isAlwaysExecutableModule() const override;
+
   bool isSimultaneousExecutableAsApprovedModule() const override;
 
   bool isSimultaneousExecutableAsCandidateModule() const override;

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
@@ -211,13 +211,32 @@ class SceneModuleManagerInterface
 
   bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; }
 
+  /**
+   * Determine if the module is always executable, regardless of the state of other modules.
+   *
+   * When this returns true:
+   * - The module can be executed even if other modules are not marked as 'simultaneously
+   * executable'.
+   * - Conversely, the presence of this module will not prevent other modules
+   *   from executing, even if they are not marked as 'simultaneously executable'.
+   */
+  virtual bool isAlwaysExecutableModule() const { return false; }
+
   virtual bool isSimultaneousExecutableAsApprovedModule() const
   {
+    if (isAlwaysExecutableModule()) {
+      return true;
+    }
+
     return enable_simultaneous_execution_as_approved_module_;
   }
 
   virtual bool isSimultaneousExecutableAsCandidateModule() const
   {
+    if (isAlwaysExecutableModule()) {
+      return true;
+    }
+
     return enable_simultaneous_execution_as_candidate_module_;
   }
 

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -188,22 +188,6 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
 
   std::vector<SceneModulePtr> request_modules{};
 
-  /**
-   * check whether it is possible to push back more modules to approved modules.
-   */
-  {
-    const auto find_block_module = [this](const auto & m) {
-      return !getManager(m)->isSimultaneousExecutableAsApprovedModule();
-    };
-
-    const auto itr =
-      std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module);
-
-    if (itr != approved_module_ptrs_.end()) {
-      return {};
-    }
-  }
-
   const auto toc = [this](const auto & name) {
     processing_time_.at(name) += stop_watch_.toc(name, true);
   };
@@ -212,14 +196,62 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
     stop_watch_.tic(manager_ptr->getModuleName());
 
     /**
-     * don't launch candidate module if approved modules already exist.
+     * determine the execution capability of modules based on existing approved modules.
      */
-    if (!approved_module_ptrs_.empty()) {
-      if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) {
-        toc(manager_ptr->getModuleName());
+    // Condition 1: always executable module can be added regardless of the existence of other
+    // modules, so skip checking the existence of other modules.
+    // in other cases, need to check the existence of other modules and which module can be added.
+    const bool has_non_always_executable_module = std::any_of(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+      [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); });
+    if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) {
+      // pairs of find_block_module and is_executable
+      std::vector<std::pair<std::function<bool(const SceneModulePtr &)>, std::function<bool()>>>
+        conditions;
+
+      // Condition 2: do not add modules that are neither always nor simultaneous executable
+      // if there exists at least one approved module that is simultaneous but not always
+      // executable. (only modules that are either always executable or simultaneous executable can
+      // be added)
+      conditions.push_back(
+        {[&](const SceneModulePtr & m) {
+           return !getManager(m)->isAlwaysExecutableModule() &&
+                  getManager(m)->isSimultaneousExecutableAsApprovedModule();
+         },
+         [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }});
+
+      // Condition 3: do not add modules that are not always executable if there exists
+      // at least one approved module that is neither always nor simultaneous executable.
+      // (only modules that are always executable can be added)
+      conditions.push_back(
+        {[&](const SceneModulePtr & m) {
+           return !getManager(m)->isAlwaysExecutableModule() &&
+                  !getManager(m)->isSimultaneousExecutableAsApprovedModule();
+         },
+         [&]() { return false; }});
+
+      bool skip_module = false;
+      for (const auto & condition : conditions) {
+        const auto & find_block_module = condition.first;
+        const auto & is_executable = condition.second;
+
+        const auto itr = std::find_if(
+          approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module);
+
+        if (itr != approved_module_ptrs_.end() && !is_executable()) {
+          toc(manager_ptr->getModuleName());
+          skip_module = true;
+          continue;
+        }
+      }
+      if (skip_module) {
         continue;
       }
     }
+    // else{
+    //   Condition 4: if none of the above conditions are met, any module can be added.
+    //   (when the approved modules are either empty or consist only of always executable modules.)
+    // }
 
     /**
      * launch new candidate module.
@@ -342,20 +374,58 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
    * remove non-executable modules.
    */
   for (const auto & module_ptr : sorted_request_modules) {
-    if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) {
-      if (executable_modules.empty()) {
-        executable_modules.push_back(module_ptr);
-        break;
-      }
+    // Condition 1: always executable module can be added regardless of the existence of other
+    // modules.
+    if (getManager(module_ptr)->isAlwaysExecutableModule()) {
+      executable_modules.push_back(module_ptr);
+      continue;
     }
 
-    const auto itr =
-      std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) {
-        return !getManager(m)->isSimultaneousExecutableAsCandidateModule();
-      });
-
-    if (itr == executable_modules.end()) {
+    // Condition 4: If the executable modules are either empty or consist only of always executable
+    // modules, any module can be added.
+    const bool has_non_always_executable_module = std::any_of(
+      executable_modules.begin(), executable_modules.end(),
+      [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); });
+    if (!has_non_always_executable_module) {
       executable_modules.push_back(module_ptr);
+      continue;
+    }
+
+    // pairs of find_block_module and is_executable
+    std::vector<std::pair<std::function<bool(const SceneModulePtr &)>, std::function<bool()>>>
+      conditions;
+
+    // Condition 3: Only modules that are always executable can be added
+    // if there exists at least one executable module that is neither always nor simultaneous
+    // executable.
+    conditions.push_back(
+      {[this](const SceneModulePtr & m) {
+         return !getManager(m)->isAlwaysExecutableModule() &&
+                !getManager(m)->isSimultaneousExecutableAsCandidateModule();
+       },
+       [&]() { return false; }});
+
+    // Condition 2: Only modules that are either always executable or simultaneous executable can be
+    // added if there exists at least one executable module that is simultaneous but not always
+    // executable.
+    conditions.push_back(
+      {[this](const SceneModulePtr & m) {
+         return !getManager(m)->isAlwaysExecutableModule() &&
+                getManager(m)->isSimultaneousExecutableAsCandidateModule();
+       },
+       [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }});
+
+    for (const auto & condition : conditions) {
+      const auto & find_block_module = condition.first;
+      const auto & is_executable = condition.second;
+
+      const auto itr =
+        std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module);
+
+      if (itr != executable_modules.end() && is_executable()) {
+        executable_modules.push_back(module_ptr);
+        break;
+      }
     }
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
@@ -262,8 +262,24 @@ void GoalPlannerModuleManager::updateModuleParams(
   });
 }
 
+bool GoalPlannerModuleManager::isAlwaysExecutableModule() const
+{
+  // enable AlwaysExecutable whenever goal modification is not allowed
+  // because only minor path refinements are made for fixed goals
+  if (!goal_planner_utils::isAllowedGoalModification(
+        planner_data_->route_handler, left_side_parking_)) {
+    return true;
+  }
+
+  return false;
+}
+
 bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const
 {
+  if (isAlwaysExecutableModule()) {
+    return true;
+  }
+
   // enable SimultaneousExecutable whenever goal modification is not allowed
   // because only minor path refinements are made for fixed goals
   if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
@@ -275,6 +291,10 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const
 
 bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const
 {
+  if (isAlwaysExecutableModule()) {
+    return true;
+  }
+
   // enable SimultaneousExecutable whenever goal modification is not allowed
   // because only minor path refinements are made for fixed goals
   if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {