diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index f16799d7ea206..babbdc60d1cf9 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -9,7 +9,10 @@ # goal search goal_search: - search_priority: "efficient_path" # "efficient_path" or "close_goal" + goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance" + minimum_weighted_distance: + lateral_weight: 40.0 + path_priority: "efficient_path" # "efficient_path" or "close_goal" parking_policy: "left_side" # "left_side" or "right_side" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 44abdb4267672..8ac004e5d60d8 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -149,7 +149,7 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio | Name | Unit | Type | Description | Default value | | :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | | use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells fr2om footprint. | 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 | ## **Goal Search** @@ -161,18 +161,19 @@ searched for in certain range of the shoulder lane. ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| search_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | +| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 8ef426d4f2488..22624bd1e0a80 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -48,8 +48,11 @@ struct GoalPlannerParameters double center_line_path_interval{0.0}; // goal search - std::string search_priority; // "efficient_path" or "close_goal" + std::string goal_priority; // "minimum_weighted_distance" or "minimum_longitudinal_distance" + double minimum_weighted_distance_lateral_weight{0.0}; + std::string path_priority; // "efficient_path" or "close_goal" ParkingPolicy parking_policy; // "left_side" or "right_side" + double forward_goal_search_length{0.0}; double backward_goal_search_length{0.0}; double goal_search_interval{0.0}; 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 bab13a287960a..cac1d37e342c2 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 @@ -37,17 +37,6 @@ struct GoalCandidate double lateral_offset{0.0}; size_t id{0}; bool is_safe{true}; - - bool operator<(const GoalCandidate & other) const noexcept - { - const double diff = distance_from_original_goal - other.distance_from_original_goal; - constexpr double eps = 0.01; - if (std::abs(diff) < eps) { - return lateral_offset < other.lateral_offset; - } - - return distance_from_original_goal < other.distance_from_original_goal; - } }; using GoalCandidates = std::vector; 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 d6e5c8902084f..1f5a823386774 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 @@ -183,13 +183,13 @@ void GoalPlannerModule::onTimer() }; // plan candidate paths and set them to the member variable - if (parameters_->search_priority == "efficient_path") { + if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } } - } else if (parameters_->search_priority == "close_goal") { + } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { planCandidatePaths(planner, goal_candidate); @@ -197,9 +197,9 @@ void GoalPlannerModule::onTimer() } } else { RCLCPP_ERROR( - getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", - parameters_->search_priority.c_str()); - throw std::domain_error("[pull_over] invalid search_priority"); + getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", + parameters_->path_priority.c_str()); + throw std::domain_error("[pull_over] invalid path_priority"); } // set member variables diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 88fef3a6c6508..79459d6efb202 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -45,7 +45,10 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // goal search { const std::string ns = base_ns + "goal_search."; - p.search_priority = node->declare_parameter(ns + "search_priority"); + p.goal_priority = node->declare_parameter(ns + "goal_priority"); + p.minimum_weighted_distance_lateral_weight = + node->declare_parameter(ns + "minimum_weighted_distance.lateral_weight"); + p.path_priority = node->declare_parameter(ns + "path_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); p.backward_goal_search_length = 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 ab5476c542e4e..9ed223c73603f 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 @@ -38,6 +38,33 @@ using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; +// Sort with smaller longitudinal distances taking precedence over smaller lateral distances. +struct SortByLongitudinalDistance +{ + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + const double diff = a.distance_from_original_goal - b.distance_from_original_goal; + constexpr double eps = 0.01; + if (std::abs(diff) < eps) { + return a.lateral_offset < b.lateral_offset; + } + return a.distance_from_original_goal < b.distance_from_original_goal; + } +}; + +// Sort with weighted lateral distance against longitudinal distance +struct SortByWeightedDistance +{ + double lateral_cost{0.0}; + explicit SortByWeightedDistance(double cost) : lateral_cost(cost) {} + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < + b.distance_from_original_goal + lateral_cost * b.lateral_offset; + } +}; + GoalSearcher::GoalSearcher( const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map) @@ -141,8 +168,13 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) } createAreaPolygons(original_search_poses); - // Sort with distance from original goal - std::sort(goal_candidates.begin(), goal_candidates.end()); + if (parameters_.goal_priority == "minimum_weighted_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance(parameters_.minimum_weighted_distance_lateral_weight)); + } else if (parameters_.goal_priority == "minimum_longitudinal_distance") { + std::sort(goal_candidates.begin(), goal_candidates.end(), SortByLongitudinalDistance()); + } return goal_candidates; }