From d8362377195947c6d99eb6f23964cfc5507bc04a Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
Date: Fri, 17 May 2024 19:15:00 +0900
Subject: [PATCH] perf(route_handler): avoid unnecessary copies and heavy loops
 (#7034)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
 planning/route_handler/src/route_handler.cpp | 62 +++++++-------------
 1 file changed, 21 insertions(+), 41 deletions(-)

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
index e9fb3cc7d2fd7..4dd559b9bbc98 100644
--- a/planning/route_handler/src/route_handler.cpp
+++ b/planning/route_handler/src/route_handler.cpp
@@ -573,44 +573,26 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo(
 
   lanelet::ConstLanelet current_lanelet = lanelet;
   double length = 0;
+  lanelet::ConstLanelets previous_lanelets;
   while (rclcpp::ok() && length < min_length) {
-    lanelet::ConstLanelets candidate_lanelets;
-    if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) {
-      if (only_route_lanes) {
-        break;
-      }
-      const auto prev_lanes = getPreviousLanelets(current_lanelet);
-      if (prev_lanes.empty()) {
-        break;
-      }
-      candidate_lanelets = prev_lanes;
+    previous_lanelets.clear();
+    if (!getPreviousLaneletsWithinRoute(current_lanelet, &previous_lanelets)) {
+      if (only_route_lanes) break;
+      const auto previous_lanelets = getPreviousLanelets(current_lanelet);
+      if (previous_lanelets.empty()) break;
     }
     // loop check
-    if (std::any_of(
-          candidate_lanelets.begin(), candidate_lanelets.end(),
-          [lanelet](auto & prev_llt) { return lanelet.id() == prev_llt.id(); })) {
-      break;
-    }
-
-    // If lanelet_sequence_backward with input lanelet contains all candidate lanelets,
-    // break the loop.
-    if (std::all_of(
-          candidate_lanelets.begin(), candidate_lanelets.end(),
-          [lanelet_sequence_backward, lanelet](auto & prev_llt) {
-            return std::any_of(
-              lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(),
-              [prev_llt, lanelet](auto & llt) {
-                return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id());
-              });
-          })) {
+    if (std::any_of(previous_lanelets.begin(), previous_lanelets.end(), [lanelet](auto & prev_llt) {
+          return lanelet.id() == prev_llt.id();
+        })) {
       break;
     }
 
-    for (const auto & prev_lanelet : candidate_lanelets) {
+    for (const auto & prev_lanelet : previous_lanelets) {
       if (std::any_of(
             lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(),
-            [prev_lanelet, lanelet](auto & llt) {
-              return (llt.id() == prev_lanelet.id() || lanelet.id() == prev_lanelet.id());
+            [prev_lanelet, lanelet](auto & backward) {
+              return (backward.id() == prev_lanelet.id());
             })) {
         continue;
       }
@@ -680,14 +662,13 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
   const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance,
   const double forward_distance, const bool only_route_lanes) const
 {
-  lanelet::ConstLanelets lanelet_sequence;
   if (only_route_lanes && !exists(route_lanelets_, lanelet)) {
-    return lanelet_sequence;
+    return {};
   }
 
   lanelet::ConstLanelets lanelet_sequence_forward =
     getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes);
-  const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() {
+  lanelet::ConstLanelets lanelet_sequence = std::invoke([&]() {
     const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose);
     if (arc_coordinate.length < backward_distance) {
       return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes);
@@ -696,17 +677,15 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
   });
 
   // loop check
-  if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) {
-    if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) {
+  if (!lanelet_sequence_forward.empty() && !lanelet_sequence.empty()) {
+    if (lanelet_sequence.back().id() == lanelet_sequence_forward.front().id()) {
       return lanelet_sequence_forward;
     }
   }
-  lanelet_sequence.insert(
-    lanelet_sequence.end(), lanelet_sequence_backward.begin(), lanelet_sequence_backward.end());
   lanelet_sequence.push_back(lanelet);
-  lanelet_sequence.insert(
-    lanelet_sequence.end(), lanelet_sequence_forward.begin(), lanelet_sequence_forward.end());
-
+  std::move(
+    lanelet_sequence_forward.begin(), lanelet_sequence_forward.end(),
+    std::back_inserter(lanelet_sequence));
   return lanelet_sequence;
 }
 
@@ -1681,8 +1660,9 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo(
   }
 
   lanelet::ConstLanelet current_lanelet = lanelet;
+  lanelet::ConstLanelets candidate_lanelets;
   while (rclcpp::ok()) {
-    lanelet::ConstLanelets candidate_lanelets;
+    candidate_lanelets.clear();
     if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) {
       break;
     }