refactor(goal_planner): divide sort function (#9650)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -159,6 +159,12 @@ bool isStopped(
   const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower,
   const double velocity_upper);
 
+void sortPullOverPaths(
+  const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
+  const std::vector<PullOverPath> & pull_over_path_candidates,
+  const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects,
+  rclcpp::Logger logger, std::vector<size_t> & sorted_path_indices);
+
 // Flag class for managing whether a certain callback is running in multi-threading
 class ScopedFlag
 {

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -981,63 +981,14 @@ BehaviorModuleOutput GoalPlannerModule::plan()
   return fixed_goal_planner_->plan(planner_data_);
 }
 
-std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
-  const PullOverContextData & context_data,
+void sortPullOverPaths(
+  const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
   const std::vector<PullOverPath> & pull_over_path_candidates,
-  const GoalCandidates & goal_candidates) const
+  const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects,
+  rclcpp::Logger logger, std::vector<size_t> & sorted_path_indices)
 {
-  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
-
-  const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose();
-  const double backward_length =
-    parameters_->backward_goal_search_length + parameters_->decide_path_distance;
-  const auto & prev_module_output_path = getPreviousModuleOutput().path;
-  const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins;
-  const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins;
-
-  // STEP1: Filter valid paths before sorting
-  // NOTE: Since copying pull over path takes time, it is handled by indices
-  std::vector<size_t> sorted_path_indices;
-  sorted_path_indices.reserve(pull_over_path_candidates.size());
-
-  // STEP1-1: Extract paths which have safe goal
-  // Create a map of goal_id to GoalCandidate for quick access
-  std::unordered_map<int, GoalCandidate> goal_candidate_map;
-  for (const auto & goal_candidate : goal_candidates) {
-    goal_candidate_map[goal_candidate.id] = goal_candidate;
-  }
-  for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) {
-    const auto & path = pull_over_path_candidates[i];
-    const auto goal_candidate_it = goal_candidate_map.find(path.goal_id());
-    if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) {
-      sorted_path_indices.push_back(i);
-    }
-  }
-
-  // STEP1-2: Remove paths which do not have enough distance
-  const double prev_path_front_to_goal_dist = calcSignedArcLength(
-    prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position,
-    goal_pose.position);
-  const auto & long_tail_reference_path = [&]() {
-    if (prev_path_front_to_goal_dist > backward_length) {
-      return prev_module_output_path;
-    }
-    // get road lanes which is at least backward_length[m] behind the goal
-    const auto road_lanes = utils::getExtendedCurrentLanesFromPath(
-      prev_module_output_path, planner_data_, backward_length, 0.0, false);
-    const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
-    return planner_data_->route_handler->getCenterLinePath(
-      road_lanes, std::max(0.0, goal_pose_length - backward_length),
-      goal_pose_length + parameters_->forward_goal_search_length);
-  }();
-
-  sorted_path_indices.erase(
-    std::remove_if(
-      sorted_path_indices.begin(), sorted_path_indices.end(),
-      [&](const size_t i) {
-        return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path);
-      }),
-    sorted_path_indices.end());
+  const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins;
+  const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins;
 
   // STEP2: Sort pull over path candidates
   // STEP2-1: Sort pull_over_path_candidates based on the order in goal_candidates
@@ -1065,14 +1016,14 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
 
   // compare to sort pull_over_path_candidates based on the order in efficient_path_order
   const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
-    const auto & order = parameters_->efficient_path_order;
+    const auto & order = parameters.efficient_path_order;
     const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type()));
     const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type()));
     return a_pos < b_pos;
   };
 
   // if object recognition is enabled, sort by collision check margin
