Skip to content

Commit

Permalink
refactor(goal_planner): divide sort function (autowarefoundation#9650)
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin authored Dec 16, 2024
1 parent d5cd150 commit 9ee2c5a
Show file tree
Hide file tree
Showing 2 changed files with 81 additions and 63 deletions.
6 changes: 6 additions & 0 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,6 +159,12 @@ bool isStopped(
const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower,
const double velocity_upper);

void sortPullOverPaths(
const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects,
rclcpp::Logger logger, std::vector<size_t> & sorted_path_indices);

// Flag class for managing whether a certain callback is running in multi-threading
class ScopedFlag
{
Expand Down
138 changes: 75 additions & 63 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -981,63 +981,14 @@ BehaviorModuleOutput GoalPlannerModule::plan()
return fixed_goal_planner_->plan(planner_data_);
}

std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
const PullOverContextData & context_data,
void sortPullOverPaths(
const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const
const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects,
rclcpp::Logger logger, std::vector<size_t> & sorted_path_indices)
{
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 auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins;
const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins;

// STEP1: Filter valid paths before sorting
// NOTE: Since copying pull over path takes time, it is handled by indices
std::vector<size_t> 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<int, GoalCandidate> 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());
const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins;
const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins;

// STEP2: Sort pull over path candidates
// STEP2-1: Sort pull_over_path_candidates based on the order in goal_candidates
Expand Down Expand Up @@ -1065,14 +1016,14 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(

// compare to sort pull_over_path_candidates based on the order in efficient_path_order
const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
const auto & order = parameters_->efficient_path_order;
const auto & order = parameters.efficient_path_order;
const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type()));
const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type()));
return a_pos < b_pos;
};

// if object recognition is enabled, sort by collision check margin
if (parameters_->use_object_recognition) {
if (parameters.use_object_recognition) {
// STEP2-2: Sort by collision check margins
const auto [margins, margins_with_zero] =
std::invoke([&]() -> std::tuple<std::vector<double>, std::vector<double>> {
Expand All @@ -1085,11 +1036,11 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(

// Create a map of PullOverPath pointer to largest collision check margin
std::map<size_t, double> path_id_to_rough_margin_map;
const auto & target_objects = context_data.static_target_objects;
const auto & target_objects = 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.parking_path(), target_objects, planner_data_->parameters, false, "max");
path.parking_path(), target_objects, planner_data->parameters, false, "max");
auto it = std::lower_bound(
margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater<double>());
if (it == margins_with_zero.end()) {
Expand Down Expand Up @@ -1119,7 +1070,7 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
// 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.parking_path_max_curvature() >= parameters_->high_curvature_threshold;
return path.parking_path_max_curvature() >= parameters.high_curvature_threshold;
};

const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
Expand Down Expand Up @@ -1155,7 +1106,7 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(

// 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") {
if (parameters.path_priority == "efficient_path") {
std::stable_sort(
sorted_path_indices.begin(), sorted_path_indices.end(),
[&](const size_t a_i, const size_t b_i) {
Expand All @@ -1182,14 +1133,14 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
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_rough_margin_map, isSoftMargin, isHighCurvature);
RCLCPP_DEBUG_STREAM(getLogger(), path_priority_info_str);
RCLCPP_DEBUG_STREAM(logger, path_priority_info_str);
} else {
/**
* 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") {
if (parameters.path_priority == "efficient_path") {
std::stable_sort(
sorted_path_indices.begin(), sorted_path_indices.end(),
[&](const size_t a_i, const size_t b_i) {
Expand All @@ -1202,6 +1153,67 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
});
}
}
}

std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
const PullOverContextData & context_data,
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) 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;

// STEP1: Filter valid paths before sorting
// NOTE: Since copying pull over path takes time, it is handled by indices
std::vector<size_t> 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<int, GoalCandidate> 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());

sortPullOverPaths(
planner_data_, *parameters_, pull_over_path_candidates, goal_candidates,
context_data.static_target_objects, getLogger(), sorted_path_indices);

// STEP3: Select the final pull over path by checking collision to make it as high priority as
// possible
Expand Down

0 comments on commit 9ee2c5a

Please sign in to comment.