fix(goal_planner): fix freespace planning chattering (autowarefoundation#8981)

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

Author: kosuke55

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

@@ -141,13 +141,7 @@ 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

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1492,6 +1492,8 @@ 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_,
@@ -1540,6 +1542,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 
   // return to lane parking if it is possible
   if (
+    is_freespace &&
     thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
     canReturnToLaneParking()) {
     thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

@@ -576,61 +576,6 @@ std::vector<Polygon2d> createPathFootPrints(
   return footprints;
 }
 
-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)
-{
-  std::stringstream ss;
-
-  // Unsafe goal and its priority are not visible as debug marker in rviz,
-  // so exclude unsafe goal from goal_priority
-  std::map<size_t, int> goal_id_and_priority;
-  for (size_t i = 0; i < goal_candidates.size(); ++i) {
-    goal_id_and_priority[goal_candidates[i].id] = goal_candidates[i].is_safe ? i : -1;
-  }
-
-  ss << "\n---------------------- path priority ----------------------\n";
-  for (size_t i = 0; i < sorted_path_indices.size(); ++i) {
-    const auto & path = pull_over_path_candidates[sorted_path_indices[i]];
-    // 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: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe")
-       << ", margin: " << path_id_to_rough_margin_map.at(path.id)
-       << (isSoftMargin(path) ? " (soft)" : " (hard)")
-       << ", curvature: " << path.getParkingPathMaxCurvature()
-       << (isHighCurvature(path) ? " (high)" : " (low)") << "\n";
-  }
-  ss << "-----------------------------------------------------------\n";
-  return ss.str();
-}
-
-lanelet::Points3d combineLanePoints(
-  const lanelet::Points3d & points, const lanelet::Points3d & points_next)
-{
-  lanelet::Points3d combined_points;
-  std::unordered_set<lanelet::Id> point_ids;
-  for (const auto & point : points) {
-    if (point_ids.insert(point.id()).second) {
-      combined_points.push_back(point);
-    }
-  }
-  for (const auto & point : points_next) {
-    if (point_ids.insert(point.id()).second) {
-      combined_points.push_back(point);
-    }
-  }
-  return combined_points;
-}
-
 lanelet::Lanelet createDepartureCheckLanelet(
   const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
   const bool left_side_parking)