Skip to content

Commit

Permalink
perf(planner_manger): remove lifo limitation (autowarefoundation#4458) (
Browse files Browse the repository at this point in the history
#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 <[email protected]>
Co-authored-by: Satoshi OTA <[email protected]>
  • Loading branch information
kosuke55 and satoshi-ota authored Sep 19, 2023
1 parent 2e52a29 commit f4f4a14
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 74 deletions.
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
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
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
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

0 comments on commit f4f4a14

Please sign in to comment.