From 0a351cc5fa1f395cf5673cf4c59ea8e27d3b7df9 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 14 May 2024 14:02:42 +0900 Subject: [PATCH 1/3] refactor straddle bound method Signed-off-by: Daniel Sanchez --- .../turn_signal_decider.hpp | 32 +++++++++---------- 1 file changed, 15 insertions(+), 17 deletions(-) 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..49091823c6f39 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,21 @@ 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); + return std::any_of( + lanes.begin(), lanes.end(), [&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)); + }); + }); }; inline bool isNearEndOfShift( From b6b94b5aedd7eb032734bef793b993f4043ec185 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 14 May 2024 15:23:35 +0900 Subject: [PATCH 2/3] remove parenthesis Signed-off-by: Daniel Sanchez --- .../behavior_path_planner_common/turn_signal_decider.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 49091823c6f39..cf0d177a7b0b1 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 @@ -238,9 +238,8 @@ class TurnSignalDecider lanes.begin(), lanes.end(), [&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 intersects(left_bound, shifted_vehicle_footprint) || + intersects(right_bound, shifted_vehicle_footprint); }); }); }; From a5e827e7ad0bf3098624e4f2ef08fee10d22b599 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 14 May 2024 16:26:35 +0900 Subject: [PATCH 3/3] add alias for inner lambda Signed-off-by: Daniel Sanchez --- .../behavior_path_planner_common/turn_signal_decider.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) 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 cf0d177a7b0b1..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 @@ -234,13 +234,16 @@ class TurnSignalDecider 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); - return std::any_of( - lanes.begin(), lanes.end(), [&shifted_vehicle_footprint](const auto & lane) { + + 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); }); };