From 4d3ddf16fe3c8b50755abdcbfebd5a3e47daf43a Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 9 Aug 2024 21:03:27 +0900 Subject: [PATCH 1/2] perf(goal_planner): faster path sorting and selection Signed-off-by: kosuke55 --- .../README.md | 14 +- .../config/goal_planner.param.yaml | 4 +- .../goal_planner_module.hpp | 3 - .../util.hpp | 8 + .../src/goal_planner_module.cpp | 299 ++++++++---------- .../src/util.cpp | 38 +++ .../path_safety_checker/safety_check.hpp | 1 - 7 files changed, 185 insertions(+), 182 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 2cfefeb741e80..d1c2224d2233c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -339,13 +339,13 @@ Then there is the concept of soft and hard margins. Although not currently param #### Parameters for object recognition based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------------------------- | :--- | :------------- | :------------------------------------------------------------------------------------------------------- | :-------------- | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation | [2.0, 1.5, 1.0] | -| object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] | -| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | -| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :------------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------------------------- | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation. It is not strictly the distance between footprints, but the maximum distance when ego and objects are oriented. | [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] | +| object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | ## **safety check** diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 0d8d9c29304f8..d3d23ae69dc1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -36,8 +36,8 @@ # object recognition object_recognition: use_object_recognition: false - collision_check_soft_margins: [2.0, 1.5, 1.0] - collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker + collision_check_soft_margins: [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] # the maximum margin when ego and objects are oriented. + collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker. object_recognition_collision_check_max_extra_stopping_margin: 1.0 th_moving_object_velocity: 1.0 detection_bound_offset: 15.0 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 aaad9d5f1a8f7..7462ec519b503 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 @@ -619,9 +619,6 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planPullOverAsOutput(); BehaviorModuleOutput planPullOverAsCandidate(); std::optional> selectPullOverPath( - const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const double collision_check_margin) const; - std::vector sortPullOverPathCandidatesByGoalPriority( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 3c717a90c93ef..f7f4a9317e603 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include @@ -109,6 +110,13 @@ MarkerArray createLaneletPolygonMarkerArray( MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); +std::string makePathPriorityDebugMessage( + const std::vector & sorted_path_indices, + const std::vector & pull_over_path_candidates, + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature); } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ 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 6dee8d7b701bc..8880a9c9b5332 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 @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -845,60 +846,65 @@ BehaviorModuleOutput GoalPlannerModule::plan() return fixed_goal_planner_->plan(planner_data_); } -std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( +std::optional> GoalPlannerModule::selectPullOverPath( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // ========================================================================================== - // print path priority for debug - const auto debugPrintPathPriority = - [this]( - const std::vector & sorted_pull_over_path_candidates, - const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, - const std::map & path_id_to_margin_map, - const std::function & isSoftMargin, - const std::function & isHighCurvature) { - std::stringstream ss; - - // unsafe goal and it's priority are not visible as debug marker in rviz, - // so exclude unsafe goal from goal_priority - std::map goal_id_and_priority; - { - int priority = 0; - for (const auto & goal_candidate : goal_candidates) { - goal_id_and_priority[goal_candidate.id] = goal_candidate.is_safe ? priority++ : -1; - } - } - - ss << "\n---------------------- path priority ----------------------\n"; - for (size_t i = 0; i < sorted_pull_over_path_candidates.size(); ++i) { - const auto & path = sorted_pull_over_path_candidates[i]; - - // goal_index is same to goal priority including unsafe goal - const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); - const bool is_safe_goal = goal_candidates[goal_index].is_safe; - const int goal_priority = goal_id_and_priority[path.goal_id]; - - ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id << ", goal_id: " << path.goal_id - << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") - << ", margin: " << path_id_to_margin_map.at(path.id) - << (isSoftMargin(path) ? " (soft)" : " (hard)") - << ", curvature: " << path.getParkingPathMaxCurvature() - << (isHighCurvature(path) ? " (high)" : " (low)"); - ss << "\n"; - } - ss << "-----------------------------------------------------------\n"; - RCLCPP_DEBUG_STREAM(getLogger(), ss.str()); - }; - // ========================================================================================== - + 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; - // (1) Sort pull_over_path_candidates based on the order in goal_candidates + // 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()); + + // STEP2: Sort pull over path candidates + // STEP2-1: Sort pull_over_path_candidates based on the order in goal_candidates // Create a map of goal_id to its index in goal_candidates std::map goal_id_to_index; for (size_t i = 0; i < goal_candidates.size(); ++i) { @@ -906,10 +912,11 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri } // Sort pull_over_path_candidates based on the order in goal_candidates - auto sorted_pull_over_path_candidates = pull_over_path_candidates; std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&goal_id_to_index](const auto & a, const auto & b) { + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; }); @@ -923,54 +930,44 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri // if object recognition is enabled, sort by collision check margin if (parameters_->use_object_recognition) { - // (2) Sort by collision check margins - const std::vector margins = std::invoke([&]() { - std::vector combined_margins = soft_margins; - combined_margins.insert(combined_margins.end(), hard_margins.begin(), hard_margins.end()); - return combined_margins; - }); - - // Create a map of PullOverPath pointer to largest collision check margin - auto calcLargestMargin = [&](const PullOverPath & pull_over_path) { - // check has safe goal - const auto goal_candidate_it = std::find_if( - goal_candidates.begin(), goal_candidates.end(), - [pull_over_path](const auto & goal_candidate) { - return goal_candidate.id == pull_over_path.goal_id; - }); - if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { - return 0.0; - } - // check path collision margin - const PathWithLaneId parking_path = pull_over_path.getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); - for (const auto & margin : margins) { - if (!checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data_, *parameters_, margin, - /*extract_static_objects=*/true)) { - return margin; - } - } - return 0.0; - }; + // STEP2-2: Sort by collision check margins + const auto [margins, margins_with_zero] = + std::invoke([&]() -> std::tuple, std::vector> { + std::vector margins = soft_margins; + margins.insert(margins.end(), hard_margins.begin(), hard_margins.end()); + std::vector margins_with_zero = margins; + margins_with_zero.push_back(0.0); + return std::make_tuple(margins, margins_with_zero); + }); // Create a map of PullOverPath pointer to largest collision check margin std::map path_id_to_margin_map; - for (const auto & path : sorted_pull_over_path_candidates) { - path_id_to_margin_map[path.id] = calcLargestMargin(path); + const auto target_objects = thread_safe_data_.get_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.getParkingPath(), 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()) { + path_id_to_margin_map[path.id] = margins_with_zero.back(); + } else { + path_id_to_margin_map[path.id] = *it; + } } // sorts in descending order so the item with larger margin comes first std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&path_id_to_margin_map](const PullOverPath & a, const PullOverPath & b) { + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) { return false; } return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; }); - // (3) Sort by curvature + // 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.getParkingPathMaxCurvature() >= parameters_->high_curvature_threshold; @@ -989,8 +986,10 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri // NOTE: this is just partition sort based on curvature threshold within each sub partitions std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&](const PullOverPath & a, const PullOverPath & b) { + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; // if both are soft margin or both are same hard margin, prioritize the path with lower // curvature. if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { @@ -1000,109 +999,71 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return false; }); - // (4) Sort pull_over_path_candidates based on the order in efficient_path_order keeping the - // collision check margin and curvature priority. + // 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") { std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&](const auto & a, const auto & b) { + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { // if any of following conditions are met, sort by path type priority // - both are soft margin // - both are same hard margin + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { return comparePathTypePriority(a, b); } // otherwise, keep the order. return false; }); - - // debug print path priority sorted by - // - efficient_path_order - // - collision check margin - // - curvature - debugPrintPathPriority( - sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map, - isSoftMargin, isHighCurvature); } + + // debug print path priority sorted by + // - efficient_path_order + // - collision check margin + // - curvature + 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_margin_map, isSoftMargin, isHighCurvature); + RCLCPP_DEBUG_STREAM(getLogger(), path_priority_info_str); } else { /** - * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the + * 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") { std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&](const auto & a, const auto & b) { return comparePathTypePriority(a, b); }); + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; + return comparePathTypePriority(a, b); + }); } } - return sorted_pull_over_path_candidates; -} - -std::optional> GoalPlannerModule::selectPullOverPath( - const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const double collision_check_margin) 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; - 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, - /*forward_only_in_route*/ 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); - }(); - for (const auto & pull_over_path : pull_over_path_candidates) { - // check if goal is safe - const auto goal_candidate_it = std::find_if( - goal_candidates.begin(), goal_candidates.end(), - [pull_over_path](const auto & goal_candidate) { - return goal_candidate.id == pull_over_path.goal_id; - }); - if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { - continue; - } - - if (!hasEnoughDistance(pull_over_path, long_tail_reference_path)) { - continue; - } - - // const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), - // 0.5); - const PathWithLaneId parking_path = pull_over_path.getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); + // STEP3: Select the final pull over path by checking collision to make it as high priority as + // possible + const double collision_check_margin = + parameters_->object_recognition_collision_check_hard_margins.back(); + for (const size_t i : sorted_path_indices) { + const auto & path = pull_over_path_candidates[i]; + const PathWithLaneId parking_path = path.getParkingPath(); + const auto parking_path_curvatures = path.getParkingPathCurvatures(); if ( - parameters_->use_object_recognition && - checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data_, *parameters_, collision_check_margin, - /*extract_static_objects=*/true, - /*update_debug_data=*/true)) { + parameters_->use_object_recognition && checkObjectsCollision( + parking_path, parking_path_curvatures, planner_data_, + *parameters_, collision_check_margin, true, true)) { continue; } - if ( parameters_->use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } - - return std::make_pair(pull_over_path, *goal_candidate_it); + return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id))); } - return {}; } @@ -1456,6 +1417,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::chrono::system_clock::time_point start; + start = std::chrono::system_clock::now(); + // if pull over path candidates generation is not finished, use previous module output if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); @@ -1478,26 +1442,20 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() auto goal_candidates = thread_safe_data_.get_goal_candidates(); goal_searcher_->update(goal_candidates, occupancy_grid_map_, planner_data_); - // update pull over path candidates - const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority( - thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); - - // select pull over path which is safe against static objects and get it's goal - const auto path_and_goal_opt = selectPullOverPath( - pull_over_path_candidates, goal_candidates, - parameters_->object_recognition_collision_check_hard_margins.back()); + // Select a path that is as safe as possible and has a high priority. + const auto pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); + const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates); // update thread_safe_data_ if (path_and_goal_opt) { auto [pull_over_path, modified_goal] = *path_and_goal_opt; deceleratePath(pull_over_path); - thread_safe_data_.set( - goal_candidates, pull_over_path_candidates, pull_over_path, modified_goal); + thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal); RCLCPP_DEBUG( getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id, modified_goal.id); } else { - thread_safe_data_.set(goal_candidates, pull_over_path_candidates); + thread_safe_data_.set(goal_candidates); } } @@ -1514,6 +1472,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() // For debug setDebugData(); + if (parameters_->print_debug_info) { // For evaluations printParkingPositionError(); @@ -2133,6 +2092,8 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // decelerate before the search area start const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( thread_safe_data_.get_goal_candidates(), planner_data_); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index b74cdbc6a569b..7b16adf42d93b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -450,4 +451,41 @@ std::vector createPathFootPrints( return footprints; } +std::string makePathPriorityDebugMessage( + const std::vector & sorted_path_indices, + const std::vector & pull_over_path_candidates, + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature) +{ + std::stringstream ss; + + // Unsafe goal and its priority are not visible as debug marker in rviz, + // so exclude unsafe goal from goal_priority + std::map goal_id_and_priority; + for (size_t i = 0; i < goal_candidates.size(); ++i) { + goal_id_and_priority[goal_candidates[i].id] = goal_candidates[i].is_safe ? i : -1; + } + + ss << "\n---------------------- path priority ----------------------\n"; + for (size_t i = 0; i < sorted_path_indices.size(); ++i) { + const auto & path = pull_over_path_candidates[sorted_path_indices[i]]; + // goal_index is same to goal priority including unsafe goal + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const bool is_safe_goal = goal_candidates[goal_index].is_safe; + const int goal_priority = goal_id_and_priority[path.goal_id]; + + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) + << ", path_id: " << path.id << ", goal_id: " << path.goal_id + << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") + << ", margin: " << path_id_to_margin_map.at(path.id) + << (isSoftMargin(path) ? " (soft)" : " (hard)") + << ", curvature: " << path.getParkingPathMaxCurvature() + << (isHighCurvature(path) ? " (high)" : " (low)") << "\n"; + } + ss << "-----------------------------------------------------------\n"; + return ss.str(); +} + } // namespace autoware::behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index df85329d62947..0e6256c3b4d7c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -181,7 +181,6 @@ double calculateRoughDistanceToObjects( const PathWithLaneId & path, const PredictedObjects & objects, const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point, const std::string & distance_type); - // debug CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( From b7dbcb6f76a93fea3d252b952b824fab7a8d3af8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 20 Aug 2024 21:46:41 +0900 Subject: [PATCH 2/2] path_id_to_rough_margin_map Signed-off-by: kosuke55 --- .../util.hpp | 3 ++- .../src/goal_planner_module.cpp | 20 ++++++++++--------- .../src/util.cpp | 4 ++-- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index f7f4a9317e603..9e4d6d2f85f50 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -30,6 +30,7 @@ #include +#include #include #include #include @@ -114,7 +115,7 @@ std::string makePathPriorityDebugMessage( const std::vector & sorted_path_indices, const std::vector & pull_over_path_candidates, const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, - const std::map & path_id_to_margin_map, + const std::map & path_id_to_rough_margin_map, const std::function & isSoftMargin, const std::function & isHighCurvature); } // namespace autoware::behavior_path_planner::goal_planner_utils 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 8880a9c9b5332..ff248fbe02a1c 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 @@ -941,17 +941,18 @@ std::optional> GoalPlannerModule::selectP }); // Create a map of PullOverPath pointer to largest collision check margin - std::map path_id_to_margin_map; + std::map path_id_to_rough_margin_map; const auto target_objects = thread_safe_data_.get_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.getParkingPath(), target_objects, planner_data_->parameters, false, "max"); - auto it = std::lower_bound(margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); + auto it = std::lower_bound( + margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); if (it == margins_with_zero.end()) { - path_id_to_margin_map[path.id] = margins_with_zero.back(); + path_id_to_rough_margin_map[path.id] = margins_with_zero.back(); } else { - path_id_to_margin_map[path.id] = *it; + path_id_to_rough_margin_map[path.id] = *it; } } @@ -961,10 +962,11 @@ std::optional> GoalPlannerModule::selectP [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; - if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) { + if ( + std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01) { return false; } - return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; + return path_id_to_rough_margin_map[a.id] > path_id_to_rough_margin_map[b.id]; }); // STEP2-3: Sort by curvature @@ -974,14 +976,14 @@ std::optional> GoalPlannerModule::selectP }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_margin_map[path.id]; + const double margin = path_id_to_rough_margin_map[path.id]; return std::any_of( soft_margins.begin(), soft_margins.end(), [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); }; const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { return !isSoftMargin(a) && !isSoftMargin(b) && - std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; + std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01; }; // NOTE: this is just partition sort based on curvature threshold within each sub partitions @@ -1024,7 +1026,7 @@ std::optional> GoalPlannerModule::selectP // - curvature 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_margin_map, isSoftMargin, isHighCurvature); + path_id_to_rough_margin_map, isSoftMargin, isHighCurvature); RCLCPP_DEBUG_STREAM(getLogger(), path_priority_info_str); } else { /** diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 7b16adf42d93b..f4b11f4f8e46d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -455,7 +455,7 @@ std::string makePathPriorityDebugMessage( const std::vector & sorted_path_indices, const std::vector & pull_over_path_candidates, const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, - const std::map & path_id_to_margin_map, + const std::map & path_id_to_rough_margin_map, const std::function & isSoftMargin, const std::function & isHighCurvature) { @@ -479,7 +479,7 @@ std::string makePathPriorityDebugMessage( ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) << ", path_id: " << path.id << ", goal_id: " << path.goal_id << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") - << ", margin: " << path_id_to_margin_map.at(path.id) + << ", margin: " << path_id_to_rough_margin_map.at(path.id) << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.getParkingPathMaxCurvature() << (isHighCurvature(path) ? " (high)" : " (low)") << "\n";