diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index fe187567cd893..29fe05775739e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/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(