From 6a37de9fffec533c96ef659aeaa41ffc033a8c4b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 4 Jul 2024 01:40:34 +0900 Subject: [PATCH 1/2] fix Signed-off-by: Takayuki Murooka --- .../behavior_path_planner_node.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 14 ++++++-------- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 073f9015d4616..242a3cb0acba0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -137,7 +137,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - std::pair getPath( + PathWithLaneId::SharedPtr getPath( const BehaviorModuleOutput & bt_out); /** 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 c241ed0324353..8274fb3fadba0 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -478,14 +478,14 @@ void BehaviorPathPlannerNode::run() const auto output = bt_manager_->run(planner_data_); // path handling - const auto paths = getPath(output); - const auto path = paths.first; - const auto original_path = paths.second; + const auto original_path = getPath(output); const auto path_candidate = getPathCandidate(output); - planner_data_->prev_output_path = path; + planner_data_->prev_output_path = original_path; + const auto path = original_path; auto clipped_path = modifyPathForSmoothGoalConnection(*path); + clipped_path = util::resamplePathWithSpline(clipped_path, planner_data_->parameters.path_interval); clipPathLength(clipped_path); if (!clipped_path.points.empty()) { @@ -531,7 +531,7 @@ void BehaviorPathPlannerNode::run() } // output: spline interpolated path, original path -std::pair BehaviorPathPlannerNode::getPath( +PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( const BehaviorModuleOutput & bt_output) { // TODO(Horibe) do some error handling when path is not available. @@ -542,9 +542,7 @@ std::pair BehaviorPathPlan RCLCPP_DEBUG( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); - const auto resampled_path = - util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); - return std::make_pair(std::make_shared(resampled_path), path); + return path; } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From 13d2d9ebe2bb84f071f697396301acde54954c1f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 4 Jul 2024 01:28:12 +0000 Subject: [PATCH 2/2] ci(pre-commit): autofix --- .../behavior_path_planner/behavior_path_planner_node.hpp | 3 +-- .../src/behavior_path_planner_node.cpp | 6 +++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 242a3cb0acba0..657cf524fc465 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -137,8 +137,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - PathWithLaneId::SharedPtr getPath( - const BehaviorModuleOutput & bt_out); + PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out); /** * @brief extract path candidate from behavior tree output 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 8274fb3fadba0..740a4bb17e4aa 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -485,7 +485,8 @@ void BehaviorPathPlannerNode::run() const auto path = original_path; auto clipped_path = modifyPathForSmoothGoalConnection(*path); - clipped_path = util::resamplePathWithSpline(clipped_path, planner_data_->parameters.path_interval); + clipped_path = + util::resamplePathWithSpline(clipped_path, planner_data_->parameters.path_interval); clipPathLength(clipped_path); if (!clipped_path.points.empty()) { @@ -531,8 +532,7 @@ void BehaviorPathPlannerNode::run() } // output: spline interpolated path, original path -PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( - const BehaviorModuleOutput & bt_output) +PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output) { // TODO(Horibe) do some error handling when path is not available.