check collision when search start pose

Signed-off-by: kyoichi-sugahara <[email protected]>

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -886,9 +886,13 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
 {
   std::vector<Pose> pull_out_start_pose_candidates{};
   const auto start_pose = planner_data_->route_handler->getOriginalStartPose();
+  const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
 
+  const auto stop_objects_in_pull_out_lanes =
+    filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity);
+
   // Set the maximum backward distance less than the distance from the vehicle's base_link to the
   // lane's rearmost point to prevent lane departure.
   const double current_arc_length =
@@ -918,6 +922,12 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
       continue;
     }
 
+    if (utils::checkCollisionBetweenFootprintAndObjects(
+          local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes,
+          parameters_->collision_check_margin)) {
+      break;  // poses behind this has a collision, so break.
+    }
+
     pull_out_start_pose_candidates.push_back(*backed_pose);
   }
   return pull_out_start_pose_candidates;