From 9adff6e41d5d0fb6f311733a26461e0a5afe5c89 Mon Sep 17 00:00:00 2001 From: h-ohta Date: Wed, 4 Oct 2023 18:05:58 +0900 Subject: [PATCH 1/5] fix(behavior_path): only apply spline interpolate for output, not for turn_signal processing --- .../src/behavior_path_planner_node.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 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 593a6cb2cf55..d1a7a662587e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -479,10 +479,12 @@ void BehaviorPathPlannerNode::run() // path handling const auto path = getPath(output); + const auto resampled_path = std::make_shared( + util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval)); const auto path_candidate = getPathCandidate(output); - planner_data_->prev_output_path = path; + planner_data_->prev_output_path = resampled_path; - auto clipped_path = modifyPathForSmoothGoalConnection(*path); + auto clipped_path = modifyPathForSmoothGoalConnection(*resampled_path); clipPathLength(clipped_path); if (!clipped_path.points.empty()) { @@ -493,7 +495,7 @@ void BehaviorPathPlannerNode::run() path_candidate_publisher_->publish(util::toPath(*path_candidate)); // debug_path_publisher_->publish(util::toPath(path)); - debug_drivable_area_publisher_->publish(path->drivable_area); + debug_drivable_area_publisher_->publish(resampled_path->drivable_area); // for turn signal { @@ -537,9 +539,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO 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_shared(resampled_path); + return path; } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From 437737f7a5ec38872f50d9fa6cc9f828af6a3aac Mon Sep 17 00:00:00 2001 From: h-ohta Date: Fri, 6 Oct 2023 16:57:55 +0900 Subject: [PATCH 2/5] fix implementation --- .../behavior_path_planner_node.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 23 +++++++++++-------- 2 files changed, 15 insertions(+), 10 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 e5c1b89454f4..0a0d943df146 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 @@ -136,7 +136,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out); + std::pair 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 d1a7a662587e..c241ed032435 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -478,13 +478,14 @@ void BehaviorPathPlannerNode::run() const auto output = bt_manager_->run(planner_data_); // path handling - const auto path = getPath(output); - const auto resampled_path = std::make_shared( - util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval)); + const auto paths = getPath(output); + const auto path = paths.first; + const auto original_path = paths.second; + const auto path_candidate = getPathCandidate(output); - planner_data_->prev_output_path = resampled_path; + planner_data_->prev_output_path = path; - auto clipped_path = modifyPathForSmoothGoalConnection(*resampled_path); + auto clipped_path = modifyPathForSmoothGoalConnection(*path); clipPathLength(clipped_path); if (!clipped_path.points.empty()) { @@ -495,7 +496,7 @@ void BehaviorPathPlannerNode::run() path_candidate_publisher_->publish(util::toPath(*path_candidate)); // debug_path_publisher_->publish(util::toPath(path)); - debug_drivable_area_publisher_->publish(resampled_path->drivable_area); + debug_drivable_area_publisher_->publish(path->drivable_area); // for turn signal { @@ -506,7 +507,7 @@ void BehaviorPathPlannerNode::run() hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { turn_signal = turn_signal_decider_.getTurnSignal( - *path, planner_data_->self_pose->pose, *(planner_data_->route_handler), + *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler), output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance); hazard_signal.command = HazardLightsCommand::DISABLE; } @@ -529,7 +530,9 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } -PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output) +// output: spline interpolated path, original path +std::pair BehaviorPathPlannerNode::getPath( + const BehaviorModuleOutput & bt_output) { // TODO(Horibe) do some error handling when path is not available. @@ -539,7 +542,9 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO RCLCPP_DEBUG( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); - return path; + const auto resampled_path = + util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); + return std::make_pair(std::make_shared(resampled_path), path); } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From 3f70c96f8fda81aa61cb865faf3582bc11d31123 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 Oct 2023 07:58:50 +0000 Subject: [PATCH 3/5] ci(pre-commit): autofix --- .../behavior_path_planner/behavior_path_planner_node.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 0a0d943df146..d4908d28e6b2 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 @@ -136,7 +136,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - std::pair getPath(const BehaviorModuleOutput & bt_out); + std::pair getPath( + const BehaviorModuleOutput & bt_out); /** * @brief extract path candidate from behavior tree output From 3750ccad2b804b754be412aba45e8acd2d38a040 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 10 Oct 2023 14:06:46 +0900 Subject: [PATCH 4/5] Update behavior_path_planner_node.hpp --- .../include/behavior_path_planner/behavior_path_planner_node.hpp | 1 + 1 file changed, 1 insertion(+) 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 d4908d28e6b2..e28435aacab0 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 @@ -51,6 +51,7 @@ #include #include #include +#include namespace behavior_path_planner { From 077e47f4118cc7e4abbf9999e87593006e591b76 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 10 Oct 2023 05:07:31 +0000 Subject: [PATCH 5/5] ci(pre-commit): autofix --- .../behavior_path_planner/behavior_path_planner_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e28435aacab0..073f9015d461 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 @@ -50,8 +50,8 @@ #include #include -#include #include +#include namespace behavior_path_planner {