feat(planner_manager): limit iteration number by parameter (#5352)

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

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;

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -91,7 +91,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.
@@ -462,6 +462,8 @@ class PlannerManager
 
   mutable std::shared_ptr<SceneModuleVisitor> debug_msg_ptr_;
 
+  size_t max_iteration_num_{100};
+
   bool verbose_{false};
 };
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -130,7 +130,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     const std::lock_guard<std::mutex> lock(mutex_manager_);  // for planner_manager_
 
     const auto & p = planner_data_->parameters;
-    planner_manager_ = std::make_shared<PlannerManager>(*this, p.verbose);
+    planner_manager_ = std::make_shared<PlannerManager>(*this, p.max_iteration_num, p.verbose);
 
     const auto register_and_create_publisher = [&](const auto & manager) {
       const auto & module_name = manager->name();
@@ -277,6 +277,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   BehaviorPathPlannerParameters p{};
 
   p.verbose = declare_parameter<bool>("verbose");
+  p.max_iteration_num = declare_parameter<int>("max_iteration_num");
 
   const auto get_scene_module_manager_param = [&](std::string && ns) {
     ModuleConfigParameters config;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -27,9 +27,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);
@@ -79,7 +81,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
       return output;
     }
 
-    while (rclcpp::ok()) {
+    for (size_t itr_num = 0;; ++itr_num) {
       /**
        * STEP1: get approved modules' output
        */
@@ -125,8 +127,17 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & 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(