From e28e11b82ef36b59382625f2ea4549306fe1cf0d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 19 Oct 2023 16:17:36 +0900 Subject: [PATCH] feat(planner_manager): limit iteration number by parameter (#5352) Signed-off-by: satoshi-ota --- .../config/behavior_path_planner.param.yaml | 1 + .../behavior_path_planner/parameters.hpp | 1 + .../behavior_path_planner/planner_manager.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 3 ++- .../src/planner_manager.cpp | 17 ++++++++++++++--- 5 files changed, 21 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 95c60612bfdb7..d333cbfd778cb 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: verbose: false + max_iteration_num: 100 planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index e3a3784c5e928..3319feae7008d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -77,6 +77,7 @@ struct LateralAccelerationMap struct BehaviorPathPlannerParameters { bool verbose; + size_t max_iteration_num{100}; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; 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 589ccebf5b0cb..941bd56145942 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 @@ -93,7 +93,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const bool verbose); + PlannerManager(rclcpp::Node & node, const size_t max_iteration_num, const bool verbose); /** * @brief run all candidate and approved modules. @@ -433,6 +433,8 @@ class PlannerManager mutable std::shared_ptr debug_msg_ptr_; + size_t max_iteration_num_{100}; + bool verbose_{false}; }; } // namespace behavior_path_planner 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 1fa05d0bafea8..35b7e07ad657f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -136,7 +136,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.verbose); + planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); const auto register_and_create_publisher = [&](const auto & manager) { const auto & module_name = manager->name(); @@ -282,6 +282,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() BehaviorPathPlannerParameters p{}; p.verbose = declare_parameter("verbose"); + p.max_iteration_num = declare_parameter("max_iteration_num"); const auto get_scene_module_manager_param = [&](std::string && ns) { ModuleConfigParameters config; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 7a7ee046ca069..a0f3d300cfc5e 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -28,9 +28,11 @@ namespace behavior_path_planner { -PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) +PlannerManager::PlannerManager( + rclcpp::Node & node, const size_t max_iteration_num, const bool verbose) : logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), + max_iteration_num_{max_iteration_num}, verbose_{verbose} { processing_time_.emplace("total_time", 0.0); @@ -80,7 +82,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da return output; } - while (rclcpp::ok()) { + for (size_t itr_num = 0;; ++itr_num) { /** * STEP1: get approved modules' output */ @@ -126,8 +128,17 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da addApprovedModule(highest_priority_module); clearCandidateModules(); debug_info_.emplace_back(highest_priority_module, Action::ADD, "To Approval"); + + if (itr_num >= max_iteration_num_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", + max_iteration_num_); + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return candidate_modules_output; + } } - return BehaviorModuleOutput{}; + + return BehaviorModuleOutput{}; // something wrong. }(); std::for_each(