Skip to content

Commit

Permalink
feat(goal_planner): sort goal candidates by weighted distance (#5110)
Browse files Browse the repository at this point in the history 
* feat(goal_planner): sort goal candidates priority by weighted distance

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* style(pre-commit): autofix

* Update planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

* update from sugahara-san review

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
  • Loading branch information
@kosuke55 @pre-commit-ci @kyoichi-sugahara
3 people authored Sep 26, 2023
1 parent d9ab02a commit 231459f
Show file tree
Hide file tree
Showing 7 changed files with 66 additions and 33 deletions.
5 changes: 4 additions & 1 deletion planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
25 changes: 13 additions & 12 deletions planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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**

Expand Down
5 changes: 4 additions & 1 deletion ...path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
11 changes: 0 additions & 11 deletions ...vior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<GoalCandidate>;

Expand Down
10 changes: 5 additions & 5 deletions planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,23 +183,23 @@ 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);
}
}
} 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
Expand Down
5 changes: 4 additions & 1 deletion planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,10 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
// goal search
{
const std::string ns = base_ns + "goal_search.";
p.search_priority = node->declare_parameter<std::string>(ns + "search_priority");
p.goal_priority = node->declare_parameter<std::string>(ns + "goal_priority");
p.minimum_weighted_distance_lateral_weight =
node->declare_parameter<double>(ns + "minimum_weighted_distance.lateral_weight");
p.path_priority = node->declare_parameter<std::string>(ns + "path_priority");
p.forward_goal_search_length =
node->declare_parameter<double>(ns + "forward_goal_search_length");
p.backward_goal_search_length =
Expand Down
38 changes: 36 additions & 2 deletions planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,35 @@ 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 the longitudinal distances are approximately equal, sort based on lateral offset.
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 the weighted sum of the longitudinal distance and the lateral distance weighted by
// lateral_cost.
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<OccupancyGridBasedCollisionDetector> & occupancy_grid_map)
Expand Down Expand Up @@ -141,8 +170,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;
}
Expand Down

0 comments on commit 231459f

Please sign in to comment.