adding bezier

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt

@@ -3,7 +3,7 @@ project(autoware_behavior_path_goal_planner_module)
 
 set(ament_cmake_lint_cmake_FOUND TRUE)
 
-option(BUILD_EXAMPLES "Build examples" OFF)
+option(BUILD_EXAMPLES "Build examples" ON)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -130,6 +130,9 @@
             neighbor_radius: 8.0
             margin: 1.0
 
+        bezier_parking:
+          pull_over_azimuth_threshold: 0.785
+
       stop_condition:
         maximum_deceleration_for_stop: 1.0
         maximum_jerk_for_stop: 1.0

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case1.yaml

@@ -6,16 +6,16 @@
 # map_path_uri: package://<package-name>/<resource-path>
 # fields(this is array)
 #   - name: <field-name-for-your-yaml-of-this-topic, str>
-#     type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TBD}
+#     type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD}
 #     topic: <topic-name, str>
 #
 format_version: 1
 map_path_uri: package://autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm
 dynamic_object:
   header:
     stamp:
-      sec: 909
-      nanosec: 742399532
+      sec: 42
+      nanosec: 459457775
     frame_id: map
   objects: []
 traffic_signal:
@@ -26,84 +26,78 @@ traffic_signal:
 route:
   header:
     stamp:
-      sec: 125
-      nanosec: 213336488
+      sec: 30
+      nanosec: 801796556
     frame_id: map
   start_pose:
     position:
-      x: 748.132
-      y: 708.703
-      z: 381.187
+      x: 266.509
+      y: 335.333
+      z: 100.000
     orientation:
       x: 0.00000
       y: 0.00000
-      z: 0.235076
-      w: 0.971977
+      z: 0.705621
+      w: 0.708589
   goal_pose:
     position:
-      x: 798.040
-      y: 689.386
-      z: 381.187
+      x: 296.939
+      y: 348.933
+      z: 100.000
     orientation:
       x: 0.00000
       y: 0.00000
-      z: -0.659713
-      w: 0.751517
+      z: 0.984630
+      w: -0.174655
   segments:
     - preferred_primitive:
-        id: 15214
+        id: 697
         primitive_type: ""
       primitives:
-        - id: 15214
-          primitive_type: lane
-        - id: 15213
+        - id: 697
           primitive_type: lane
     - preferred_primitive:
-        id: 15226
+        id: 759
         primitive_type: ""
       primitives:
-        - id: 15226
-          primitive_type: lane
-        - id: 15225
+        - id: 759
           primitive_type: lane
     - preferred_primitive:
-        id: 15228
+        id: 675
         primitive_type: ""
       primitives:
-        - id: 15228
-          primitive_type: lane
-        - id: 15229
+        - id: 675
           primitive_type: lane
     - preferred_primitive:
-        id: 15231
+        id: 676
         primitive_type: ""
       primitives:
-        - id: 15231
+        - id: 676
           primitive_type: lane
   uuid:
     uuid:
-      - 100
-      - 171
-      - 232
-      - 218
-      - 65
-      - 244
-      - 197
-      - 172
+      - 163
+      - 32
+      - 242
+      - 174
+      - 255
+      - 45
+      - 183
+      - 95
+      - 124
       - 167
-      - 40
-      - 148
-      - 169
-      - 170
-      - 48
-      - 29
-      - 68
+      - 172
+      - 137
+      - 228
+      - 24
+      - 2
+      - 220
   allow_modification: false
 operation_mode:
   stamp:
-    sec: 902
-    nanosec: 525199106
-  mode: 2
+    sec: 0
+    nanosec: 513411336
+  mode: 1
   is_autoware_control_enabled: true
   is_in_transition: false
   is_stop_mode_available: true
@@ -113,14 +107,14 @@ operation_mode:
 self_acceleration:
   header:
     stamp:
-      sec: 909
-      nanosec: 769937073
+      sec: 42
+      nanosec: 489453464
     frame_id: /base_link
   accel:
     accel:
       linear:
-        x: -0.581360
-        y: -0.290347
+        x: 0.00000
+        y: 0.00000
         z: 0.00000
       angular:
         x: 0.00000
@@ -166,21 +160,21 @@ self_acceleration:
 self_odometry:
   header:
     stamp:
-      sec: 909
-      nanosec: 769937073
+      sec: 42
+      nanosec: 489453464
     frame_id: map
   child_frame_id: base_link
   pose:
     pose:
       position:
-        x: 760.155
-        y: 713.417
-        z: 381.187
+        x: 266.509
+        y: 335.333
+        z: 100.000
       orientation:
         x: 0.00000
         y: 0.00000
-        z: 0.116943
-        w: 0.993139
+        z: 0.705621
+        w: 0.708589
     covariance:
       - 0.000100000
       - 0.00000
@@ -221,13 +215,13 @@ self_odometry:
   twist:
     twist:
       linear:
-        x: 3.83825
+        x: 0.00000
         y: 0.00000
         z: 0.00000
       angular:
         x: 0.00000
         y: 0.00000
-        z: -0.0756457
+        z: 0.00000
     covariance:
       - 0.00000
       - 0.00000

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp

@@ -1,4 +1,4 @@
 // Copyright 2024 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.
@@ -16,6 +16,7 @@
 #include "autoware/behavior_path_goal_planner_module/util.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
+#include <autoware/behavior_path_goal_planner_module/goal_planner_module.hpp>
 #include <autoware/behavior_path_goal_planner_module/manager.hpp>
 #include <autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp>
 #include <autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp>
@@ -32,6 +33,7 @@
 #include <autoware_test_utils/mock_data_parser.hpp>
 
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
+#include <autoware_perception_msgs/msg/detail/predicted_objects__struct.hpp>
 #include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/msg/lanelet_segment.hpp>
