From 4c92de28a9145c498df0b72413df0b4382f98239 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Wed, 30 Nov 2022 23:17:40 +0900 Subject: [PATCH] fix(behavior_path_planner): apply resampling with spline around a goal pose Signed-off-by: Makoto Kurihara --- .../src/behavior_path_planner_node.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 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 d766030205f8c..467fd407992e7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -620,19 +620,11 @@ void BehaviorPathPlannerNode::run() planner_data_->prev_output_path = path; mutex_pd_.unlock(); - PathWithLaneId clipped_path; - const auto module_status_ptr_vec = bt_manager_->getModulesStatus(); - if (skipSmoothGoalConnection(module_status_ptr_vec)) { - clipped_path = *path; - } else { - clipped_path = modifyPathForSmoothGoalConnection(*path); - } - - const size_t target_idx = findEgoIndex(clipped_path.points); - util::clipPathLength(clipped_path, target_idx, planner_data_->parameters); + const size_t target_idx = findEgoIndex(path->points); + util::clipPathLength(*path, target_idx, planner_data_->parameters); - if (!clipped_path.points.empty()) { - path_publisher_->publish(clipped_path); + if (!path->points.empty()) { + path_publisher_->publish(*path); } else { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); @@ -730,8 +722,16 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( RCLCPP_DEBUG( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); + PathWithLaneId connected_path; + const auto module_status_ptr_vec = bt_manager_->getModulesStatus(); + if (skipSmoothGoalConnection(module_status_ptr_vec)) { + connected_path = *path; + } else { + connected_path = modifyPathForSmoothGoalConnection(*path); + } + const auto resampled_path = - util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); + util::resamplePathWithSpline(connected_path, planner_data_->parameters.path_interval); return std::make_shared(resampled_path); }