perf(planner_manger): remove lifo limitation (autowarefoundation#4458)

* feat(route_handler): add function to get closest preferred lanelet Signed-off-by: satoshi-ota <[email protected]> * perf(planner_manager): remove module immediately once its status is success Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
tier4 · Sep 19, 2023 · 3348f16 · 3348f16
1 parent 72df177
commit 3348f16
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 74 deletions.
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
@@ -348,41 +348,21 @@ class PlannerManager
     candidate_module_ptrs_.clear();
   }
 
-  /**
-   * @brief stop and remove not RUNNING modules in candidate_module_ptrs_.
-   */
-  void clearNotRunningCandidateModules()
-  {
-    const auto it = std::remove_if(
-      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) {
-        if (m->getCurrentStatus() != ModuleStatus::RUNNING) {
-          deleteExpiredModules(m);
-          return true;
-        }
-        return false;
-      });
-    candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end());
-  }
-
-  /**
-   * @brief check if there is any RUNNING module in candidate_module_ptrs_.
-   */
-  bool hasAnyRunningCandidateModule()
-  {
-    return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) {
-      return m->getCurrentStatus() == ModuleStatus::RUNNING;
-    });
-  }
-
   /**
    * @brief get current root lanelet. the lanelet is used for reference path generation.
    * @param planner data.
    * @return root lanelet.
    */
-  lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr<PlannerData> & data) const
+  lanelet::ConstLanelet updateRootLanelet(
+    const std::shared_ptr<PlannerData> & data, bool success_lane_change = false) const
   {
     lanelet::ConstLanelet ret{};
-    data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret);
+    if (success_lane_change) {
+      data->route_handler->getClosestPreferredLaneletWithinRoute(
+        data->self_odometry->pose.pose, &ret);
+    } else {
+      data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret);
+    }
     RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id());
     return ret;
   }

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -509,8 +509,6 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
 
 BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<PlannerData> & data)
 {
-  const bool has_any_running_candidate_module = hasAnyRunningCandidateModule();
-
   std::unordered_map<std::string, BehaviorModuleOutput> results;
   BehaviorModuleOutput output = getReferencePath(data);
   results.emplace("root", output);
@@ -608,59 +606,28 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
     return results.at("root");
   }();
 
-  const auto not_success_itr = std::find_if(
-    approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(),
-    [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; });
-
-  // convert reverse iterator -> iterator
-  const auto success_itr = std::prev(not_success_itr).base() - 1;
-
   /**
-   * there is no succeeded module. return.
+   * remove success module immediately. if lane change module has succeeded, update root lanelet.
    */
-  if (success_itr == approved_module_ptrs_.end()) {
-    return approved_modules_output;
-  }
-
-  const auto lane_change_itr = std::find_if(
-    success_itr, approved_module_ptrs_.end(),
-    [](const auto & m) { return m->name().find("lane_change") != std::string::npos; });
-
-  /**
-   * remove success modules according to Last In First Out(LIFO) policy. when the next module is in
-   * ModuleStatus::RUNNING, the previous module keeps running even if it is in
-   * ModuleStatus::SUCCESS.
-   */
-  if (lane_change_itr == approved_module_ptrs_.end() && !has_any_running_candidate_module) {
-    std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) {
-      debug_info_.emplace_back(m, Action::DELETE, "From Approved");
-      deleteExpiredModules(m);
-    });
+  {
+    const auto success_module_itr = std::partition(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+      [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; });
 
-    approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end());
-    clearNotRunningCandidateModules();
+    const auto success_lane_change = std::any_of(
+      success_module_itr, approved_module_ptrs_.end(),
+      [](const auto & m) { return m->name().find("lane_change") != std::string::npos; });
 
-    return approved_modules_output;
-  }
+    if (success_lane_change) {
+      root_lanelet_ = updateRootLanelet(data, true);
+    }
 
-  /**
-   * as an exception, when there is lane change module is in succeeded modules, it doesn't remove
-   * any modules if module whose status is NOT ModuleStatus::SUCCESS exists. this is because the
-   * root lanelet is updated at the moment of lane change module's unregistering, and that causes
-   * change First In module's input.
-   */
-  if (not_success_itr == approved_module_ptrs_.rend() && !has_any_running_candidate_module) {
-    std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) {
+    std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) {
       debug_info_.emplace_back(m, Action::DELETE, "From Approved");
       deleteExpiredModules(m);
     });
 
-    approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end());
-    clearNotRunningCandidateModules();
-
-    root_lanelet_ = updateRootLanelet(data);
-
-    return approved_modules_output;
+    approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end());
   }
 
   return approved_modules_output;

diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -307,6 +307,8 @@ class RouteHandler
 
   bool getClosestLaneletWithinRoute(
     const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const;
+  bool getClosestPreferredLaneletWithinRoute(
+    const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const;
   bool getClosestLaneletWithConstrainsWithinRoute(
     const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
     const double yaw_threshold) const;

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
@@ -825,6 +825,13 @@ bool RouteHandler::getClosestLaneletWithinRoute(
   return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet);
 }
 
+bool RouteHandler::getClosestPreferredLaneletWithinRoute(
+  const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const
+{
+  return lanelet::utils::query::getClosestLanelet(
+    preferred_lanelets_, search_pose, closest_lanelet);
+}
+
 bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute(
   const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
   const double yaw_threshold) const