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

* prevent infinite loop

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* calculate max_iteration_num from number of scene modules

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add doxygen explanation for calculateMaxIterationNum

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

planning/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/include/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.
@@ -393,11 +404,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.
@@ -417,10 +430,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/src/behavior_path_planner_node.cpp

@@ -132,16 +132,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(
@@ -208,7 +210,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/src/planner_manager.cpp

@@ -30,11 +30,10 @@
 
 namespace behavior_path_planner
 {
-PlannerManager::PlannerManager(rclcpp::Node & node, const size_t max_iteration_num)
+PlannerManager::PlannerManager(rclcpp::Node & node)
 : plugin_loader_("behavior_path_planner", "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");
@@ -64,6 +63,11 @@ 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) {
@@ -122,17 +126,19 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
         "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
@@ -243,7 +249,8 @@ void PlannerManager::generateCombinedDrivableArea(
 }
 
 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(
@@ -261,6 +268,18 @@ std::vector<SceneModulePtr> 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.
@@ -635,7 +654,8 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
   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);
@@ -751,6 +771,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
     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");
@@ -822,6 +845,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
         }))
       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);