Skip to content

Commit d836237

Browse files
authored
perf(route_handler): avoid unnecessary copies and heavy loops (#7034)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 90f50b7 commit d836237

File tree

1 file changed

+21
-41
lines changed

1 file changed

+21
-41
lines changed

planning/route_handler/src/route_handler.cpp

+21-41
Original file line numberDiff line numberDiff line change
@@ -573,44 +573,26 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo(
573573

574574
lanelet::ConstLanelet current_lanelet = lanelet;
575575
double length = 0;
576+
lanelet::ConstLanelets previous_lanelets;
576577
while (rclcpp::ok() && length < min_length) {
577-
lanelet::ConstLanelets candidate_lanelets;
578-
if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) {
579-
if (only_route_lanes) {
580-
break;
581-
}
582-
const auto prev_lanes = getPreviousLanelets(current_lanelet);
583-
if (prev_lanes.empty()) {
584-
break;
585-
}
586-
candidate_lanelets = prev_lanes;
578+
previous_lanelets.clear();
579+
if (!getPreviousLaneletsWithinRoute(current_lanelet, &previous_lanelets)) {
580+
if (only_route_lanes) break;
581+
const auto previous_lanelets = getPreviousLanelets(current_lanelet);
582+
if (previous_lanelets.empty()) break;
587583
}
588584
// loop check
589-
if (std::any_of(
590-
candidate_lanelets.begin(), candidate_lanelets.end(),
591-
[lanelet](auto & prev_llt) { return lanelet.id() == prev_llt.id(); })) {
592-
break;
593-
}
594-
595-
// If lanelet_sequence_backward with input lanelet contains all candidate lanelets,
596-
// break the loop.
597-
if (std::all_of(
598-
candidate_lanelets.begin(), candidate_lanelets.end(),
599-
[lanelet_sequence_backward, lanelet](auto & prev_llt) {
600-
return std::any_of(
601-
lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(),
602-
[prev_llt, lanelet](auto & llt) {
603-
return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id());
604-
});
605-
})) {
585+
if (std::any_of(previous_lanelets.begin(), previous_lanelets.end(), [lanelet](auto & prev_llt) {
586+
return lanelet.id() == prev_llt.id();
587+
})) {
606588
break;
607589
}
608590

609-
for (const auto & prev_lanelet : candidate_lanelets) {
591+
for (const auto & prev_lanelet : previous_lanelets) {
610592
if (std::any_of(
611593
lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(),
612-
[prev_lanelet, lanelet](auto & llt) {
613-
return (llt.id() == prev_lanelet.id() || lanelet.id() == prev_lanelet.id());
594+
[prev_lanelet, lanelet](auto & backward) {
595+
return (backward.id() == prev_lanelet.id());
614596
})) {
615597
continue;
616598
}
@@ -680,14 +662,13 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
680662
const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance,
681663
const double forward_distance, const bool only_route_lanes) const
682664
{
683-
lanelet::ConstLanelets lanelet_sequence;
684665
if (only_route_lanes && !exists(route_lanelets_, lanelet)) {
685-
return lanelet_sequence;
666+
return {};
686667
}
687668

688669
lanelet::ConstLanelets lanelet_sequence_forward =
689670
getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes);
690-
const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() {
671+
lanelet::ConstLanelets lanelet_sequence = std::invoke([&]() {
691672
const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose);
692673
if (arc_coordinate.length < backward_distance) {
693674
return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes);
@@ -696,17 +677,15 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
696677
});
697678

698679
// loop check
699-
if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) {
700-
if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) {
680+
if (!lanelet_sequence_forward.empty() && !lanelet_sequence.empty()) {
681+
if (lanelet_sequence.back().id() == lanelet_sequence_forward.front().id()) {
701682
return lanelet_sequence_forward;
702683
}
703684
}
704-
lanelet_sequence.insert(
705-
lanelet_sequence.end(), lanelet_sequence_backward.begin(), lanelet_sequence_backward.end());
706685
lanelet_sequence.push_back(lanelet);
707-
lanelet_sequence.insert(
708-
lanelet_sequence.end(), lanelet_sequence_forward.begin(), lanelet_sequence_forward.end());
709-
686+
std::move(
687+
lanelet_sequence_forward.begin(), lanelet_sequence_forward.end(),
688+
std::back_inserter(lanelet_sequence));
710689
return lanelet_sequence;
711690
}
712691

@@ -1681,8 +1660,9 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo(
16811660
}
16821661

16831662
lanelet::ConstLanelet current_lanelet = lanelet;
1663+
lanelet::ConstLanelets candidate_lanelets;
16841664
while (rclcpp::ok()) {
1685-
lanelet::ConstLanelets candidate_lanelets;
1665+
candidate_lanelets.clear();
16861666
if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) {
16871667
break;
16881668
}

0 commit comments

Comments
 (0)