From eb607d29f40d78c07225519e7796209a1f65a1f9 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 23 Jun 2024 15:37:06 +0900 Subject: [PATCH] fix(autoware_behavior_velocity_no_drivable_lane_module): fix containerOutOfBounds wawrning (#7631) * fix(autoware_behavior_velocity_no_drivable_lane_module): fix containerOutOfBounds wawrning Signed-off-by: Ryuta Kambe * style(pre-commit): autofix --------- Signed-off-by: Ryuta Kambe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/util.cpp | 50 +++++++++++-------- 1 file changed, 29 insertions(+), 21 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp index 1339fee207416..25315fd397aa4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -85,29 +85,37 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP bg::within(first_path_point, polygon); auto const & is_last_path_point_inside_polygon = bg::within(last_path_point, polygon); - if ( - intersects.empty() && - path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon && - is_last_path_point_inside_polygon) { - path_no_drivable_lane_polygon_intersection.is_path_inside_of_polygon = true; - } else { - // classify first and second intersection points - for (size_t i = 0; i < intersects.size(); ++i) { - const auto & p = intersects.at(i); - if ( - (intersects.size() == 2 && i == 0) || - (intersects.size() == 1 && is_last_path_point_inside_polygon)) { - path_no_drivable_lane_polygon_intersection.first_intersection_point = - createPoint(p.x(), p.y(), ego_pos.z); - } else if ( - (intersects.size() == 2 && i == 1) || - (intersects.size() == 1 && - path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon)) { - path_no_drivable_lane_polygon_intersection.second_intersection_point = - createPoint(p.x(), p.y(), ego_pos.z); - } + if (intersects.empty()) { + if ( + path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon && + is_last_path_point_inside_polygon) { + path_no_drivable_lane_polygon_intersection.is_path_inside_of_polygon = true; + } else { + // do nothing + } + } else if (intersects.size() == 1) { + const auto & p = intersects.at(0); + if (is_last_path_point_inside_polygon) { + path_no_drivable_lane_polygon_intersection.first_intersection_point = + createPoint(p.x(), p.y(), ego_pos.z); + } else if (path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon) { + path_no_drivable_lane_polygon_intersection.second_intersection_point = + createPoint(p.x(), p.y(), ego_pos.z); + } else { + // do nothing } + } else if (intersects.size() == 2) { + // classify first and second intersection points + const auto & p0 = intersects.at(0); + const auto & p1 = intersects.at(1); + path_no_drivable_lane_polygon_intersection.first_intersection_point = + createPoint(p0.x(), p0.y(), ego_pos.z); + path_no_drivable_lane_polygon_intersection.second_intersection_point = + createPoint(p1.x(), p1.y(), ego_pos.z); + } else { + // do nothing } + return path_no_drivable_lane_polygon_intersection; } } // namespace autoware::behavior_velocity_planner