From ff30b258c1bd97905508d7c36018b2153e623ae1 Mon Sep 17 00:00:00 2001 From: h-ohta Date: Thu, 12 Oct 2023 12:40:36 +0900 Subject: [PATCH 1/6] add base points to resampled path in behavior_path --- .../src/path_utilities.cpp | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 9a02ced1c41c4..ac847d23564aa 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -90,7 +90,11 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) { const auto base_points = calcPathArcLengthArray(path); - const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); + auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); + sampling_points.insert(sampling_points.end(), base_points.begin(), base_points.end()); + std::sort(sampling_points.begin(), sampling_points.end()); + sampling_points.erase( + std::unique(sampling_points.begin(), sampling_points.end()), sampling_points.end()); if (base_points.empty() || sampling_points.empty()) { return path; @@ -122,22 +126,25 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv // For LaneIds, Type, Twist // - // ------|----|----|----|----|----|----|-------> resampled - // [0] [1] [2] [3] [4] [5] [6] + // ------|----|----|----|----|----|----|----|--------> resampled + // [0] [1] [2] [3] [4] [5] [6] [7] // - // --------|---------------|----------|---------> base - // [0] [1] [2] + // ------|---------------|----------|-------|---> base + // [0] [1] [2] [3] // - // resampled[0~3] = base[0] - // resampled[4~5] = base[1] - // resampled[6] = base[2] + // resampled[0] = base[0] + // resampled[1~3] = base[1] + // resampled[4~5] = base[2] + // resampled[6~7] = base[3] // size_t base_idx{0}; for (size_t i = 0; i < resampled_path.points.size(); ++i) { - while (base_idx < base_points.size() - 1 && sampling_points.at(i) > base_points.at(base_idx)) { + while (base_idx < base_points.size() - 1 && ( + (sampling_points.at(i) > base_points.at(base_idx)) || + (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) { ++base_idx; } - size_t ref_idx = std::max(static_cast(base_idx) - 1, 0); + size_t ref_idx = std::max(static_cast(base_idx), 0); if (i == resampled_path.points.size() - 1) { ref_idx = base_points.size() - 1; // for last point } From e6dd540d354a9cdb488da0b12157b55c8c895ae9 Mon Sep 17 00:00:00 2001 From: h-ohta Date: Thu, 12 Oct 2023 14:28:42 +0900 Subject: [PATCH 2/6] Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)" This reverts commit c80c986e9b4b37f0d6af92c041bfc174819ef843. --- .../behavior_path_planner_node.hpp | 4 +--- .../src/behavior_path_planner_node.cpp | 13 ++++--------- 2 files changed, 5 insertions(+), 12 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..e5c1b89454f4e 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,7 +50,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -137,8 +136,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - std::pair 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 c241ed0324353..593a6cb2cf555 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -478,10 +478,7 @@ 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 path = getPath(output); const auto path_candidate = getPathCandidate(output); planner_data_->prev_output_path = path; @@ -507,7 +504,7 @@ void BehaviorPathPlannerNode::run() hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { turn_signal = turn_signal_decider_.getTurnSignal( - *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler), + *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; } @@ -530,9 +527,7 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } -// output: spline interpolated path, original path -std::pair 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. @@ -544,7 +539,7 @@ std::pair BehaviorPathPlan const auto resampled_path = util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); - return std::make_pair(std::make_shared(resampled_path), path); + return std::make_shared(resampled_path); } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From 6ef72fa2559fba29bc8d55eafae29fe33ea5147f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 12 Oct 2023 05:40:50 +0000 Subject: [PATCH 3/6] ci(pre-commit): autofix --- planning/behavior_path_planner/src/path_utilities.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index ac847d23564aa..4e43f83a98863 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -139,9 +139,9 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv // size_t base_idx{0}; for (size_t i = 0; i < resampled_path.points.size(); ++i) { - while (base_idx < base_points.size() - 1 && ( - (sampling_points.at(i) > base_points.at(base_idx)) || - (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) { + while (base_idx < base_points.size() - 1 && + ((sampling_points.at(i) > base_points.at(base_idx)) || + (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) { ++base_idx; } size_t ref_idx = std::max(static_cast(base_idx), 0); From 2e29b83af60a20f572279e24156d2ebebe1630fe Mon Sep 17 00:00:00 2001 From: h-ohta Date: Thu, 12 Oct 2023 17:34:45 +0900 Subject: [PATCH 4/6] fix insert --- .../behavior_path_planner/src/path_utilities.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 4e43f83a98863..34e5111c1a2cb 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -91,10 +91,17 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv { const auto base_points = calcPathArcLengthArray(path); auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); - sampling_points.insert(sampling_points.end(), base_points.begin(), base_points.end()); + + constexpr double epsilon = 0.01; + for (const auto & b : base_points) { + const auto is_almost_same_value = std::find_if( + sampling_points.begin(), sampling_points.end(), + [&](const auto v) { return std::abs(v - b) < epsilon; }); + if (is_almost_same_value == sampling_points.end()) { + sampling_points.push_back(b); + } + } std::sort(sampling_points.begin(), sampling_points.end()); - sampling_points.erase( - std::unique(sampling_points.begin(), sampling_points.end()), sampling_points.end()); if (base_points.empty() || sampling_points.empty()) { return path; From 046c467447186a2f087900a0baa5ac45434f4c37 Mon Sep 17 00:00:00 2001 From: h-ohta Date: Thu, 12 Oct 2023 20:07:12 +0900 Subject: [PATCH 5/6] fix: not interpolate behavior velocity path --- planning/behavior_velocity_planner/src/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ad30db2917ecd..c8600e25716b9 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -420,10 +420,10 @@ void BehaviorVelocityPlannerNode::onTrigger( const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + // const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); // check stop point - auto output_path_msg = filterStopPathPoint(interpolated_path_msg); + auto output_path_msg = filterStopPathPoint(filtered_path); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); From 2f2ded44a21acfd66887320d5126522e1002724f Mon Sep 17 00:00:00 2001 From: h-ohta Date: Fri, 13 Oct 2023 09:38:23 +0900 Subject: [PATCH 6/6] Revert "Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)"" This reverts commit e6dd540d354a9cdb488da0b12157b55c8c895ae9. --- .../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(