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 53e06631a81d5..e6f84e7eece0c 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 @@ -22,6 +22,7 @@ lateral_offset_interval: 0.25 ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 + high_curvature_threshold: 0.1 # occupancy grid map occupancy_grid: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index bdb267ce223d3..6a10c0fb183f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -60,6 +60,7 @@ struct GoalPlannerParameters double lateral_offset_interval{0.0}; double ignore_distance_from_lane_start{0.0}; double margin_from_boundary{0.0}; + double high_curvature_threshold{0.0}; // occupancy grid map bool use_occupancy_grid_for_goal_search{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index d2ba416c7fa90..05385600926f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -52,6 +52,7 @@ struct PullOverPath size_t goal_id{}; size_t id{}; bool decided_velocity{false}; + double max_curvature{0.0}; // max curvature of the parking path PathWithLaneId getFullPath() const { @@ -72,6 +73,7 @@ struct PullOverPath return path; } + // path from the pull over start pose to the end pose PathWithLaneId getParkingPath() const { const PathWithLaneId full_path = getFullPath(); 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 00510a8725e32..1a711adb133eb 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 @@ -293,9 +293,18 @@ void GoalPlannerModule::onTimer() planner->setPlannerData(local_planner_data); planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path) { + if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); + + // calculate absolute maximum curvature of parking path(start pose to end pose) for path + // priority + const std::vector curvatures = + autoware::motion_utils::calcCurvature(pull_over_path->getParkingPath().points); + pull_over_path->max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -817,23 +826,37 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto debugPrintPathPriority = [this]( const std::vector & sorted_pull_over_path_candidates, - const std::map & goal_id_to_index, - const std::optional> & path_id_to_margin_map_opt = std::nullopt, - const std::optional> & isSoftMarginOpt = - std::nullopt) { + 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; - ss << "\n---------------------- path priority ----------------------\n"; - for (const auto & path : sorted_pull_over_path_candidates) { - // clang-format off - ss << "path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id - << ", goal_id: " << path.goal_id - << ", goal_priority:" << goal_id_to_index.at(path.goal_id); - // clang-format on - if (path_id_to_margin_map_opt && isSoftMarginOpt) { - ss << ", margin: " << path_id_to_margin_map_opt->at(path.id) - << ((*isSoftMarginOpt)(path) ? " (soft)" : " (hard)"); + + // 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.max_curvature + << (isHighCurvature(path) ? " (high)" : " (low)"); ss << "\n"; } ss << "-----------------------------------------------------------\n"; @@ -844,6 +867,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri 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 // 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) { @@ -868,6 +892,7 @@ 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()); @@ -914,23 +939,45 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; }); - // Sort pull_over_path_candidates based on the order in efficient_path_order - if (parameters_->path_priority == "efficient_path") { - const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_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; - }; + // (3) Sort by curvature + // If the curvature is less than the threshold, prioritize the path. + const auto isHighCurvature = [&](const PullOverPath & path) -> bool { + return path.max_curvature >= parameters_->high_curvature_threshold; + }; + const auto isSoftMargin = [&](const PullOverPath & path) -> bool { + const double margin = path_id_to_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; + }; + + // 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) { + // 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)) { + return !isHighCurvature(a) && isHighCurvature(b); + } + // otherwise, keep the order based on the margin. + return false; + }); + + // (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) { - // if both are soft margin or both are same hard margin, sort by planner priority + // if any of following conditions are met, sort by path type priority + // - both are soft margin + // - both are same hard margin if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { return comparePathTypePriority(a, b); } @@ -938,18 +985,24 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return false; }); - // debug print path priority: sorted by efficient_path_order and collision check margin + // debug print path priority sorted by + // - efficient_path_order + // - collision check margin + // - curvature debugPrintPathPriority( - sorted_pull_over_path_candidates, goal_id_to_index, path_id_to_margin_map, isSoftMargin); + sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map, + isSoftMargin, isHighCurvature); } } else { - // Sort pull_over_path_candidates based on the order in efficient_path_order + /** + * 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); }); - // debug print path priority: sorted by efficient_path_order and collision check margin - debugPrintPathPriority(sorted_pull_over_path_candidates, goal_id_to_index); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index b079db9babf31..b86bbbca7d98b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -61,6 +61,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.ignore_distance_from_lane_start = node->declare_parameter(ns + "ignore_distance_from_lane_start"); p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); + p.high_curvature_threshold = node->declare_parameter(ns + "high_curvature_threshold"); const std::string parking_policy_name = node->declare_parameter(ns + "parking_policy");