Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(goal_planner): sort goal candidates by weighted distance #5110

Merged
merged 4 commits into from
Sep 26, 2023
Merged

feat(goal_planner): sort goal candidates by weighted distance #5110

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ff81ae7
feat(goal_planner): sort goal candidates priority by weighted distance
kosuke55 Sep 25, 2023
1db5fc0
style(pre-commit): autofix
pre-commit-ci[bot] Sep 25, 2023
e6f3068
Update planning/behavior_path_planner/src/utils/goal_planner/goal_sea…
kosuke55 Sep 26, 2023
e606546
update from sugahara-san review
kosuke55 Sep 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
36 changes: 34 additions & 2 deletions planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
kosuke55 marked this conversation as resolved.
Show resolved Hide resolved
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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[NITS]
If my understanding is correct, comments below is easier to understand for me.
Sort with the weighted sum of the longitudinal distance and the lateral distance weighted by lateral_cost.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

thanks! it is better! fixed in e6f3068

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 +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;
}
Expand Down