refactor(turn_signal_decider): straddle bound method (autowarefoundat…

…ion#7006) * refactor straddle bound method Signed-off-by: Daniel Sanchez <[email protected]> * remove parenthesis Signed-off-by: Daniel Sanchez <[email protected]> * add alias for inner lambda Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
Interplai · Jun 3, 2024 · 39ced77 · 39ced77
1 parent 3ce0802
commit 39ced77
Showing 1 changed file with 17 additions and 17 deletions.
diff --git a/...behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/...behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
@@ -228,23 +228,23 @@ class TurnSignalDecider
     using tier4_autoware_utils::transformVector;
 
     const auto footprint = vehicle_info.createFootprint();
-
-    for (const auto & lane : lanes) {
-      for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
-        const auto transform = pose2transform(path.path.points.at(i).point.pose);
-        const auto shifted_vehicle_footprint = transformVector(footprint, transform);
-
-        if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
-          return true;
-        }
-
-        if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
-          return true;
-        }
-      }
-    }
-
-    return false;
+    const auto start_itr = std::next(path.path.points.begin(), shift_line.start_idx);
+    const auto end_itr = std::next(path.path.points.begin(), shift_line.end_idx);
+
+    return std::any_of(start_itr, end_itr, [&footprint, &lanes](const auto & point) {
+      const auto transform = pose2transform(point.point.pose);
+      const auto shifted_vehicle_footprint = transformVector(footprint, transform);
+
+      auto check_for_vehicle_and_bound_intersection =
+        [&shifted_vehicle_footprint](const auto & lane) {
+          const auto & left_bound = lane.leftBound2d().basicLineString();
+          const auto & right_bound = lane.rightBound2d().basicLineString();
+          return intersects(left_bound, shifted_vehicle_footprint) ||
+                 intersects(right_bound, shifted_vehicle_footprint);
+        };
+
+      return std::any_of(lanes.begin(), lanes.end(), check_for_vehicle_and_bound_intersection);
+    });
   };
 
   inline bool isNearEndOfShift(