diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 01ad67e45d39d..c47dc70213d1a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/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 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 index 638dcfe520289..7ceafb6f428de 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/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. */ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3aceedcbde2ba..907c5226c3908 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 5c5c675d64b88..c867bffc8e9bd 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -263,10 +263,7 @@ std::vector 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::function>> conditions;