From 75a8e92f7dbe8e6a73d660f6e8d70af8dd4a7abb Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Fri, 9 Dec 2022 21:27:18 +0900 Subject: [PATCH] revert(behavior_path): revert removal of refineGoalFunction (#2340)" (#2485) This reverts commit 8e13ced6dfb6edfea77a589ef4cb93d82683bf51. Signed-off-by: mitsudome-r Signed-off-by: mitsudome-r Signed-off-by: kminoda --- .../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::epsilon()) { + return goal; + } + const auto segment = lanelet::utils::getClosestSegment( lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline()); if (segment.empty()) {