From 75e0613fb3527bd07087f3b7cd40cb76c86bb147 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 4 Jul 2024 12:29:08 +0900 Subject: [PATCH 1/6] prevent infinite loop Signed-off-by: kyoichi-sugahara --- .../behavior_path_planner/planner_manager.hpp | 10 +++++-- .../src/planner_manager.cpp | 30 ++++++++++++++++--- 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index a76284b70b7d8..be146ef3f52e9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -396,11 +396,13 @@ class PlannerManager * @brief run all modules in approved_module_ptrs_ and get a planning result as * approved_modules_output. * @param planner data. + * @param deleted modules. * @return valid planning result. * @details in this function, expired modules (ModuleStatus::FAILURE or ModuleStatus::SUCCESS) are - * removed from approved_module_ptrs_. + * removed from approved_module_ptrs_ and added to deleted_modules. */ - BehaviorModuleOutput runApprovedModules(const std::shared_ptr & data); + BehaviorModuleOutput runApprovedModules( + const std::shared_ptr & data, std::vector & deleted_modules); /** * @brief select a module that should be execute at first. @@ -420,10 +422,12 @@ class PlannerManager /** * @brief get all modules that make execution request. * @param decided (=approved) path. + * @param deleted modules. * @return request modules. */ std::vector getRequestModules( - const BehaviorModuleOutput & previous_module_output) const; + const BehaviorModuleOutput & previous_module_output, + const std::vector & deleted_modules) const; /** * @brief checks whether a path of trajectory has forward driving direction diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 05dcc2163d14a..a47252b668648 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -127,17 +127,19 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da "Ego is out of route, no module is running. Skip running scene modules."); return output; } + std::vector + deleted_modules; // store the scene modules deleted from approved modules for (size_t itr_num = 1;; ++itr_num) { /** * STEP1: get approved modules' output */ - auto approved_modules_output = runApprovedModules(data); + auto approved_modules_output = runApprovedModules(data, deleted_modules); /** * STEP2: check modules that need to be launched */ - const auto request_modules = getRequestModules(approved_modules_output); + const auto request_modules = getRequestModules(approved_modules_output, deleted_modules); /** * STEP3: if there is no module that need to be launched, return approved modules' output @@ -250,7 +252,8 @@ void PlannerManager::generateCombinedDrivableArea( } std::vector PlannerManager::getRequestModules( - const BehaviorModuleOutput & previous_module_output) const + const BehaviorModuleOutput & previous_module_output, + const std::vector & deleted_modules) const { if (previous_module_output.path.points.empty()) { RCLCPP_ERROR_STREAM( @@ -268,6 +271,18 @@ std::vector PlannerManager::getRequestModules( for (const auto & manager_ptr : manager_ptrs_) { stop_watch_.tic(manager_ptr->name()); + /** + * skip the module that is already deleted. + */ + { + const auto name = manager_ptr->name(); + const auto find_deleted_module = [&name](const auto & m) { return m->name() == name; }; + const auto itr = + std::find_if(deleted_modules.begin(), deleted_modules.end(), find_deleted_module); + if (itr != deleted_modules.end()) { + continue; + } + } /** * determine the execution capability of modules based on existing approved modules. @@ -655,7 +670,8 @@ std::pair PlannerManager::runRequestModule return std::make_pair(module_ptr, results.at(module_ptr->name())); } -BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) +BehaviorModuleOutput PlannerManager::runApprovedModules( + const std::shared_ptr & data, std::vector & deleted_modules) { std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); @@ -771,6 +787,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() == ModuleStatus::FAILURE; }); + if (itr != approved_module_ptrs_.end()) { + deleted_modules.push_back(*itr); + } std::for_each(itr, approved_module_ptrs_.end(), [this](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); @@ -842,6 +861,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr Date: Mon, 8 Jul 2024 20:17:50 +0900 Subject: [PATCH 2/6] calculate max_iteration_num from number of scene modules Signed-off-by: kyoichi-sugahara --- .../autoware/behavior_path_planner/planner_manager.hpp | 7 +++++++ .../src/behavior_path_planner_node.cpp | 3 +++ .../autoware_behavior_path_planner/src/planner_manager.cpp | 6 ++++++ 3 files changed, 16 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index be146ef3f52e9..75209f961451b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -111,6 +111,13 @@ class PlannerManager */ void launchScenePlugin(rclcpp::Node & node, const std::string & name); + /** + * @brief set max iteration numbers + * @param number of scene module + * + */ + void calculateMaxIterationNum(const size_t scene_module_num); + /** * @brief unregister managers. * @param node. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 0e586211bc9ee..33a3c58d7c588 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -73,13 +73,16 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto & p = planner_data_->parameters; planner_manager_ = std::make_shared(*this, p.max_iteration_num); + size_t scene_module_num = 0; for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. if (name == "") { break; } planner_manager_->launchScenePlugin(*this, name); + scene_module_num++; } + planner_manager_->calculateMaxIterationNum(scene_module_num); for (const auto & manager : planner_manager_->getSceneModuleManagers()) { path_candidate_publishers_.emplace( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index a47252b668648..6d22be19c7920 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -66,6 +66,12 @@ void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & } } +void PlannerManager::calculateMaxIterationNum(const size_t scene_module_num) +{ + max_iteration_num_ = scene_module_num * (scene_module_num + 1) / 2; + +} + void PlannerManager::removeScenePlugin(rclcpp::Node & node, const std::string & name) { auto it = std::remove_if(manager_ptrs_.begin(), manager_ptrs_.end(), [&](const auto plugin) { From f3055b6b04d2c3d8eddb0f4e34cedc71b18d21e3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 8 Jul 2024 11:20:04 +0000 Subject: [PATCH 3/6] style(pre-commit): autofix --- .../autoware_behavior_path_planner/src/planner_manager.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 6d22be19c7920..3eec6bf3e69f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -69,7 +69,6 @@ void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & void PlannerManager::calculateMaxIterationNum(const size_t scene_module_num) { max_iteration_num_ = scene_module_num * (scene_module_num + 1) / 2; - } void PlannerManager::removeScenePlugin(rclcpp::Node & node, const std::string & name) From d2c8977939e8bf8fa19663eb4fa3d1407c251d5a Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 9 Jul 2024 12:12:21 +0900 Subject: [PATCH 4/6] feat: Remove max_iteration_num parameter from PlannerManager constructor Signed-off-by: kyoichi-sugahara --- .../config/behavior_path_planner.param.yaml | 1 - .../autoware/behavior_path_planner/planner_manager.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 4 +--- .../autoware_behavior_path_planner/src/planner_manager.cpp | 5 ++--- 4 files changed, 4 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml index e5c3a76fd1c45..7d5249dea689f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: - max_iteration_num: 100 traffic_light_signal_timeout: 1.0 planning_hz: 10.0 backward_path_length: 5.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 75209f961451b..d81c1c6fbb95c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -96,7 +96,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const size_t max_iteration_num); + explicit PlannerManager(rclcpp::Node & node); /** * @brief run all candidate and approved modules. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 33a3c58d7c588..e92fe937708b8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -70,8 +70,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ - const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.max_iteration_num); + planner_manager_ = std::make_shared(*this); size_t scene_module_num = 0; for (const auto & name : declare_parameter>("launch_modules")) { @@ -150,7 +149,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; - p.max_iteration_num = declare_parameter("max_iteration_num"); p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); // vehicle info diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 3eec6bf3e69f6..5278ea8e76981 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -30,13 +30,12 @@ namespace autoware::behavior_path_planner { -PlannerManager::PlannerManager(rclcpp::Node & node, const size_t max_iteration_num) +PlannerManager::PlannerManager(rclcpp::Node & node) : plugin_loader_( "autoware_behavior_path_planner", "autoware::behavior_path_planner::SceneModuleManagerInterface"), logger_(node.get_logger().get_child("planner_manager")), - clock_(*node.get_clock()), - max_iteration_num_{max_iteration_num} + clock_(*node.get_clock()) { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); From e215ce5bfe80afd9e474dc0997d2a92b245c252d Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 9 Jul 2024 16:49:03 +0900 Subject: [PATCH 5/6] feat: Calculate max iteration numbers in PlannerManager Signed-off-by: kyoichi-sugahara --- .../include/autoware/behavior_path_planner/planner_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index d81c1c6fbb95c..424b503525c9c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -112,7 +112,7 @@ class PlannerManager void launchScenePlugin(rclcpp::Node & node, const std::string & name); /** - * @brief set max iteration numbers + * @brief calculate max iteration numbers * @param number of scene module * */ From 10320d14b7e0830750b37f2aec086a2377541027 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 9 Jul 2024 17:45:30 +0900 Subject: [PATCH 6/6] add doxygen explanation for calculateMaxIterationNum Signed-off-by: kyoichi-sugahara --- .../autoware/behavior_path_planner/planner_manager.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 424b503525c9c..1386af7525422 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -112,7 +112,11 @@ class PlannerManager void launchScenePlugin(rclcpp::Node & node, const std::string & name); /** - * @brief calculate max iteration numbers + * @brief calculate max iteration numbers. + * Let N be the number of scene modules. The maximum number of iterations executed in a loop is N, + * but after that, if there are any modules that have succeeded or failed, the approve_modules of + * all modules are cleared, and the loop is executed for N-1 modules. As this process repeats, it + * becomes N + (N-1) + (N-2) + … + 1, therefore the maximum number of iterations is N(N+1)/2. * @param number of scene module * */