From e0357cdd8628e526f42f06a6435fdac39011a413 Mon Sep 17 00:00:00 2001
From: TetsuKawa <kawaguchitnon@icloud.com>
Date: Fri, 6 Dec 2024 19:24:13 +0900
Subject: [PATCH] Revert "fix(goal_planner): fix freespace planning chattering
 (#8981)"

This reverts commit 2113aa19790c3e9e13db739c6267ed4b09d575ab.
---
 .../autoware/behavior_path_goal_planner_module/util.hpp   | 8 +++++++-
 .../src/goal_planner_module.cpp                           | 4 +---
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
index ba3f79c09a889..3e4e08007f515 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
@@ -109,7 +109,13 @@ MarkerArray createLaneletPolygonMarkerArray(
 MarkerArray createNumObjectsToAvoidTextsMarkerArray(
   const GoalCandidates & goal_candidates, std::string && ns,
   const std_msgs::msg::ColorRGBA & color);
-
+std::string makePathPriorityDebugMessage(
+  const std::vector<size_t> & sorted_path_indices,
+  const std::vector<PullOverPath> & pull_over_path_candidates,
+  const std::map<size_t, size_t> & goal_id_to_index, const GoalCandidates & goal_candidates,
+  const std::map<size_t, double> & path_id_to_rough_margin_map,
+  const std::function<bool(const PullOverPath &)> & isSoftMargin,
+  const std::function<bool(const PullOverPath &)> & isHighCurvature);
 /**
  * @brief combine two points
  * @param points lane points
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
index 70127a390e743..cfe5c797d7130 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1456,8 +1456,6 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
     return getPreviousModuleOutput();
   }
 
-  const bool is_freespace =
-    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE;
   if (
     hasNotDecidedPath(
       planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_,
@@ -1504,7 +1502,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 
   // return to lane parking if it is possible
   if (
-    is_freespace && thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
+    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
     canReturnToLaneParking()) {
     thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
   }