fix(behavior_path_planner): fix invalid access for null drivable area…

… bound (#3758) * fix(behavior_path_planner): fix invalid access for null drivable area bound Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * fix message Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> --------- Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
autowarefoundation · May 18, 2023 · 632a2ff · 632a2ff
1 parent a8fecbd
commit 632a2ff
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -1376,6 +1376,14 @@ void generateDrivableArea(
   auto left_bound = calcBound(route_handler, lanes, enable_expanding_polygon, true);
   auto right_bound = calcBound(route_handler, lanes, enable_expanding_polygon, false);
 
+  if (left_bound.empty() || right_bound.empty()) {
+    auto clock{rclcpp::Clock{RCL_ROS_TIME}};
+    RCLCPP_ERROR_STREAM_THROTTLE(
+      rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000,
+      "The right or left bound of drivable area is empty");
+    return;
+  }
+
   // Insert points after goal
   lanelet::ConstLanelet goal_lanelet;
   if (