feat(goal_planner): sort goal candidates by weighted distance

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

planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md

@@ -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**
 

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};

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<GoalCandidate>;
 

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -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

planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp

@@ -45,7 +45,10 @@
   // 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 =

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

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -38,6 +38,35 @@
 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)
@@ -141,8 +170,13 @@
   }
   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;
 }