From d1fca8ecbc4863dd4ff3d81ee58ea7058abcfe73 Mon Sep 17 00:00:00 2001
From: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Date: Fri, 9 Dec 2022 20:17:20 +0900
Subject: [PATCH] Revert "fix(behavior_path): remove refineGoal which is never
 executed from the node and fix the function (#2340)"

This reverts commit 8e13ced6dfb6edfea77a589ef4cb93d82683bf51.
---
 .../src/behavior_path_planner_node.cpp               | 12 +++++++++++-
 planning/behavior_path_planner/src/utilities.cpp     |  7 +++++++
 2 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index ab9f506de00a7..acaa0be7291ef 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -886,8 +886,18 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
   const auto goal = planner_data_->route_handler->getGoalPose();
   const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();
 
+  Pose refined_goal{};
+  {
+    lanelet::ConstLanelet goal_lanelet;
+    if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) {
+      refined_goal = util::refineGoal(goal, goal_lanelet);
+    } else {
+      refined_goal = goal;
+    }
+  }
+
   auto refined_path = util::refinePathForGoal(
-    planner_data_->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, goal,
+    planner_data_->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, refined_goal,
     goal_lane_id);
   refined_path.header.frame_id = "map";
   refined_path.header.stamp = this->now();
diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
index 339965bdea493..aaf3dce50f1fc 100644
--- a/planning/behavior_path_planner/src/utilities.cpp
+++ b/planning/behavior_path_planner/src/utilities.cpp
@@ -1098,7 +1098,14 @@ bool setGoal(
 
 const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet)
 {
+  // return goal;
   const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position);
+  const double distance = boost::geometry::distance(
+    goal_lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(lanelet_point).basicPoint());
+  if (distance < std::numeric_limits<double>::epsilon()) {
+    return goal;
+  }
+
   const auto segment = lanelet::utils::getClosestSegment(
     lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline());
   if (segment.empty()) {