feat(autoware_behavior_path_planner): prevent infinite loop in approving scene module process

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

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.
@@ -111,6 +111,17 @@ class PlannerManager
    */
   void launchScenePlugin(rclcpp::Node & node, const std::string & name);
 
+  /**
+   * @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
+   *
+   */
+  void calculateMaxIterationNum(const size_t scene_module_num);
+
   /**
    * @brief unregister managers.
    * @param node.
@@ -396,11 +407,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<PlannerData> & data);
+  BehaviorModuleOutput runApprovedModules(
+    const std::shared_ptr<PlannerData> & data, std::vector<SceneModulePtr> & deleted_modules);
 
   /**
    * @brief select a module that should be execute at first.
@@ -420,10 +433,12 @@ class PlannerManager
   /**
    * @brief get all modules that make execution request.
    * @param decided (=approved) path.
+   * @param deleted modules.
    * @return request modules.
    */
   std::vector<SceneModulePtr> getRequestModules(
-    const BehaviorModuleOutput & previous_module_output) const;
+    const BehaviorModuleOutput & previous_module_output,
+    const std::vector<SceneModulePtr> & deleted_modules) const;
 
   /**
    * @brief checks whether a path of trajectory has forward driving direction

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -70,16 +70,18 @@ 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.max_iteration_num);
+    planner_manager_ = std::make_shared<PlannerManager>(*this);
 
+    size_t scene_module_num = 0;
     for (const auto & name : declare_parameter<std::vector<std::string>>("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(
@@ -147,7 +149,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
 {
   BehaviorPathPlannerParameters p{};
 
-  p.max_iteration_num = declare_parameter<int>("max_iteration_num");
   p.traffic_light_signal_timeout = declare_parameter<double>("traffic_light_signal_timeout");
 
   // vehicle info

planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -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<DebugPublisher>(&node, "~/debug");
@@ -66,6 +65,11 @@
   }
 }
 
+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) {
@@ -127,17 +131,19 @@
         "Ego is out of route, no module is running. Skip running scene modules.");
       return output;
     }
+    std::vector<SceneModulePtr>
+      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,24 +256,37 @@
 }
 
 std::vector<SceneModulePtr> PlannerManager::getRequestModules(
-  const BehaviorModuleOutput & previous_module_output) const
+  const BehaviorModuleOutput & previous_module_output,
+  const std::vector<SceneModulePtr> & deleted_modules) const
 {
   if (previous_module_output.path.points.empty()) {
     RCLCPP_ERROR_STREAM(
       logger_, "Current module output is null. Skip candidate module check."
                  << "\n      - Approved  module list: " << getNames(approved_module_ptrs_)
                  << "\n      - Candidate module list: " << getNames(candidate_module_ptrs_));
     return {};
   }
 
   std::vector<SceneModulePtr> request_modules{};
 
   const auto toc = [this](const auto & name) {
     processing_time_.at(name) += stop_watch_.toc(name, true);
   };
 
   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,193 +674,200 @@
   return std::make_pair(module_ptr, results.at(module_ptr->name()));
 }
 
-BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<PlannerData> & data)
+BehaviorModuleOutput PlannerManager::runApprovedModules(
+  const std::shared_ptr<PlannerData> & data, std::vector<SceneModulePtr> & deleted_modules)
 {
   std::unordered_map<std::string, BehaviorModuleOutput> results;
   BehaviorModuleOutput output = getReferencePath(data);
   results.emplace("root", output);
 
   if (approved_module_ptrs_.empty()) {
     return output;
   }
 
   // move modules whose keep last flag is true to end of the approved_module_ptrs_.
   // if there are multiple keep last modules, sort by priority
   {
     const auto move_to_end = [](auto & modules, const auto & module_to_move) {
       auto itr = std::find(modules.begin(), modules.end(), module_to_move);
 
       if (itr != modules.end()) {
         auto tmp = std::move(*itr);
         modules.erase(itr);
         modules.push_back(std::move(tmp));
       }
     };
 
     const auto get_sorted_keep_last_modules = [this](const auto & modules) {
       std::vector<SceneModulePtr> keep_last_modules;
 
       std::copy_if(
         modules.begin(), modules.end(), std::back_inserter(keep_last_modules),
         [this](const auto & m) { return getManager(m)->isKeepLast(); });
 
       // sort by priority (low -> high)
       std::sort(
         keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) {
           return getManager(a)->getPriority() < getManager(b)->getPriority();
         });
 
       return keep_last_modules;
     };
 
     for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) {
       move_to_end(approved_module_ptrs_, module);
     }
   }
 
   // lock approved modules besides last one
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
     m->lockOutputPath();
   });
 
   // unlock only last approved module except keep last module.
   {
     const auto not_keep_last_modules = std::find_if(
       approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
       [this](const auto & m) { return !getManager(m)->isKeepLast(); });
 
     if (not_keep_last_modules != approved_module_ptrs_.rend()) {
       (*not_keep_last_modules)->unlockOutputPath();
     }
   }
 
   /**
    * execute approved modules except keep last modules.
    */
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
     if (!getManager(m)->isKeepLast()) {
       output = run(m, data, output);
       results.emplace(m->name(), output);
     }
   });
 
   /**
    * pop waiting approve module. push it back candidate modules. if waiting approve module is found
    * in iteration std::find_if, not only the module but also the next one are removed from
    * approved_module_ptrs_ since the next module's input (= waiting approve module's output) maybe
    * change significantly. And, only the waiting approve module is pushed back to
    * candidate_module_ptrs_.
    */
   {
     const auto not_keep_last_module = std::find_if(
       approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
       [this](const auto & m) { return !getManager(m)->isKeepLast(); });
 
     // convert reverse iterator -> iterator
     const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend()
                              ? std::next(not_keep_last_module).base()
                              : approved_module_ptrs_.begin();
 
     const auto waiting_approval_modules_itr = std::find_if(
       begin_itr, approved_module_ptrs_.end(),
       [](const auto & m) { return m->isWaitingApproval(); });
 
     if (waiting_approval_modules_itr != approved_module_ptrs_.end()) {
       clearCandidateModules();
       candidate_module_ptrs_.push_back(*waiting_approval_modules_itr);
 
       debug_info_.emplace_back(
         *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval");
 
       std::for_each(
         waiting_approval_modules_itr, approved_module_ptrs_.end(),
         [&results](const auto & m) { results.erase(m->name()); });
 
       approved_module_ptrs_.erase(waiting_approval_modules_itr);
 
       std::for_each(
         manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
     }
   }
 
   /**
    * remove failure modules. these modules' outputs are discarded as invalid plan.
    */
   {
     const auto itr = std::find_if(
       approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
       [](const auto & m) { return m->getCurrentStatus() == 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");
       deleteExpiredModules(m);
     });
 
     std::for_each(
       manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
 
     if (itr != approved_module_ptrs_.end()) {
       clearCandidateModules();
     }
 
     approved_module_ptrs_.erase(itr, approved_module_ptrs_.end());
   }
 
   if (approved_module_ptrs_.empty()) {
     return results.at("root");
   }
 
   /**
    * use the last module's output as approved modules planning result.
    */
   const auto approved_modules_output = [&results, this]() {
     const auto itr = std::find_if(
       approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
       [&results](const auto & m) { return results.count(m->name()) != 0; });
 
     if (itr != approved_module_ptrs_.rend()) {
       return results.at((*itr)->name());
     }
     return results.at("root");
   }();
 
   /**
    * remove success module immediately. if lane change module has succeeded, update current route
    * lanelet.
    */
   {
     const auto move_to_end = [](auto & modules, const auto & cond) {
       auto itr = modules.begin();
       while (itr != modules.end()) {
         const auto satisfied_exit_cond =
           std::all_of(itr, modules.end(), [&cond](const auto & m) { return cond(m); });
 
         if (satisfied_exit_cond) {
           return;
         }
 
         if (cond(*itr)) {
           auto tmp = std::move(*itr);
           itr = modules.erase(itr);
           modules.insert(modules.end(), std::move(tmp));
         } else {
           itr++;
         }
       }
     };
     const auto success_module_cond = [](const auto & m) {
       return m->getCurrentStatus() == ModuleStatus::SUCCESS;
     };
     move_to_end(approved_module_ptrs_, success_module_cond);
 
     const auto itr =
       std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), success_module_cond);
 
     if (std::any_of(itr, approved_module_ptrs_.end(), [](const auto & m) {
           return m->isCurrentRouteLaneletToBeReset();
         }))
       resetCurrentRouteLanelet(data);
 
+    std::copy_if(
+      itr, approved_module_ptrs_.end(), std::back_inserter(deleted_modules), success_module_cond);
+
     std::for_each(itr, approved_module_ptrs_.end(), [&](auto & m) {
       debug_info_.emplace_back(m, Action::DELETE, "From Approved");
       deleteExpiredModules(m);