fix(autoware_behavior_velocity_no_drivable_lane_module): fix containe…

…rOutOfBounds wawrning (#7631) * fix(autoware_behavior_velocity_no_drivable_lane_module): fix containerOutOfBounds wawrning Signed-off-by: Ryuta Kambe <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Ryuta Kambe <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jun 23, 2024 · 30ecaad · 30ecaad
1 parent ca8cb30
commit 30ecaad
Showing 1 changed file with 29 additions and 21 deletions.
diff --git a/...behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp b/...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