fix(autoware_behavior_velocity_speed_bump_module): fix containerOutOf…

…Bounds warning (autowarefoundation#7671) fix(autoware_behavior_velocity_speed_bump_module): fix containerOutOfBounds Signed-off-by: Ryuta Kambe <[email protected]>
tby-udel · Jul 14, 2024 · c6e9966 · c6e9966
1 parent 90ef37c
commit c6e9966
Showing 1 changed file with 23 additions and 17 deletions.
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp
@@ -95,25 +95,31 @@ PathPolygonIntersectionStatus getPathPolygonIntersectionStatus(
   auto const & is_first_path_point_inside_polygon = bg::within(first_path_point, polygon);
   auto const & is_last_path_point_inside_polygon = bg::within(last_path_point, polygon);
 
-  if (
-    intersects.empty() && is_first_path_point_inside_polygon && is_last_path_point_inside_polygon) {
-    polygon_intersection_status.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)) {
-        polygon_intersection_status.first_intersection_point = createPoint(p.x(), p.y(), ego_pos.z);
-      } else if (
-        (intersects.size() == 2 && i == 1) ||
-        (intersects.size() == 1 && is_first_path_point_inside_polygon)) {
-        polygon_intersection_status.second_intersection_point =
-          createPoint(p.x(), p.y(), ego_pos.z);
-      }
+  // classify first and second intersection points
+  if (intersects.empty()) {
+    if (is_first_path_point_inside_polygon && is_last_path_point_inside_polygon) {
+      polygon_intersection_status.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) {
+      polygon_intersection_status.first_intersection_point = createPoint(p.x(), p.y(), ego_pos.z);
+    } else if (is_first_path_point_inside_polygon) {
+      polygon_intersection_status.second_intersection_point = createPoint(p.x(), p.y(), ego_pos.z);
+    } else {
+      // do nothing
     }
+  } else if (intersects.size() == 2) {
+    const auto & p0 = intersects.at(0);
+    const auto & p1 = intersects.at(1);
+    polygon_intersection_status.first_intersection_point = createPoint(p0.x(), p0.y(), ego_pos.z);
+    polygon_intersection_status.second_intersection_point = createPoint(p1.x(), p1.y(), ego_pos.z);
+  } else {
+    // do nothing
   }
+
   return polygon_intersection_status;
 }