From 2e41fe1ca62a067ecb066bde07a8d5d7c10b0ac9 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 12 May 2024 13:39:38 +0900 Subject: [PATCH] underflow check Signed-off-by: Ryuta Kambe --- .../src/scene_no_stopping_area.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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 index e5e583e1e6f9c..3b44ec001e71c 100644 --- 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 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::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);