fix

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -833,20 +833,27 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
       std::stringstream ss;
 
       // unsafe goal and it's priority are not visible as debug marker in rviz,
-      // so subtract of unsafe goal from goal_priority(index)
-      const size_t num_of_unsafe_goals = std::count_if(
-        goal_candidates.begin(), goal_candidates.end(),
-        [](const auto & goal_candidate) { return !goal_candidate.is_safe; });
+      // so exclude unsafe goal from goal_priority
+      std::map<size_t, int> goal_id_and_priority;
+      {
+        int priority = 0;
+        for (const auto & goal_candidate : goal_candidates) {
+          goal_id_and_priority[goal_candidate.id] = goal_candidate.is_safe ? priority++ : -1;
+        }
+      }
 
       ss << "\n---------------------- path priority ----------------------\n";
       for (size_t i = 0; i < sorted_pull_over_path_candidates.size(); ++i) {
         const auto & path = sorted_pull_over_path_candidates[i];
-        const int goal_priority =
-          (static_cast<int>(goal_id_to_index.at(path.goal_id)) -
-           static_cast<int>(num_of_unsafe_goals));
+
+        // goal_index is same to goal priority including unsafe goal
+        const int goal_index = static_cast<int>(goal_id_to_index.at(path.goal_id));
+        const bool is_safe_goal = goal_candidates[goal_index].is_safe;
+        const int goal_priority = goal_id_and_priority[path.goal_id];
+
         ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type)
            << ", path_id: " << path.id << ", goal_id: " << path.goal_id
-           << ", goal_priority:" << goal_priority
+           << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe")
            << ", margin: " << path_id_to_margin_map.at(path.id)
            << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.max_curvature
            << (isHighCurvature(path) ? " (high)" : " (low)");
@@ -938,38 +945,39 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
       return path.max_curvature >= parameters_->high_curvature_threshold;
     };
 
+    const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
+      const double margin = path_id_to_margin_map[path.id];
+      return std::any_of(
+        soft_margins.begin(), soft_margins.end(),
+        [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; });
+    };
+    const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
+      return !isSoftMargin(a) && !isSoftMargin(b) &&
+             std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01;
+    };
+
     std::stable_sort(
       sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(),
       [&](const PullOverPath & a, const PullOverPath & b) {
-        return !isHighCurvature(a) && isHighCurvature(b);
+        // if both are soft margin or both are same hard margin, prioritize the path with lower
+        // curvature.
+        if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) {
+          return !isHighCurvature(a) && isHighCurvature(b);
+        }
+        // otherwise, keep the order based on the margin.
+        return false;
       });
 
     // (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") {
-      const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
-        const double margin = path_id_to_margin_map[path.id];
-        return std::any_of(
-          soft_margins.begin(), soft_margins.end(),
-          [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; });
-      };
-      const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
-        return !isSoftMargin(a) && !isSoftMargin(b) &&
-               std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01;
-      };
-
       std::stable_sort(
         sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(),
         [&](const auto & a, const auto & b) {
           // if any of following conditions are met, sort by path type priority
           // - both are soft margin
           // - both are same hard margin
-          // - both are high curvature
-          // - both are low curvature
-          if (
-            (isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b) ||
-            (isHighCurvature(a) && isHighCurvature(b)) ||
-            (!isHighCurvature(a) && !isHighCurvature(b))) {
+          if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) {
             return comparePathTypePriority(a, b);
           }
           // otherwise, keep the order.