@@ -630,32 +630,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
630
630
return output;
631
631
}
632
632
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);
642
638
643
- if (cond (* itr)) {
639
+ if (itr != modules. end ( )) {
644
640
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));
649
643
}
650
- }
651
- };
644
+ };
652
645
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;
657
660
};
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
+ }
659
665
}
660
666
661
667
// lock approved modules besides last one
@@ -768,6 +774,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
768
774
* remove success module immediately. if lane change module has succeeded, update root lanelet.
769
775
*/
770
776
{
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
+ };
771
796
const auto success_module_cond = [](const auto & m) {
772
797
return m->getCurrentStatus () == ModuleStatus::SUCCESS;
773
798
};
0 commit comments