Skip to content

Commit 0a351cc

Browse files
refactor straddle bound method
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent b6bd750 commit 0a351cc

File tree

1 file changed

+15
-17
lines changed

1 file changed

+15
-17
lines changed

planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp

+15-17
Original file line numberDiff line numberDiff line change
@@ -228,23 +228,21 @@ class TurnSignalDecider
228228
using tier4_autoware_utils::transformVector;
229229

230230
const auto footprint = vehicle_info.createFootprint();
231-
232-
for (const auto & lane : lanes) {
233-
for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
234-
const auto transform = pose2transform(path.path.points.at(i).point.pose);
235-
const auto shifted_vehicle_footprint = transformVector(footprint, transform);
236-
237-
if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
238-
return true;
239-
}
240-
241-
if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
242-
return true;
243-
}
244-
}
245-
}
246-
247-
return false;
231+
const auto start_itr = std::next(path.path.points.begin(), shift_line.start_idx);
232+
const auto end_itr = std::next(path.path.points.begin(), shift_line.end_idx);
233+
234+
return std::any_of(start_itr, end_itr, [&footprint, &lanes](const auto & point) {
235+
const auto transform = pose2transform(point.point.pose);
236+
const auto shifted_vehicle_footprint = transformVector(footprint, transform);
237+
return std::any_of(
238+
lanes.begin(), lanes.end(), [&shifted_vehicle_footprint](const auto & lane) {
239+
const auto & left_bound = lane.leftBound2d().basicLineString();
240+
const auto & right_bound = lane.rightBound2d().basicLineString();
241+
return (
242+
intersects(left_bound, shifted_vehicle_footprint) ||
243+
intersects(right_bound, shifted_vehicle_footprint));
244+
});
245+
});
248246
};
249247

250248
inline bool isNearEndOfShift(

0 commit comments

Comments
 (0)