From 2525ffb1b1a42335d715bb552e5113b03124a94a Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Sun, 19 Nov 2023 09:49:50 +0900
Subject: [PATCH] fixup! feat(avoidance): suppress unnecessary avoidance path

---
 .../utils/avoidance/utils.hpp                 |  4 +++
 .../avoidance/avoidance_module.cpp            | 20 ++------------
 .../src/utils/avoidance/utils.cpp             | 27 +++++++++++++++++++
 3 files changed, 33 insertions(+), 18 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
index d703c89bf658a..9e27d1eb6d17a 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
@@ -94,6 +94,10 @@ lanelet::ConstLanelets getTargetLanelets(
 lanelet::ConstLanelets getCurrentLanesFromPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
 
+lanelet::ConstLanelets getExtendLanes(
+  const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
+  const std::shared_ptr<const PlannerData> & planner_data);
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out);
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index 99966b50d9ac8..93f773523dd6c 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -264,24 +264,8 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
   data.current_lanelets = utils::avoidance::getCurrentLanesFromPath(
     *getPreviousModuleOutput().reference_path, planner_data_);
 
-  data.extend_lanelets = data.current_lanelets;
-  while (rclcpp::ok()) {
-    const auto distance = utils::getArcLengthToTargetLanelet(
-      data.extend_lanelets, data.extend_lanelets.back(), getEgoPose());
-
-    if (distance > planner_data_->parameters.forward_path_length) {
-      break;
-    }
-
-    const auto next_lanelets =
-      planner_data_->route_handler->getNextLanelets(data.extend_lanelets.back());
-
-    if (next_lanelets.empty()) {
-      break;
-    }
-
-    data.extend_lanelets.push_back(next_lanelets.front());
-  }
+  data.extend_lanelets =
+    utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
 
   // expand drivable lanes
   std::for_each(
diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
index 7bc797912296d..126d2a0550c19 100644
--- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -971,6 +971,33 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
     start_lane, p.backward_path_length, p.forward_path_length);
 }
 
+lanelet::ConstLanelets getExtendLanes(
+  const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
+  const std::shared_ptr<const PlannerData> & planner_data)
+{
+  lanelet::ConstLanelets extend_lanelets = lanelets;
+
+  while (rclcpp::ok()) {
+    const double lane_length = lanelet::utils::getLaneletLength2d(extend_lanelets);
+    const auto arc_coodinates = lanelet::utils::getArcCoordinates(extend_lanelets, ego_pose);
+    const auto forward_length = lane_length - arc_coodinates.length;
+
+    if (forward_length > planner_data->parameters.forward_path_length) {
+      break;
+    }
+
+    const auto next_lanelets = planner_data->route_handler->getNextLanelets(extend_lanelets.back());
+
+    if (next_lanelets.empty()) {
+      break;
+    }
+
+    extend_lanelets.push_back(next_lanelets.front());
+  }
+
+  return extend_lanelets;
+}
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out)