feat(goal_planner): add bezier based pull over planner (#9642)

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt

@@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/pull_over_planner/freespace_pull_over.cpp
   src/pull_over_planner/geometric_pull_over.cpp
   src/pull_over_planner/shift_pull_over.cpp
+  src/pull_over_planner/bezier_pull_over.cpp
   src/default_fixed_goal_planner.cpp
   src/goal_searcher.cpp
   src/util.cpp

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case1.yaml

@@ -0,0 +1,267 @@
+#
+# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver
+# format1:
+#
+# format_version: <format-major-version, int>
+# 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}
+#     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
+    frame_id: map
+  objects: []
+traffic_signal:
+  stamp:
+    sec: 0
+    nanosec: 0
+  traffic_light_groups: []
+route:
+  header:
+    stamp:
+      sec: 125
+      nanosec: 213336488
+    frame_id: map
+  start_pose:
+    position:
+      x: 748.132
+      y: 708.703
+      z: 381.187
+    orientation:
+      x: 0.00000
+      y: 0.00000
+      z: 0.235076
+      w: 0.971977
+  goal_pose:
+    position:
+      x: 798.040
+      y: 689.386
+      z: 381.187
+    orientation:
+      x: 0.00000
+      y: 0.00000
+      z: -0.659713
+      w: 0.751517
+  segments:
+    - preferred_primitive:
+        id: 15214
+        primitive_type: ""
+      primitives:
+        - id: 15214
+          primitive_type: lane
+        - id: 15213
+          primitive_type: lane
+    - preferred_primitive:
+        id: 15226
+        primitive_type: ""
+      primitives:
+        - id: 15226
+          primitive_type: lane
+        - id: 15225
+          primitive_type: lane
+    - preferred_primitive:
+        id: 15228
+        primitive_type: ""
+      primitives:
+        - id: 15228
+          primitive_type: lane
+        - id: 15229
+          primitive_type: lane
+    - preferred_primitive:
+        id: 15231
+        primitive_type: ""
+      primitives:
+        - id: 15231
+          primitive_type: lane
+  uuid:
+    uuid:
+      - 100
+      - 171
+      - 232
+      - 218
+      - 65
+      - 244
+      - 197
+      - 172
+      - 167
+      - 40
+      - 148
+      - 169
+      - 170
+      - 48
+      - 29
+      - 68
+  allow_modification: false
+operation_mode:
+  stamp:
+    sec: 902
+    nanosec: 525199106
+  mode: 2
+  is_autoware_control_enabled: true
+  is_in_transition: false
+  is_stop_mode_available: true
+  is_autonomous_mode_available: true
+  is_local_mode_available: true
+  is_remote_mode_available: true
+self_acceleration:
+  header:
+    stamp:
+      sec: 909
+      nanosec: 769937073
+    frame_id: /base_link
+  accel:
+    accel:
+      linear:
+        x: -0.581360
+        y: -0.290347
+        z: 0.00000
+      angular:
+        x: 0.00000
+        y: 0.00000
+        z: 0.00000
+    covariance:
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+self_odometry:
+  header:
+    stamp:
+      sec: 909
+      nanosec: 769937073
+    frame_id: map
+  child_frame_id: base_link
+  pose:
+    pose:
+      position:
+        x: 760.155
+        y: 713.417
+        z: 381.187
+      orientation:
+        x: 0.00000
+        y: 0.00000
+        z: 0.116943
+        w: 0.993139
+    covariance:
+      - 0.000100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.000100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+  twist:
+    twist:
+      linear:
+        x: 3.83825
+        y: 0.00000
+        z: 0.00000
+      angular:
+        x: 0.00000
+        y: 0.00000
+        z: -0.0756457
+    covariance:
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000