From 8e13ced6dfb6edfea77a589ef4cb93d82683bf51 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 30 Nov 2022 18:03:38 +0900 Subject: [PATCH] fix(behavior_path): remove refineGoal which is never executed from the node and fix the function (#2340) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/behavior_path_planner_node.cpp | 12 +----------- planning/behavior_path_planner/src/utilities.cpp | 7 ------- 2 files changed, 1 insertion(+), 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 index d0efc611ae807..d766030205f8c 100644 --- 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 index 15214a2198ac4..89933bb020a3d 100644 --- 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::epsilon()) { - return goal; - } - const auto segment = lanelet::utils::getClosestSegment( lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline()); if (segment.empty()) {