feat(behavior_path_planner): allow reroute when always executable module running or candidate modules is running (#5786)

feat(behavior_path_planner): allow reroute when always executable module running or candidate modules

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -173,7 +173,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief publish reroute availability
    */
-  void publish_reroute_availability();
+  void publish_reroute_availability() const;
 
   /**
    * @brief publish steering factor from intersection

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -226,6 +226,13 @@ class PlannerManager
    */
   bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); }
 
+  bool hasNonAlwaysExecutableApprovedModules() const
+  {
+    return std::any_of(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+      [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); });
+  }
+
   /**
    * @brief check if there are candidate modules.
    */

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -551,18 +551,14 @@ void BehaviorPathPlannerNode::publish_steering_factor(
   steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now());
 }
 
-void BehaviorPathPlannerNode::publish_reroute_availability()
+void BehaviorPathPlannerNode::publish_reroute_availability() const
 {
-  const bool has_approved_modules = planner_manager_->hasApprovedModules();
-  const bool has_candidate_modules = planner_manager_->hasCandidateModules();
-
-  // In the current behavior path planner, we might get unexpected behavior when rerouting while
-  // modules other than lane follow are active. Therefore, rerouting will be allowed only when the
-  // lane follow module is running Note that if there is a approved module or candidate module, it
-  // means non-lane-following modules are runnning.
+  // In the current behavior path planner, we might encounter unexpected behavior when rerouting
+  // while modules other than lane following are active. If non-lane-following module except
+  // always-executable module is approved and running, rerouting will not be　possible.
   RerouteAvailability is_reroute_available;
   is_reroute_available.stamp = this->now();
-  if (has_approved_modules || has_candidate_modules) {
+  if (planner_manager_->hasNonAlwaysExecutableApprovedModules()) {
     is_reroute_available.availability = false;
   } else {
     is_reroute_available.availability = true;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -263,10 +263,7 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
     // 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) {
+    if (!manager_ptr->isAlwaysExecutableModule() && hasNonAlwaysExecutableApprovedModules()) {
       // pairs of find_block_module and is_executable
       std::vector<std::pair<std::function<bool(const SceneModulePtr &)>, std::function<bool()>>>
         conditions;