@@ -151,18 +153,24 @@
 
 void plot_path_with_lane_id(
   matplotlibcpp17::axes::Axes & axes, const PathWithLaneId & path,
-  const std::string & color = "red", const std::string & label = "", const double linewidth = 1.0)
+  const std::string & color = "red", const std::string & label = "", const double linewidth = 1.0,
+  const bool point_idx = false)
 {
   std::vector<double> xs, ys;
   std::vector<double> yaw_cos, yaw_sin;
+  int cnt = 0;
   for (const auto & point : path.points) {
     xs.push_back(point.point.pose.position.x);
     ys.push_back(point.point.pose.position.y);
     const double yaw = autoware::universe_utils::getRPY(point.point.pose).z;
     yaw_cos.push_back(std::cos(yaw));
     yaw_sin.push_back(std::sin(yaw));
     axes.scatter(
       Args(xs.back(), ys.back()), Kwargs("marker"_a = "o", "color"_a = "blue", "s"_a = 10));
+    if (point_idx) {
+      axes.text(Args(xs.back(), ys.back(), std::to_string(cnt)));
+      cnt++;
+    }
   }
   axes.quiver(
     Args(xs, ys, yaw_cos, yaw_sin),
@@ -312,126 +320,39 @@
     }
   }
 
+  autoware::behavior_path_planner::sortPullOverPaths(
+    planner_data, parameters, pull_over_path_candidates, goal_candidates,
+    autoware_perception_msgs::msg::PredictedObjects{}, rclcpp::get_logger("temp"),
+    sorted_path_indices);
+
   const double prev_path_front_to_goal_dist = calcSignedArcLength(
     previous_module_output.path.points,
     previous_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 previous_module_output.path;
     }
     // get road lanes which is at least backward_length[m] behind the goal
     const auto road_lanes = getExtendedCurrentLanesFromPath(
       previous_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, planner_data, parameters);
       }),
     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;
-
-  const auto [margins, margins_with_zero] =
-    std::invoke([&]() -> std::tuple<std::vector<double>, std::vector<double>> {
-      std::vector<double> margins = soft_margins;
-      margins.insert(margins.end(), hard_margins.begin(), hard_margins.end());
-      std::vector<double> margins_with_zero = margins;
-      margins_with_zero.push_back(0.0);
-      return std::make_tuple(margins, margins_with_zero);
-    });
-
-  // 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 = autoware_perception_msgs::msg::PredictedObjects{};
-  for (const size_t i : sorted_path_indices) {
-    const auto & path = pull_over_path_candidates[i];  // cppcheck-suppress containerOutOfBounds
-    const double distance =
-      autoware::behavior_path_planner::utils::path_safety_checker::calculateRoughDistanceToObjects(
-        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()) {
-      path_id_to_rough_margin_map[path.id()] = margins_with_zero.back();
-    } else {
-      path_id_to_rough_margin_map[path.id()] = *it;
-    }
-  }
-
-  // sorts in descending order so the item with larger margin comes first
-  std::stable_sort(
-    sorted_path_indices.begin(), sorted_path_indices.end(),
-    [&](const size_t a_i, const size_t b_i) {
-      const auto & a = pull_over_path_candidates[a_i];
-      const auto & b = pull_over_path_candidates[b_i];
-      if (
-        std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) <
-        0.01) {
-        return false;
-      }
-      return path_id_to_rough_margin_map[a.id()] > path_id_to_rough_margin_map[b.id()];
-    });
-
-  // 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;
-  };
-
-  const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
-    const double margin = path_id_to_rough_margin_map[path.id()];
-    return std::any_of(
-      soft_margins.begin(), soft_margins.end(),
-      [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; });
-  };
-  const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
-    return !isSoftMargin(a) && !isSoftMargin(b) &&
-           std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) <
-             0.01;
-  };
-
-  // NOTE: this is just partition sort based on curvature threshold within each sub partitions
-  std::stable_sort(
-    sorted_path_indices.begin(), sorted_path_indices.end(),
-    [&](const size_t a_i, const size_t b_i) {
-      const auto & a = pull_over_path_candidates[a_i];
-      const auto & b = pull_over_path_candidates[b_i];
-      // if both are soft margin or both are same hard margin, prioritize the path with lower
-      // curvature.
-      if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) {
-        return !isHighCurvature(a) && isHighCurvature(b);
-      }
-      // otherwise, keep the order based on the margin.
-      return false;
-    });
-
-  // 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") {
-    std::stable_sort(
-      sorted_path_indices.begin(), sorted_path_indices.end(),
-      [&](const size_t a_i, const size_t b_i) {
-        // if any of following conditions are met, sort by path type priority
-        // - both are soft margin
-        // - both are same hard margin
-        const auto & a = pull_over_path_candidates[a_i];
-        const auto & b = pull_over_path_candidates[b_i];
-        // otherwise, keep the order.
-        return false;
-      });
-  }
-
   std::vector<PullOverPath> selected;
   for (const auto & sorted_index : sorted_path_indices) {
     selected.push_back(pull_over_path_candidates.at(sorted_index));
   }
 
   return selected;
@@ -675,7 +596,7 @@
     const auto max_parking_curvature = filtered_path.parking_path_max_curvature();
     if (i == 0) {
       plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color);
-      plot_path_with_lane_id(ax2, filtered_path.full_path(), color, "most prio", 2.0);
+      plot_path_with_lane_id(ax2, filtered_path.full_path(), color, "most prio", 2.0, true);
       for (const auto & path_point : filtered_path.full_path().points) {
         const auto pose_footprint = transformVector(
           footprint, autoware::universe_utils::pose2transform(path_point.point.pose));