From ee9a38959181ff4850d3d300e2816ac75a5b4fa1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 20 Dec 2022 00:39:25 +0900 Subject: [PATCH] feat(behavior_path_planner): keep original points in resampling of pull over (#2478) * feat(behavior_path_planner): keep original points in resampling Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/path_utilities.cpp Signed-off-by: kosuke55 * change motion_utils::overlap_threshold to 0.2 and use it Signed-off-by: kosuke55 * Revert "change motion_utils::overlap_threshold to 0.2 and use it" This reverts commit edfb2d59df8446e50bb582b83ccc8f2522e298fe. Signed-off-by: kosuke55 * keep input points only in pull over Signed-off-by: kosuke55 * Add comment of temporary processe Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../behavior_path_planner_node.hpp | 2 ++ .../behavior_path_planner/path_utilities.hpp | 3 ++- .../src/behavior_path_planner_node.cpp | 21 +++++++++++++++++-- .../src/path_utilities.cpp | 8 +++++-- 4 files changed, 29 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 1a0800b268c15..75d1f798d8b65 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 @@ -168,6 +168,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool skipSmoothGoalConnection( const std::vector> & statuses) const; + bool keepInputPoints(const std::vector> & statuses) const; + /** * @brief skip smooth goal connection */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp index 5ab15f17306aa..2d6f44633cc5d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp @@ -41,7 +41,8 @@ std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, const size_t end = std::numeric_limits::max(), const double offset = 0.0); -PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval); +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points = false); Path toPath(const PathWithLaneId & input); 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 732464ec82106..58afd67a77f14 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -817,8 +817,9 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( connected_path = modifyPathForSmoothGoalConnection(*path); } - const auto resampled_path = - util::resamplePathWithSpline(connected_path, planner_data_->parameters.path_interval); + const auto resampled_path = util::resamplePathWithSpline( + connected_path, planner_data_->parameters.path_interval, + keepInputPoints(module_status_ptr_vec)); return std::make_shared(resampled_path); } @@ -837,6 +838,22 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( return false; } +// This is a temporary process until motion planning can take the terminal pose into account +bool BehaviorPathPlannerNode::keepInputPoints( + const std::vector> & statuses) const +{ + const auto target_module = "PullOver"; + + for (auto & status : statuses) { + if (status->is_waiting_approval || status->status == BT::NodeStatus::RUNNING) { + if (target_module == status->module_name) { + return true; + } + } + } + return false; +} + void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 20d9cfc0fc3b8..457687a6667d3 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -55,7 +55,8 @@ std::vector calcPathArcLengthArray( /** * @brief resamplePathWithSpline */ -PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points) { if (path.points.size() < 2) { return path; @@ -66,7 +67,7 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv transformed_path.at(i) = path.points.at(i).point; } - constexpr double epsilon = 0.01; + constexpr double epsilon = 0.2; const auto has_almost_same_value = [&](const auto & vec, const auto x) { if (vec.empty()) return false; const auto has_close = [&](const auto v) { return std::abs(v - x) < epsilon; }; @@ -79,6 +80,9 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv for (size_t i = 0; i < path.points.size(); ++i) { const double s = motion_utils::calcSignedArcLength(transformed_path, 0, i); for (const auto & lane_id : path.points.at(i).lane_ids) { + if (keep_input_points && !has_almost_same_value(s_in, s)) { + s_in.push_back(s); + } if ( std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) != unique_lane_ids.end()) {