Skip to content

Commit 0a057e1

Browse files
authored
fix(behavior_path_planner): sort keep last modules considering priority (#6174)
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent 34a7c2d commit 0a057e1

File tree

1 file changed

+46
-21
lines changed

1 file changed

+46
-21
lines changed

planning/behavior_path_planner/src/planner_manager.cpp

+46-21
Original file line numberDiff line numberDiff line change
@@ -630,32 +630,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
630630
return output;
631631
}
632632

633-
const auto move_to_end = [](auto & modules, const auto & cond) {
634-
auto itr = modules.begin();
635-
while (itr != modules.end()) {
636-
const auto satisfied_exit_cond =
637-
std::all_of(itr, modules.end(), [&cond](const auto & m) { return cond(m); });
638-
639-
if (satisfied_exit_cond) {
640-
return;
641-
}
633+
// move modules whose keep last flag is true to end of the approved_module_ptrs_.
634+
// if there are multiple keep last modules, sort by priority
635+
{
636+
const auto move_to_end = [](auto & modules, const auto & module_to_move) {
637+
auto itr = std::find(modules.begin(), modules.end(), module_to_move);
642638

643-
if (cond(*itr)) {
639+
if (itr != modules.end()) {
644640
auto tmp = std::move(*itr);
645-
itr = modules.erase(itr);
646-
modules.insert(modules.end(), std::move(tmp));
647-
} else {
648-
itr++;
641+
modules.erase(itr);
642+
modules.push_back(std::move(tmp));
649643
}
650-
}
651-
};
644+
};
652645

653-
// move modules whose keep last flag is true to end of the approved_module_ptrs_.
654-
{
655-
const auto keep_last_module_cond = [this](const auto & m) {
656-
return getManager(m)->isKeepLast();
646+
const auto get_sorted_keep_last_modules = [this](const auto & modules) {
647+
std::vector<SceneModulePtr> keep_last_modules;
648+
649+
std::copy_if(
650+
modules.begin(), modules.end(), std::back_inserter(keep_last_modules),
651+
[this](const auto & m) { return getManager(m)->isKeepLast(); });
652+
653+
// sort by priority (low -> high)
654+
std::sort(
655+
keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) {
656+
return getManager(a)->getPriority() < getManager(b)->getPriority();
657+
});
658+
659+
return keep_last_modules;
657660
};
658-
move_to_end(approved_module_ptrs_, keep_last_module_cond);
661+
662+
for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) {
663+
move_to_end(approved_module_ptrs_, module);
664+
}
659665
}
660666

661667
// lock approved modules besides last one
@@ -768,6 +774,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
768774
* remove success module immediately. if lane change module has succeeded, update root lanelet.
769775
*/
770776
{
777+
const auto move_to_end = [](auto & modules, const auto & cond) {
778+
auto itr = modules.begin();
779+
while (itr != modules.end()) {
780+
const auto satisfied_exit_cond =
781+
std::all_of(itr, modules.end(), [&cond](const auto & m) { return cond(m); });
782+
783+
if (satisfied_exit_cond) {
784+
return;
785+
}
786+
787+
if (cond(*itr)) {
788+
auto tmp = std::move(*itr);
789+
itr = modules.erase(itr);
790+
modules.insert(modules.end(), std::move(tmp));
791+
} else {
792+
itr++;
793+
}
794+
}
795+
};
771796
const auto success_module_cond = [](const auto & m) {
772797
return m->getCurrentStatus() == ModuleStatus::SUCCESS;
773798
};

0 commit comments

Comments
 (0)