Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 1 commit into from
Sep 19, 2023
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 8 additions & 28 deletions planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
59 changes: 13 additions & 46 deletions planning/behavior_path_planner/src/planner_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions planning/route_handler/include/route_handler/route_handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
7 changes: 7 additions & 0 deletions planning/route_handler/src/route_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading