add empty polygon and point checks

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -339,7 +339,7 @@ std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPoly
 
       std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
       boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
-
+      if(lanelet_union_temp.empty()) continue;
       // Update merged_polygon by accumulating all merged results
       merged_polygon.clear();
       for (const auto & temp_poly : lanelet_union_temp) {

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -99,10 +99,6 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
       continue;
     }
 
-    if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) {
-      continue;
-    }
-
     // crop backward path
     // removes points which are out of lanes up to the start pose.
     // this ensures that the backward_path stays within the drivable area when starting from a
@@ -115,9 +111,15 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
     const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes(
       lanelet_map_ptr, shift_path, start_segment_idx);
 
+    if (cropped_path.points.empty()) continue;
+
     shift_path.points = cropped_path.points;
     shift_path.header = planner_data_->route_handler->getRouteHeader();
 
+    if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) {
+      continue;
+    }
+
     return pull_out_path;
   }