feat(start_planner): add a sanity check if the path is empty (#6643)

add a sanity check if the path is empty

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

Author: danielsanchezaran

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp

@@ -83,11 +83,14 @@ class PullOutPlannerBase
     utils::path_safety_checker::filterObjectsByClass(
       pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation);
 
-    return utils::checkCollisionBetweenPathFootprintsAndObjects(
-      vehicle_footprint_,
+    const auto collision_check_section_path =
       behavior_path_planner::start_planner_utils::extractCollisionCheckSection(
-        pull_out_path, collision_check_distance_from_end),
-      pull_out_lane_stop_objects, collision_check_margin_);
+        pull_out_path, collision_check_distance_from_end);
+    if (!collision_check_section_path) return true;
+
+    return utils::checkCollisionBetweenPathFootprintsAndObjects(
+      vehicle_footprint_, collision_check_section_path.value(), pull_out_lane_stop_objects,
+      collision_check_margin_);
   };
   std::shared_ptr<const PlannerData> planner_data_;
   vehicle_info_util::VehicleInfo vehicle_info_;

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp

@@ -51,7 +51,7 @@ lanelet::ConstLanelets getPullOutLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length);
 Pose getBackedPose(
   const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance);
-PathWithLaneId extractCollisionCheckSection(
+std::optional<PathWithLaneId> extractCollisionCheckSection(
   const PullOutPath & path, const double collision_check_distance_from_end);
 }  // namespace behavior_path_planner::start_planner_utils
 

planning/behavior_path_start_planner_module/src/util.cpp

@@ -104,7 +104,7 @@ lanelet::ConstLanelets getPullOutLanes(
     /*forward_only_in_route*/ true);
 }
 
-PathWithLaneId extractCollisionCheckSection(
+std::optional<PathWithLaneId> extractCollisionCheckSection(
   const PullOutPath & path, const double collision_check_distance_from_end)
 {
   PathWithLaneId full_path;
@@ -113,6 +113,7 @@ PathWithLaneId extractCollisionCheckSection(
       full_path.points.end(), partial_path.points.begin(), partial_path.points.end());
   }
 
+  if (full_path.points.empty()) return std::nullopt;
   // Find the start index for collision check section based on the shift start pose
   const auto shift_start_idx =
     motion_utils::findNearestIndex(full_path.points, path.start_pose.position);