From f4f4a1480548b703afd2ddf4e99ba846aa97a781 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 19 Sep 2023 20:19:50 +0900 Subject: [PATCH] perf(planner_manger): remove lifo limitation (#4458) (#855) * feat(route_handler): add function to get closest preferred lanelet * perf(planner_manager): remove module immediately once its status is success --------- Signed-off-by: satoshi-ota Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../behavior_path_planner/planner_manager.hpp | 36 +++-------- .../src/planner_manager.cpp | 59 ++++--------------- .../include/route_handler/route_handler.hpp | 2 + planning/route_handler/src/route_handler.cpp | 7 +++ 4 files changed, 30 insertions(+), 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 index 8820ef70c37bf..6ca2ecf638da6 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 @@ -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 & data) const + lanelet::ConstLanelet updateRootLanelet( + const std::shared_ptr & 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 index 3e9411b141fb3..d3ee7bcc093ec 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -509,8 +509,6 @@ std::pair PlannerManager::runRequestModule BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) { - const bool has_any_running_candidate_module = hasAnyRunningCandidateModule(); - std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); results.emplace("root", output); @@ -608,59 +606,28 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() != 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 index 061953f30005c..9c36c8bec30e7 100644 --- 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 index 7c2b4c2af7974..b4e7be025dcaf 100644 --- 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