From 848a5c60bd85733fab6d2006222a6acc195d44ff Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 25 Oct 2023 21:09:57 +0900 Subject: [PATCH 1/6] feat(goal_planner): calculate stop pose from closest goal candidate Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 2 ++ .../utils/goal_planner/goal_searcher.hpp | 1 + .../utils/goal_planner/goal_searcher_base.hpp | 1 + .../goal_planner/goal_planner_module.cpp | 25 +++++++++++++------ .../src/utils/goal_planner/goal_searcher.cpp | 24 ++++++++++++++++++ 5 files changed, 45 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 8b722d3316093..c5b2209c16f19 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -142,6 +142,7 @@ class PullOverStatus DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER(std::optional, closest_start_pose) @@ -174,6 +175,7 @@ class PullOverStatus std::optional modified_goal_pose_; Pose refined_goal_pose_{}; GoalCandidates goal_candidates_{}; + Pose closest_goal_candidate_pose_{}; // pull over path std::vector pull_over_path_candidates_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 2ed58b9678e70..e1f10b883030a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -35,6 +35,7 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; + GoalCandidate getClosetGoalCandidateAlongLanes(const GoalCandidates & goal_candidates) const override; private: void countObjectsToAvoid( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index ab319111f6da6..f86e9b7ac4326 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -58,6 +58,7 @@ class GoalSearcherBase MultiPolygon2d getAreaPolygons() { return area_polygons_; } virtual GoalCandidates search() = 0; virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } + virtual GoalCandidate getClosetGoalCandidateAlongLanes(const GoalCandidates & goal_candidates) const = 0; protected: GoalPlannerParameters parameters_{}; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 2db3560d6e4f3..f1118f2b7882a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -557,6 +557,11 @@ void GoalPlannerModule::generateGoalCandidates() goal_searcher_->setPlannerData(planner_data_); goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); status_.set_goal_candidates(goal_searcher_->search()); + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, false); + status_.set_closest_goal_candidate_pose( + goal_searcher_->getClosetGoalCandidateAlongLanes(status_.get_goal_candidates()).goal_pose); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; @@ -564,6 +569,7 @@ void GoalPlannerModule::generateGoalCandidates() GoalCandidates goal_candidates{}; goal_candidates.push_back(goal_candidate); status_.set_goal_candidates(goal_candidates); + status_.set_closest_goal_candidate_pose(goal_pose); } } @@ -1089,20 +1095,23 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // difference between the outer and inner sides) // 4. feasible stop const auto search_start_offset_pose = calcLongitudinalOffsetPose( - reference_path.points, status_.get_refined_goal_pose().position, - -parameters_->backward_goal_search_length - common_parameters.base_link2front - - approximate_pull_over_distance_); + reference_path.points, status_.get_closest_goal_candidate_pose().position, + -approximate_pull_over_distance_); if ( !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = - status_.get_is_safe_static_objects() - ? status_.get_pull_over_path()->start_pose - : (status_.get_closest_start_pose() ? status_.get_closest_start_pose().value() - : *search_start_offset_pose); + const Pose stop_pose = [&]() -> Pose { + if (status_.get_is_safe_static_objects()) { + return status_.get_pull_over_path()->start_pose; + } + if (status_.get_closest_start_pose()) { + return status_.get_closest_start_pose().value(); + } + return *search_start_offset_pose; + }(); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e6dc1776359b..cb66cd459ba1b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -468,4 +468,28 @@ bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons return false; } +GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const +{ + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + + const auto closest_goal_candidate = std::min_element( + goal_candidates.begin(), goal_candidates.end(), + [¤t_lanes](const auto & a, const auto & b) { + const auto goal_arc_length_a = + lanelet::utils::getArcCoordinates(current_lanes, a.goal_pose).length; + const auto goal_arc_length_b = + lanelet::utils::getArcCoordinates(current_lanes, b.goal_pose).length; + return goal_arc_length_a < goal_arc_length_b; + }); + + if (closest_goal_candidate == goal_candidates.end()) { + return {}; // return empty GoalCandidate in case no valid candidates are found. + } + + return *closest_goal_candidate; +} + } // namespace behavior_path_planner From 1c7d2a5e7096dd0b72f8559e34208344b5f937d6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 25 Oct 2023 12:17:03 +0000 Subject: [PATCH 2/6] style(pre-commit): autofix --- .../behavior_path_planner/utils/goal_planner/goal_searcher.hpp | 3 ++- .../utils/goal_planner/goal_searcher_base.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index e1f10b883030a..2fc0acf1c5086 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -35,7 +35,8 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; - GoalCandidate getClosetGoalCandidateAlongLanes(const GoalCandidates & goal_candidates) const override; + GoalCandidate getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const override; private: void countObjectsToAvoid( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index f86e9b7ac4326..24c614e072b8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -58,7 +58,8 @@ class GoalSearcherBase MultiPolygon2d getAreaPolygons() { return area_polygons_; } virtual GoalCandidates search() = 0; virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } - virtual GoalCandidate getClosetGoalCandidateAlongLanes(const GoalCandidates & goal_candidates) const = 0; + virtual GoalCandidate getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const = 0; protected: GoalPlannerParameters parameters_{}; From ba24f69b53286aab8131322fe764082ce1ad3962 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Oct 2023 12:19:48 +0900 Subject: [PATCH 3/6] Update planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp Co-authored-by: Kyoichi Sugahara --- .../src/utils/goal_planner/goal_searcher.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index cb66cd459ba1b..f48a3e7d98699 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -475,14 +475,17 @@ GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); + // Define a lambda function to compute the arc length for a given goal candidate. + auto getGoalArcLength = [¤t_lanes](const auto & candidate) { + return lanelet::utils::getArcCoordinates(current_lanes, candidate.goal_pose).length; + }; + + // Find the closest goal candidate by comparing the arc lengths of each candidate. const auto closest_goal_candidate = std::min_element( goal_candidates.begin(), goal_candidates.end(), - [¤t_lanes](const auto & a, const auto & b) { - const auto goal_arc_length_a = - lanelet::utils::getArcCoordinates(current_lanes, a.goal_pose).length; - const auto goal_arc_length_b = - lanelet::utils::getArcCoordinates(current_lanes, b.goal_pose).length; - return goal_arc_length_a < goal_arc_length_b; + [&getGoalArcLength](const auto & a, const auto & b) { + return getGoalArcLength(a) < getGoalArcLength(b); + }); }); if (closest_goal_candidate == goal_candidates.end()) { From d530909fd70fbd8be72de85650f67ec36e9fd6fc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 26 Oct 2023 03:21:50 +0000 Subject: [PATCH 4/6] style(pre-commit): autofix --- .../src/utils/goal_planner/goal_searcher.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index f48a3e7d98699..901533dd6354e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -486,13 +486,13 @@ GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( [&getGoalArcLength](const auto & a, const auto & b) { return getGoalArcLength(a) < getGoalArcLength(b); }); - }); +}); - if (closest_goal_candidate == goal_candidates.end()) { - return {}; // return empty GoalCandidate in case no valid candidates are found. - } +if (closest_goal_candidate == goal_candidates.end()) { + return {}; // return empty GoalCandidate in case no valid candidates are found. +} - return *closest_goal_candidate; +return *closest_goal_candidate; } } // namespace behavior_path_planner From 32d04dbc86eb1433f515c5e08a3628fca9d784f5 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Oct 2023 15:01:25 +0900 Subject: [PATCH 5/6] Update planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp --- .../src/utils/goal_planner/goal_searcher.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 901533dd6354e..9b20fb7ae5716 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -486,7 +486,6 @@ GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( [&getGoalArcLength](const auto & a, const auto & b) { return getGoalArcLength(a) < getGoalArcLength(b); }); -}); if (closest_goal_candidate == goal_candidates.end()) { return {}; // return empty GoalCandidate in case no valid candidates are found. From 0e3681fcf58c822c66ad62c56d7a60a85862fe14 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 26 Oct 2023 06:04:01 +0000 Subject: [PATCH 6/6] style(pre-commit): autofix --- .../src/utils/goal_planner/goal_searcher.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 9b20fb7ae5716..f498ecd9ed8ec 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -487,11 +487,11 @@ GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( return getGoalArcLength(a) < getGoalArcLength(b); }); -if (closest_goal_candidate == goal_candidates.end()) { - return {}; // return empty GoalCandidate in case no valid candidates are found. -} + if (closest_goal_candidate == goal_candidates.end()) { + return {}; // return empty GoalCandidate in case no valid candidates are found. + } -return *closest_goal_candidate; + return *closest_goal_candidate; } } // namespace behavior_path_planner