-  if (parameters_->use_object_recognition) {
+  if (parameters.use_object_recognition) {
     // STEP2-2: Sort by collision check margins
     const auto [margins, margins_with_zero] =
       std::invoke([&]() -> std::tuple<std::vector<double>, std::vector<double>> {
@@ -1085,11 +1036,11 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
 
     // Create a map of PullOverPath pointer to largest collision check margin
     std::map<size_t, double> path_id_to_rough_margin_map;
-    const auto & target_objects = context_data.static_target_objects;
+    const auto & target_objects = static_target_objects;
     for (const size_t i : sorted_path_indices) {
       const auto & path = pull_over_path_candidates[i];
       const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects(
-        path.parking_path(), target_objects, planner_data_->parameters, false, "max");
+        path.parking_path(), target_objects, planner_data->parameters, false, "max");
       auto it = std::lower_bound(
         margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater<double>());
       if (it == margins_with_zero.end()) {
@@ -1119,7 +1070,7 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
     // STEP2-3: Sort by curvature
     // If the curvature is less than the threshold, prioritize the path.
     const auto isHighCurvature = [&](const PullOverPath & path) -> bool {
-      return path.parking_path_max_curvature() >= parameters_->high_curvature_threshold;
+      return path.parking_path_max_curvature() >= parameters.high_curvature_threshold;
     };
 
     const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
@@ -1155,7 +1106,7 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
 
     // STEP2-4: Sort pull_over_path_candidates based on the order in efficient_path_order keeping
     // the collision check margin and curvature priority.
-    if (parameters_->path_priority == "efficient_path") {
+    if (parameters.path_priority == "efficient_path") {
       std::stable_sort(
         sorted_path_indices.begin(), sorted_path_indices.end(),
         [&](const size_t a_i, const size_t b_i) {
@@ -1182,14 +1133,14 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
     const std::string path_priority_info_str = goal_planner_utils::makePathPriorityDebugMessage(
       sorted_path_indices, pull_over_path_candidates, goal_id_to_index, goal_candidates,
       path_id_to_rough_margin_map, isSoftMargin, isHighCurvature);
-    RCLCPP_DEBUG_STREAM(getLogger(), path_priority_info_str);
+    RCLCPP_DEBUG_STREAM(logger, path_priority_info_str);
   } else {
     /**
      * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the
      * future. sort by curvature is not implemented yet.
      * Sort pull_over_path_candidates based on the order in efficient_path_order
      */
-    if (parameters_->path_priority == "efficient_path") {
+    if (parameters.path_priority == "efficient_path") {
       std::stable_sort(
         sorted_path_indices.begin(), sorted_path_indices.end(),
         [&](const size_t a_i, const size_t b_i) {
@@ -1202,6 +1153,67 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
         });
     }
   }
+}
+
+std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
+  const PullOverContextData & context_data,
+  const std::vector<PullOverPath> & pull_over_path_candidates,
+  const GoalCandidates & goal_candidates) const
+{
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
+  const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose();
+  const double backward_length =
+    parameters_->backward_goal_search_length + parameters_->decide_path_distance;
+  const auto & prev_module_output_path = getPreviousModuleOutput().path;
+
+  // STEP1: Filter valid paths before sorting
+  // NOTE: Since copying pull over path takes time, it is handled by indices
+  std::vector<size_t> sorted_path_indices;
+  sorted_path_indices.reserve(pull_over_path_candidates.size());
+
+  // STEP1-1: Extract paths which have safe goal
+  // Create a map of goal_id to GoalCandidate for quick access
+  std::unordered_map<int, GoalCandidate> goal_candidate_map;
+  for (const auto & goal_candidate : goal_candidates) {
+    goal_candidate_map[goal_candidate.id] = goal_candidate;
+  }
+  for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) {
+    const auto & path = pull_over_path_candidates[i];
+    const auto goal_candidate_it = goal_candidate_map.find(path.goal_id());
+    if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) {
+      sorted_path_indices.push_back(i);
+    }
+  }
+
+  // STEP1-2: Remove paths which do not have enough distance
+  const double prev_path_front_to_goal_dist = calcSignedArcLength(
+    prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position,
+    goal_pose.position);
+  const auto & long_tail_reference_path = [&]() {
+    if (prev_path_front_to_goal_dist > backward_length) {
+      return prev_module_output_path;
+    }
+    // get road lanes which is at least backward_length[m] behind the goal
+    const auto road_lanes = utils::getExtendedCurrentLanesFromPath(
+      prev_module_output_path, planner_data_, backward_length, 0.0, false);
+    const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
+    return planner_data_->route_handler->getCenterLinePath(
+      road_lanes, std::max(0.0, goal_pose_length - backward_length),
+      goal_pose_length + parameters_->forward_goal_search_length);
+  }();
+
+  sorted_path_indices.erase(
+    std::remove_if(
+      sorted_path_indices.begin(), sorted_path_indices.end(),
+      [&](const size_t i) {
+        return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path);
+      }),
+    sorted_path_indices.end());
+
+  sortPullOverPaths(
+    planner_data_, *parameters_, pull_over_path_candidates, goal_candidates,
+    context_data.static_target_objects, getLogger(), sorted_path_indices);
 
   // STEP3: Select the final pull over path by checking collision to make it as high priority as
   // possible