From 9ee2c5aec8c0723e91675f9d54c2408e8b34c7c1 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 16 Dec 2024 10:34:07 +0900 Subject: [PATCH] refactor(goal_planner): divide sort function (#9650) Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 6 + .../src/goal_planner_module.cpp | 138 ++++++++++-------- 2 files changed, 81 insertions(+), 63 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index aed8066ef30af..3a1773324fe24 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -159,6 +159,12 @@ bool isStopped( const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, const double velocity_upper); +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices); + // Flag class for managing whether a certain callback is running in multi-threading class ScopedFlag { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 61b4bf00fc13c..60c056280e4db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -981,63 +981,14 @@ BehaviorModuleOutput GoalPlannerModule::plan() return fixed_goal_planner_->plan(planner_data_); } -std::optional GoalPlannerModule::selectPullOverPath( - const PullOverContextData & context_data, +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); - const double backward_length = - parameters_->backward_goal_search_length + parameters_->decide_path_distance; - const auto & prev_module_output_path = getPreviousModuleOutput().path; - const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins; - const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins; - - // STEP1: Filter valid paths before sorting - // NOTE: Since copying pull over path takes time, it is handled by indices - std::vector sorted_path_indices; - sorted_path_indices.reserve(pull_over_path_candidates.size()); - - // STEP1-1: Extract paths which have safe goal - // Create a map of goal_id to GoalCandidate for quick access - std::unordered_map goal_candidate_map; - for (const auto & goal_candidate : goal_candidates) { - goal_candidate_map[goal_candidate.id] = goal_candidate; - } - for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { - const auto & path = pull_over_path_candidates[i]; - const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); - if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { - sorted_path_indices.push_back(i); - } - } - - // STEP1-2: Remove paths which do not have enough distance - const double prev_path_front_to_goal_dist = calcSignedArcLength( - prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, - goal_pose.position); - const auto & long_tail_reference_path = [&]() { - if (prev_path_front_to_goal_dist > backward_length) { - return prev_module_output_path; - } - // get road lanes which is at least backward_length[m] behind the goal - const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - prev_module_output_path, planner_data_, backward_length, 0.0, false); - const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; - return planner_data_->route_handler->getCenterLinePath( - road_lanes, std::max(0.0, goal_pose_length - backward_length), - goal_pose_length + parameters_->forward_goal_search_length); - }(); - - sorted_path_indices.erase( - std::remove_if( - sorted_path_indices.begin(), sorted_path_indices.end(), - [&](const size_t i) { - return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path); - }), - sorted_path_indices.end()); + const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins; + const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins; // STEP2: Sort pull over path candidates // STEP2-1: Sort pull_over_path_candidates based on the order in goal_candidates @@ -1065,14 +1016,14 @@ std::optional GoalPlannerModule::selectPullOverPath( // compare to sort pull_over_path_candidates based on the order in efficient_path_order const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - const auto & order = parameters_->efficient_path_order; + const auto & order = parameters.efficient_path_order; const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); return a_pos < b_pos; }; // if object recognition is enabled, sort by collision check margin - if (parameters_->use_object_recognition) { + if (parameters.use_object_recognition) { // STEP2-2: Sort by collision check margins const auto [margins, margins_with_zero] = std::invoke([&]() -> std::tuple, std::vector> { @@ -1085,11 +1036,11 @@ std::optional GoalPlannerModule::selectPullOverPath( // Create a map of PullOverPath pointer to largest collision check margin std::map path_id_to_rough_margin_map; - const auto & target_objects = context_data.static_target_objects; + const auto & target_objects = static_target_objects; for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( - path.parking_path(), target_objects, planner_data_->parameters, false, "max"); + path.parking_path(), target_objects, planner_data->parameters, false, "max"); auto it = std::lower_bound( margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); if (it == margins_with_zero.end()) { @@ -1119,7 +1070,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // STEP2-3: Sort by curvature // If the curvature is less than the threshold, prioritize the path. const auto isHighCurvature = [&](const PullOverPath & path) -> bool { - return path.parking_path_max_curvature() >= parameters_->high_curvature_threshold; + return path.parking_path_max_curvature() >= parameters.high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { @@ -1155,7 +1106,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // STEP2-4: Sort pull_over_path_candidates based on the order in efficient_path_order keeping // the collision check margin and curvature priority. - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { std::stable_sort( sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { @@ -1182,14 +1133,14 @@ std::optional GoalPlannerModule::selectPullOverPath( const std::string path_priority_info_str = goal_planner_utils::makePathPriorityDebugMessage( sorted_path_indices, pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_rough_margin_map, isSoftMargin, isHighCurvature); - RCLCPP_DEBUG_STREAM(getLogger(), path_priority_info_str); + RCLCPP_DEBUG_STREAM(logger, path_priority_info_str); } else { /** * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the * future. sort by curvature is not implemented yet. * Sort pull_over_path_candidates based on the order in efficient_path_order */ - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { std::stable_sort( sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { @@ -1202,6 +1153,67 @@ std::optional GoalPlannerModule::selectPullOverPath( }); } } +} + +std::optional GoalPlannerModule::selectPullOverPath( + const PullOverContextData & context_data, + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); + const double backward_length = + parameters_->backward_goal_search_length + parameters_->decide_path_distance; + const auto & prev_module_output_path = getPreviousModuleOutput().path; + + // STEP1: Filter valid paths before sorting + // NOTE: Since copying pull over path takes time, it is handled by indices + std::vector sorted_path_indices; + sorted_path_indices.reserve(pull_over_path_candidates.size()); + + // STEP1-1: Extract paths which have safe goal + // Create a map of goal_id to GoalCandidate for quick access + std::unordered_map goal_candidate_map; + for (const auto & goal_candidate : goal_candidates) { + goal_candidate_map[goal_candidate.id] = goal_candidate; + } + for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { + const auto & path = pull_over_path_candidates[i]; + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); + if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { + sorted_path_indices.push_back(i); + } + } + + // STEP1-2: Remove paths which do not have enough distance + const double prev_path_front_to_goal_dist = calcSignedArcLength( + prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, + goal_pose.position); + const auto & long_tail_reference_path = [&]() { + if (prev_path_front_to_goal_dist > backward_length) { + return prev_module_output_path; + } + // get road lanes which is at least backward_length[m] behind the goal + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + prev_module_output_path, planner_data_, backward_length, 0.0, false); + const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + return planner_data_->route_handler->getCenterLinePath( + road_lanes, std::max(0.0, goal_pose_length - backward_length), + goal_pose_length + parameters_->forward_goal_search_length); + }(); + + sorted_path_indices.erase( + std::remove_if( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t i) { + return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path); + }), + sorted_path_indices.end()); + + sortPullOverPaths( + planner_data_, *parameters_, pull_over_path_candidates, goal_candidates, + context_data.static_target_objects, getLogger(), sorted_path_indices); // STEP3: Select the final pull over path by checking collision to make it as high priority as // possible