remove collision check in pull out planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -169,11 +169,10 @@ class StartPlannerModule : public SceneModuleInterface
   bool isMoving() const;
 
   PriorityOrder determinePriorityOrder(
-    const std::string & search_priority, const size_t candidates_size);
+    const std::string & search_priority, const size_t start_pose_candidates_num);
   bool findPullOutPath(
-    const std::vector<Pose> & start_pose_candidates, const size_t index,
-    const std::shared_ptr<PullOutPlannerBase> & planner, const Pose & refined_start_pose,
-    const Pose & goal_pose);
+    const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
+    const Pose & refined_start_pose, const Pose & goal_pose);
   void updateStatusWithCurrentPath(
     const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
     const behavior_path_planner::PlannerType & planner_type);

planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -69,11 +69,6 @@ std::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, const
     utils::path_safety_checker::separateObjectsByLanelets(
       stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
 
-  if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-        vehicle_footprint_, arc_path, pull_out_lane_stop_objects,
-        parameters_.collision_check_margin)) {
-    return {};
-  }
   const double velocity = parallel_parking_parameters_.forward_parking_velocity;
 
   if (parameters_.divide_pull_out_path) {

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -46,7 +46,6 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
 {
   const auto & route_handler = planner_data_->route_handler;
   const auto & common_parameters = planner_data_->parameters;
-  const auto & dynamic_objects = planner_data_->dynamic_object;
 
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
@@ -64,13 +63,6 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
     return std::nullopt;
   }
 
-  // extract stop objects in pull out lane for collision check
-  const auto [pull_out_lane_objects, others] =
-    utils::path_safety_checker::separateObjectsByLanelets(
-      *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
-  const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
-    pull_out_lane_objects, parameters_.th_moving_object_velocity);
-
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {
     auto & shift_path =
@@ -143,13 +135,6 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
       continue;
     }
 
-    // check collision
-    if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-          vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects,
-          parameters_.collision_check_margin)) {
-      continue;
-    }
-
     shift_path.header = planner_data_->route_handler->getRouteHeader();
 
     return pull_out_path;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -585,25 +585,25 @@ void StartPlannerModule::planWithPriority(
     determinePriorityOrder(search_priority, start_pose_candidates.size());
 
   for (const auto & [index, planner] : order_priority) {
-    if (findPullOutPath(start_pose_candidates, index, planner, refined_start_pose, goal_pose))
+    if (findPullOutPath(start_pose_candidates[index], planner, refined_start_pose, goal_pose))
       return;
   }
 
   updateStatusIfNoSafePathFound();
 }
 
 PriorityOrder StartPlannerModule::determinePriorityOrder(
-  const std::string & search_priority, const size_t candidates_size)
+  const std::string & search_priority, const size_t start_pose_candidates_num)
 {
   PriorityOrder order_priority;
   if (search_priority == "efficient_path") {
     for (const auto & planner : start_planners_) {
-      for (size_t i = 0; i < candidates_size; i++) {
+      for (size_t i = 0; i < start_pose_candidates_num; i++) {
         order_priority.emplace_back(i, planner);
       }
     }
   } else if (search_priority == "short_back_distance") {
-    for (size_t i = 0; i < candidates_size; i++) {
+    for (size_t i = 0; i < start_pose_candidates_num; i++) {
       for (const auto & planner : start_planners_) {
         order_priority.emplace_back(i, planner);
       }
@@ -616,43 +616,62 @@ PriorityOrder StartPlannerModule::determinePriorityOrder(
 }
 
 bool StartPlannerModule::findPullOutPath(
-  const std::vector<Pose> & start_pose_candidates, const size_t index,
-  const std::shared_ptr<PullOutPlannerBase> & planner, const Pose & refined_start_pose,
-  const Pose & goal_pose)
+  const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
+  const Pose & refined_start_pose, const Pose & goal_pose)
 {
-  // Ensure the index is within the bounds of the start_pose_candidates vector
-  if (index >= start_pose_candidates.size()) return false;
+  const auto & dynamic_objects = planner_data_->dynamic_object;
+  const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
+    planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
+  const auto & vehicle_footprint = createVehicleFootprint(vehicle_info_);
+  // extract stop objects in pull out lane for collision check
+  const auto [pull_out_lane_objects, others] =
+    utils::path_safety_checker::separateObjectsByLanelets(
+      *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+  const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
+    pull_out_lane_objects, parameters_->th_moving_object_velocity);
 
-  const Pose & pull_out_start_pose = start_pose_candidates.at(index);
-  const bool is_driving_forward =
-    tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01;
+  // if start_pose_candidate is far from refined_start_pose, backward driving is necessary
+  const bool backward_is_unnecessary =
+    tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01;
 
   planner->setPlannerData(planner_data_);
-  const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose);
+  const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose);
 
   // If no path is found, return false
   if (!pull_out_path) {
     return false;
   }
 
-  // If driving forward, update status with the current path and return true
-  if (is_driving_forward) {
-    updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner->getPlannerType());
-    return true;
-  }
+  // lambda function for combining partial_paths
+  const auto combine_partial_path = [pull_out_lanes](const auto & path) {
+    PathWithLaneId combined_path;
+    for (const auto & partial_path : path.partial_paths) {
+      combined_path.points.insert(
+        combined_path.points.end(), partial_path.points.begin(), partial_path.points.end());
+    }
+    // remove the point behind of shift end pose
+    const auto shift_end_pose_idx =
+      motion_utils::findNearestIndex(combined_path.points, path.end_pose);
+    combined_path.points.erase(
+      combined_path.points.begin() + *shift_end_pose_idx + 1, combined_path.points.end());
+    return combined_path;
+  };
 
-  // If this is the last start pose candidate, return false
-  if (index == start_pose_candidates.size() - 1) return false;
+  // check collision
+  if (utils::checkCollisionBetweenPathFootprintsAndObjects(
+        vehicle_footprint, combine_partial_path(*pull_out_path), pull_out_lane_stop_objects,
+        parameters_->collision_check_margin)) {
+    return false;
+  }
 
-  const Pose & next_pull_out_start_pose = start_pose_candidates.at(index + 1);
-  const auto next_pull_out_path = planner->plan(next_pull_out_start_pose, goal_pose);
+  // If start pose candidate
+  if (backward_is_unnecessary) {
+    updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType());
+    return true;
+  }
 
-  // If no next path is found, return false
-  if (!next_pull_out_path) return false;
+  updateStatusWithNextPath(*pull_out_path, start_pose_candidate, planner->getPlannerType());
 
-  // Update status with the next path and return true
-  updateStatusWithNextPath(
-    *next_pull_out_path, next_pull_out_start_pose, planner->getPlannerType());
   return true;
 }
 
@@ -775,13 +794,9 @@ std::vector<DrivableLanes> StartPlannerModule::generateDrivableLanes(
 
 void StartPlannerModule::updatePullOutStatus()
 {
-  const bool has_received_new_route =
-    !planner_data_->prev_route_id ||
-    *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();
-
   // skip updating if enough time has not passed for preventing chattering between back and
   // start_planner
-  if (!has_received_new_route) {
+  if (!receivedNewRoute()) {
     if (!last_pull_out_start_update_time_) {
       last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
     }
@@ -871,13 +886,9 @@ 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 =
@@ -907,12 +918,6 @@ 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;

planning/behavior_path_start_planner_module/src/util.cpp

@@ -92,7 +92,6 @@ lanelet::ConstLanelets getPullOutLanes(
   const auto start_pose = planner_data->route_handler->getOriginalStartPose();
 
   lanelet::ConstLanelet current_shoulder_lane;
-  lanelet::ConstLanelets shoulder_lanes;
   if (route_handler->getPullOutStartLane(
         route_handler->getShoulderLanelets(), start_pose, vehicle_width, &current_shoulder_lane)) {
     // pull out from shoulder lane