From 4c1fa747b314766c9e5806e5d2ac4d01ee10268c Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Fri, 23 Dec 2022 16:18:17 +0900
Subject: [PATCH] fix(behaivior_path_planner): remove unused util function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../behavior_path_planner/utilities.hpp       | 10 ----
 .../behavior_path_planner/src/utilities.cpp   | 58 -------------------
 2 files changed, 68 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
index ff7ba51828ef8..92c7b5ab244e0 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
@@ -260,13 +260,6 @@ std::vector<size_t> filterObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & lanelets,
   const double start_arc_length, const double end_arc_length);
 
-/**
- * @brief Get index of the obstacles inside the lanelets
- * @return Indices corresponding to the obstacle inside the lanelets
- */
-std::vector<size_t> filterObjectIndicesByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
-
 /**
  * @brief Separate index of the obstacles into two part based on whether the object is within
  * lanelet.
@@ -276,9 +269,6 @@ std::vector<size_t> filterObjectIndicesByLanelets(
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
 
-PredictedObjects filterObjectsByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
-
 /**
  * @brief Separate the objects into two part based on whether the object is within lanelet.
  * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet.
diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
index 37df76b21a896..bb34339e75f86 100644
--- a/planning/behavior_path_planner/src/utilities.cpp
+++ b/planning/behavior_path_planner/src/utilities.cpp
@@ -552,52 +552,6 @@ std::vector<size_t> filterObjectIndicesByLanelets(
   return indices;
 }
 
-// works with random lanelets
-std::vector<size_t> filterObjectIndicesByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
-{
-  std::vector<size_t> indices;
-  if (target_lanelets.empty()) {
-    return {};
-  }
-
-  for (size_t i = 0; i < objects.objects.size(); i++) {
-    // create object polygon
-    const auto & obj = objects.objects.at(i);
-    // create object polygon
-    Polygon2d obj_polygon;
-    if (!calcObjectPolygon(obj, &obj_polygon)) {
-      RCLCPP_ERROR_STREAM(
-        rclcpp::get_logger("behavior_path_planner").get_child("utilities"),
-        "Failed to calcObjectPolygon...!!!");
-      continue;
-    }
-
-    for (const auto & llt : target_lanelets) {
-      // create lanelet polygon
-      const auto polygon2d = llt.polygon2d().basicPolygon();
-      if (polygon2d.empty()) {
-        // no lanelet polygon
-        continue;
-      }
-      Polygon2d lanelet_polygon;
-      lanelet_polygon.outer().reserve(polygon2d.size() + 1);
-      for (const auto & lanelet_point : polygon2d) {
-        lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y());
-      }
-
-      lanelet_polygon.outer().push_back(lanelet_polygon.outer().front());
-
-      // check the object does not intersect the lanelet
-      if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) {
-        indices.push_back(i);
-        break;
-      }
-    }
-  }
-  return indices;
-}
-
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
 {
@@ -650,18 +604,6 @@ std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanel
   return std::make_pair(target_indices, other_indices);
 }
 
-PredictedObjects filterObjectsByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
-{
-  PredictedObjects filtered_objects;
-  const auto indices = filterObjectIndicesByLanelets(objects, target_lanelets);
-  filtered_objects.objects.reserve(indices.size());
-  for (const size_t i : indices) {
-    filtered_objects.objects.push_back(objects.objects.at(i));
-  }
-  return filtered_objects;
-}
-
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
 {