From c80c986e9b4b37f0d6af92c041bfc174819ef843 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 10 Oct 2023 14:08:45 +0900 Subject: [PATCH] fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909) * fix(behavior_path): only apply spline interpolate for output, not for turn_signal processing * fix implementation * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_path_planner_node.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 13 +++++++++---- 2 files changed, 12 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 e5c1b89454f4e..073f9015d4616 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,6 +50,7 @@ #include #include +#include #include namespace behavior_path_planner @@ -136,7 +137,8 @@ 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 593a6cb2cf555..c241ed0324353 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -478,7 +478,10 @@ void BehaviorPathPlannerNode::run() const auto output = bt_manager_->run(planner_data_); // path handling - const auto path = getPath(output); + 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 = path; @@ -504,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; } @@ -527,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 +544,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO const auto resampled_path = util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); - return std::make_shared(resampled_path); + return std::make_pair(std::make_shared(resampled_path), path); } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(