fix(behavior_path): remove refineGoal which is never executed from th…

…e node and fix the function (#2340) Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Nov 30, 2022 · 8e13ced · 8e13ced
1 parent 1b77bba
commit 8e13ced
Show file tree

Hide file tree

Showing 2 changed files with 1 addition and 18 deletions.
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -883,18 +883,8 @@ 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, refined_goal,
+    planner_data_->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, 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
@@ -1096,14 +1096,7 @@ 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()) {