underflow check

Signed-off-by: Ryuta Kambe <[email protected]>
autowarefoundation · May 12, 2024 · 2e41fe1 · 2e41fe1
1 parent 9e76436
commit 2e41fe1
Showing 1 changed file with 12 additions and 0 deletions.
diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
@@ -78,6 +78,10 @@ boost::optional<LineString2d> NoStoppingAreaModule::getStopLineGeometry2d(
      *        ---------------
      **/
 
+    if (path.points.size() == 0) {
+      return {};
+    }
+
     for (const auto & no_stopping_area : no_stopping_area_reg_elem_.noStoppingAreas()) {
       const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon();
       lanelet::BasicLineString2d path_line;
@@ -254,6 +258,10 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea(
 {
   const double stop_vel = std::numeric_limits<float>::min();
   // stuck points by stop line
+  if (path.points.size() == 0) {
+    return false;
+  }
+
   for (size_t i = 0; i < path.points.size() - 1; ++i) {
     const auto p0 = path.points.at(i).point.pose.position;
     const auto p1 = path.points.at(i + 1).point.pose.position;
@@ -290,6 +298,10 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
     return ego_area;
   }
   auto & pp = interpolated_path.points;
+  if (pp.size() == 0) {
+    return ego_area;
+  }
+
   /* calc closest index */
   const auto closest_idx_opt =
     motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4);