diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index d680fd2ecd695..bc8705dc76174 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/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 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case1.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case1.yaml new file mode 100644 index 0000000000000..7bbc4b54e7f08 --- /dev/null +++ b/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: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TBD} +# topic: +# +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 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case2.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case2.yaml new file mode 100644 index 0000000000000..ab86692166164 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case2.yaml @@ -0,0 +1,261 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm +dynamic_object: + header: + stamp: + sec: 57 + nanosec: 642011543 + frame_id: map + objects: [] +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 49 + nanosec: 727009929 + frame_id: map + start_pose: + position: + x: 266.506 + y: 343.545 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695321 + w: 0.718700 + goal_pose: + position: + x: 291.295 + y: 346.135 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999225 + w: 0.0393747 + segments: + - preferred_primitive: + id: 697 + primitive_type: "" + primitives: + - id: 697 + primitive_type: lane + - preferred_primitive: + id: 759 + primitive_type: "" + primitives: + - id: 759 + primitive_type: lane + - preferred_primitive: + id: 675 + primitive_type: "" + primitives: + - id: 675 + primitive_type: lane + - preferred_primitive: + id: 676 + primitive_type: "" + primitives: + - id: 676 + primitive_type: lane + uuid: + uuid: + - 126 + - 92 + - 126 + - 245 + - 182 + - 56 + - 251 + - 150 + - 125 + - 65 + - 22 + - 180 + - 59 + - 40 + - 250 + - 241 + allow_modification: false +operation_mode: + stamp: + sec: 40 + nanosec: 229462653 + 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: 57 + nanosec: 675161172 + frame_id: /base_link + accel: + accel: + linear: + x: -0.0735332 + y: -0.539254 + 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: 57 + nanosec: 675161172 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 288.819 + y: 363.189 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0508455 + w: 0.998707 + 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: 1.95939 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.275215 + 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 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case3.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case3.yaml new file mode 100644 index 0000000000000..48a84480dd3a4 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case3.yaml @@ -0,0 +1,261 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm +dynamic_object: + header: + stamp: + sec: 59 + nanosec: 863135063 + frame_id: map + objects: [] +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 49 + nanosec: 727009929 + frame_id: map + start_pose: + position: + x: 266.506 + y: 343.545 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695321 + w: 0.718700 + goal_pose: + position: + x: 291.295 + y: 346.135 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999225 + w: 0.0393747 + segments: + - preferred_primitive: + id: 697 + primitive_type: "" + primitives: + - id: 697 + primitive_type: lane + - preferred_primitive: + id: 759 + primitive_type: "" + primitives: + - id: 759 + primitive_type: lane + - preferred_primitive: + id: 675 + primitive_type: "" + primitives: + - id: 675 + primitive_type: lane + - preferred_primitive: + id: 676 + primitive_type: "" + primitives: + - id: 676 + primitive_type: lane + uuid: + uuid: + - 126 + - 92 + - 126 + - 245 + - 182 + - 56 + - 251 + - 150 + - 125 + - 65 + - 22 + - 180 + - 59 + - 40 + - 250 + - 241 + allow_modification: false +operation_mode: + stamp: + sec: 40 + nanosec: 229462653 + 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: 59 + nanosec: 863135063 + frame_id: /base_link + accel: + accel: + linear: + x: 0.0180450 + y: -0.573187 + 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: 59 + nanosec: 863135063 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 292.928 + y: 362.175 + z: 100.000 + orientation: + x: 0.00000 + y: -0.00000 + z: -0.283955 + w: 0.958837 + 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: 2.01175 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.284920 + 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 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp new file mode 100644 index 0000000000000..56e207c160873 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -0,0 +1,706 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +using autoware::behavior_path_planner::BehaviorModuleOutput; +using autoware::behavior_path_planner::GoalCandidate; +using autoware::behavior_path_planner::GoalCandidates; +using autoware::behavior_path_planner::GoalPlannerParameters; +using autoware::behavior_path_planner::PlannerData; +using autoware::behavior_path_planner::PullOverPath; +using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_planning_msgs::msg::PathWithLaneId; + +std::vector g_colors = { + "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", + "#0000FF", "#8A2BE2", "#A52A2A", "#DEB887", "#5F9EA0", "#7FFF00", "#D2691E", "#FF7F50", "#6495ED", + "#FFF8DC", "#DC143C", "#00FFFF", "#00008B", "#008B8B", "#B8860B", "#A9A9A9", "#006400", "#A9A9A9", + "#BDB76B", "#8B008B", "#556B2F", "#FF8C00", "#9932CC", "#8B0000", "#E9967A", "#8FBC8F", "#483D8B", + "#2F4F4F", "#2F4F4F", "#00CED1", "#9400D3", "#FF1493", "#00BFFF", "#696969", "#696969", "#1E90FF", + "#B22222", "#FFFAF0", "#228B22", "#FF00FF", "#DCDCDC", "#F8F8FF", "#FFD700", "#DAA520", "#808080", + "#008000", "#ADFF2F", "#808080", "#F0FFF0", "#FF69B4", "#CD5C5C", "#4B0082", "#FFFFF0", "#F0E68C", + "#E6E6FA", "#FFF0F5", "#7CFC00", "#FFFACD", "#ADD8E6", "#F08080", "#E0FFFF", "#FAFAD2", "#D3D3D3", + "#90EE90", "#D3D3D3", "#FFB6C1", "#FFA07A", "#20B2AA", "#87CEFA", "#778899", "#778899", "#B0C4DE", + "#FFFFE0", "#00FF00", "#32CD32", "#FAF0E6", "#FF00FF", "#800000", "#66CDAA", "#0000CD", "#BA55D3", + "#9370DB", "#3CB371", "#7B68EE", "#00FA9A", "#48D1CC", "#C71585", "#191970", "#F5FFFA", "#FFE4E1", + "#FFE4B5", "#FFDEAD", "#000080", "#FDF5E6", "#808000", "#6B8E23", "#FFA500", "#FF4500", "#DA70D6", + "#EEE8AA", "#98FB98", "#AFEEEE", "#DB7093", "#FFEFD5", "#FFDAB9", "#CD853F", "#FFC0CB", "#DDA0DD", + "#B0E0E6", "#800080", "#663399", "#FF0000", "#BC8F8F", "#4169E1", "#8B4513", "#FA8072", "#F4A460", + "#2E8B57", "#FFF5EE", "#A0522D", "#C0C0C0", "#87CEEB", "#6A5ACD", "#708090", "#708090", "#FFFAFA", + "#00FF7F", "#4682B4", "#D2B48C", "#008080", "#D8BFD8", "#FF6347", "#40E0D0", "#EE82EE", "#F5DEB3", + "#FFFFFF", "#F5F5F5", "#FFFF00", "#9ACD32"}; + +void plot_footprint( + matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint, + const std::string & color) +{ + std::vector xs, ys; + for (const auto & pt : footprint) { + xs.push_back(pt.x()); + ys.push_back(pt.y()); + } + xs.push_back(xs.front()); + ys.push_back(ys.front()); + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted")); +} + +void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon) +{ + std::vector xs, ys; + for (const auto & p : polygon) { + xs.push_back(p.x()); + ys.push_back(p.y()); + } + xs.push_back(xs.front()); + ys.push_back(ys.front()); + axes.fill(Args(xs, ys), Kwargs("color"_a = "grey", "alpha"_a = 0.5)); +} + +void plot_goal_candidate( + matplotlibcpp17::axes::Axes & axes, const GoalCandidate & goal, const size_t prio, + const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color) +{ + std::vector xs, ys; + std::vector yaw_cos, yaw_sin; + const auto goal_footprint = + transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); + plot_footprint(axes, goal_footprint, color); + xs.push_back(goal.goal_pose.position.x); + ys.push_back(goal.goal_pose.position.y); + axes.text(Args(xs.back(), ys.back(), std::to_string(prio))); + const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; + yaw_cos.push_back(std::cos(yaw)); + yaw_sin.push_back(std::sin(yaw)); + axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); +} + +void plot_goal_candidates( + matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals, + const std::map & goal_id2prio, + const autoware::universe_utils::LinearRing2d & local_footprint, + const std::string & color = "green") +{ + for (const auto & goal : goals) { + const auto it = goal_id2prio.find(goal.id); + if (it != goal_id2prio.end()) { + plot_goal_candidate(axes, goal, it->second, local_footprint, color); + } + } +} + +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) +{ + std::vector xs, ys; + std::vector yaw_cos, yaw_sin; + 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)); + } + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); + + if (label == "") { + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth)); + } else { + axes.plot( + Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth, "label"_a = label)); + } +} + +void plot_lanelet( + matplotlibcpp17::axes::Axes & axes, lanelet::ConstLanelet lanelet, + const std::string & color = "blue", const double linewidth = 0.5) +{ + const auto lefts = lanelet.leftBound(); + const auto rights = lanelet.rightBound(); + std::vector xs_left, ys_left; + for (const auto & point : lefts) { + xs_left.push_back(point.x()); + ys_left.push_back(point.y()); + } + + std::vector xs_right, ys_right; + for (const auto & point : rights) { + xs_right.push_back(point.x()); + ys_right.push_back(point.y()); + } + + std::vector xs_center, ys_center; + for (const auto & point : lanelet.centerline()) { + xs_center.push_back(point.x()); + ys_center.push_back(point.y()); + } + + axes.plot(Args(xs_left, ys_left), Kwargs("color"_a = color, "linewidth"_a = linewidth)); + axes.plot(Args(xs_right, ys_right), Kwargs("color"_a = color, "linewidth"_a = linewidth)); + axes.plot( + Args(xs_center, ys_center), + Kwargs("color"_a = "black", "linewidth"_a = linewidth, "linestyle"_a = "dashed")); +} + +std::shared_ptr instantiate_planner_data( + rclcpp::Node::SharedPtr node, const std::string & map_path, + const std::string & sample_planner_data_yaml_path) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr lanelet_map_ptr = lanelet::load(map_path, projector, &errors); + if (!errors.empty()) { + for (const auto & error : errors) { + std::cout << error << std::endl; + } + return nullptr; + } + autoware_map_msgs::msg::LaneletMapBin map_bin; + lanelet::utils::conversion::toBinMsg( + lanelet_map_ptr, &map_bin); // TODO(soblin): pass lanelet_map_ptr to RouteHandler + + YAML::Node config = YAML::LoadFile(sample_planner_data_yaml_path); + + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node); + planner_data->route_handler->setMap(map_bin); + planner_data->route_handler->setRoute( + autoware::test_utils::parse(config["route"])); + + auto odometry = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse(config["self_odometry"])); + planner_data->self_odometry = odometry; + + auto accel_ptr = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse( + config["self_acceleration"])); + planner_data->self_acceleration = accel_ptr; + return planner_data; +} + +bool hasEnoughDistance( + const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path, + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + using autoware::motion_utils::calcSignedArcLength; + + const Pose & current_pose = planner_data->self_odometry->pose.pose; + const double current_vel = planner_data->self_odometry->twist.twist.linear.x; + + // when the path is separated and start_pose is close, + // once stopped, the vehicle cannot start again. + // so need enough distance to restart. + // distance to restart should be less than decide_path_distance. + // otherwise, the goal would change immediately after departure. + const bool is_separated_path = pull_over_path.partial_paths().size() > 1; + const double distance_to_start = calcSignedArcLength( + long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); + const double distance_to_restart = parameters.decide_path_distance / 2; + const double eps_vel = 0.01; + const bool is_stopped = std::abs(current_vel) < eps_vel; + if (is_separated_path && is_stopped && distance_to_start < distance_to_restart) { + return false; + } + + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data, parameters.maximum_deceleration, parameters.maximum_jerk, 0.0); + if (!current_to_stop_distance) { + return false; + } + + /* + // If the stop line is subtly exceeded, it is assumed that there is not enough distance to the + // starting point of parking, so to prevent this, once the vehicle has stopped, it also has a + // stop_distance_buffer to allow for the amount exceeded. + const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; + if (distance_to_start + buffer < *current_to_stop_distance) { + return false; + }*/ + + return true; +} + +std::vector selectPullOverPaths( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) +{ + using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; + using autoware::motion_utils::calcSignedArcLength; + + const auto & goal_pose = planner_data->route_handler->getOriginalGoalPose(); + const double backward_length = + parameters.backward_goal_search_length + parameters.decide_path_distance; + + std::vector sorted_path_indices; + sorted_path_indices.reserve(pull_over_path_candidates.size()); + + std::unordered_map 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); + } + } + + 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> { + std::vector margins = soft_margins; + margins.insert(margins.end(), hard_margins.begin(), hard_margins.end()); + std::vector 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 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()); + 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 selected; + for (const auto & sorted_index : sorted_path_indices) { + selected.push_back(pull_over_path_candidates.at(sorted_index)); + } + + return selected; +} + +std::optional calculate_centerline_path( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + const auto original_goal_pose = planner_data->route_handler->getOriginalGoalPose(); + const auto refined_goal_opt = + autoware::behavior_path_planner::goal_planner_utils::calcRefinedGoal( + original_goal_pose, planner_data->route_handler, true, + planner_data->parameters.vehicle_length, planner_data->parameters.base_link2front, + planner_data->parameters.base_link2front, parameters); + if (!refined_goal_opt) { + return std::nullopt; + } + const auto & refined_goal = refined_goal_opt.value(); + + const auto & route_handler = planner_data->route_handler; + const double forward_length = parameters.forward_goal_search_length; + const double backward_length = parameters.backward_goal_search_length; + const bool use_bus_stop_area = parameters.bus_stop_area.use_bus_stop_area; + /* + const double margin_from_boundary = parameters.margin_from_boundary; + const double lateral_offset_interval = use_bus_stop_area + ? parameters.bus_stop_area.lateral_offset_interval + : parameters.lateral_offset_interval; + const double max_lateral_offset = parameters.max_lateral_offset; + const double ignore_distance_from_lane_start = parameters.ignore_distance_from_lane_start; + */ + + const auto pull_over_lanes = + autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes( + *route_handler, true, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + const auto departure_check_lane = + autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *route_handler, true); + const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, refined_goal); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + const double longitudinal_interval = use_bus_stop_area + ? parameters.bus_stop_area.goal_search_interval + : parameters.goal_search_interval; + auto center_line_path = autoware::behavior_path_planner::utils::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval); + return center_line_path; +} + +std::vector getBusStopAreaPolygons( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + const auto pull_over_lanes = + autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes( + *(planner_data->route_handler), true, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + return autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons( + pull_over_lanes); +} + +struct SortByWeightedDistance +{ + double lateral_cost{0.0}; + bool prioritize_goals_before_objects{false}; + + SortByWeightedDistance(double cost, bool prioritize_goals_before_objects) + : lateral_cost(cost), prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < + b.distance_from_original_goal + lateral_cost * b.lateral_offset; + } +}; + +int main(int argc, char ** argv) +{ + using autoware::behavior_path_planner::utils::getReferencePath; + rclcpp::init(argc, argv); + + auto node_options = rclcpp::NodeOptions{}; + node_options.parameter_overrides( + std::vector{{"launch_modules", std::vector{}}}); + node_options.arguments(std::vector{ + "--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + + "/config/behavior_path_planner.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + + "/config/drivable_area_expansion.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + + "/config/scene_module_manager.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_nearest_search.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_goal_planner_module") + + "/config/goal_planner.param.yaml"}); + auto node = rclcpp::Node::make_shared("plot_map", node_options); + + auto planner_data = instantiate_planner_data( + node, + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/test_map/road_shoulder/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_goal_planner_module") + + "/config/sample_planner_data_case1.yaml"); + + lanelet::ConstLanelet current_route_lanelet; + planner_data->route_handler->getClosestLaneletWithinRoute( + planner_data->self_odometry->pose.pose, ¤t_route_lanelet); + const auto reference_path = getReferencePath(current_route_lanelet, planner_data); + auto goal_planner_parameter = + autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters( + node.get(), "goal_planner."); + goal_planner_parameter.bus_stop_area.use_bus_stop_area = true; + goal_planner_parameter.lane_departure_check_expansion_margin = 0.2; + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); + autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{}; + lane_departure_checker.setVehicleInfo(vehicle_info); + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + goal_planner_parameter.lane_departure_check_expansion_margin; + lane_departure_checker.setParam(lane_departure_checker_params); + const auto footprint = vehicle_info.createFootprint(); + autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); + auto goal_candidates = goal_searcher.search(planner_data); + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance( + goal_planner_parameter.minimum_weighted_distance_lateral_weight, + goal_planner_parameter.prioritize_goals_before_objects)); + std::map goal_id2prio{}; + for (auto i = 0; i < goal_candidates.size(); ++i) { + goal_id2prio[goal_candidates.at(i).id] = i; + } + + pybind11::scoped_interpreter guard{}; + auto plt = matplotlibcpp17::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 2); + + auto & ax1 = axes[0]; + auto & ax2 = axes[1]; + // auto & ax3 = axes[2]; + const std::vector ids{/*15213, 15214, */ 15225, + 15226, + 15224, + 15227, + 15228, + 15229, + 15230, + 15231, + 15232}; + for (const auto & id : ids) { + const auto lanelet = planner_data->route_handler->getLaneletMapPtr()->laneletLayer.get(id); + plot_lanelet(ax1, lanelet); + plot_lanelet(ax2, lanelet); + // plot_lanelet(ax3, lanelet); + } + + // plot_goal_candidates(ax1, goal_candidates, footprint); + + // plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + + const auto start = std::chrono::steady_clock::now(); + std::vector candidates; + for (auto i = 0; i < goal_candidates.size(); ++i) { + const auto & goal_candidate = goal_candidates.at(i); + auto shift_pull_over_planner = autoware::behavior_path_planner::BezierPullOver( + *node, goal_planner_parameter, lane_departure_checker); + auto pull_over_paths = + shift_pull_over_planner.plans(goal_candidate, 0, planner_data, reference_path); + if (!pull_over_paths.empty()) { + std::copy( + std::make_move_iterator(pull_over_paths.begin()), + std::make_move_iterator(pull_over_paths.end()), std::back_inserter(candidates)); + } + } + + const auto filtered_paths = selectPullOverPaths( + candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path); + std::cout << filtered_paths.size() << std::endl; + const auto end = std::chrono::steady_clock::now(); + std::cout << "computed candidate bezier paths in " + << std::chrono::duration_cast(end - start).count() * 1.0 / + 1000000 + << "msecs" << std::endl; + std::cout << "filtered " << filtered_paths.size() << "/" << candidates.size() << " paths" + << std::endl; + /* + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + const auto max_parking_curvature = filtered_path.parking_path_max_curvature(); + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + if (i == 0) { + plot_path_with_lane_id(ax1, filtered_path.parking_path(), color, "most prio", 2.0); + } else { + plot_path_with_lane_id( + ax1, filtered_path.full_path(), color, + std::to_string(prio) + "-th goal(id=" + std::to_string(goal_id) + + "): k_max=" + std::to_string(max_parking_curvature), + 0.5); + } + } + */ + const auto original_goal_pos = planner_data->route_handler->getOriginalGoalPose().position; + ax1.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + if (goal_planner_parameter.bus_stop_area.use_bus_stop_area) { + const auto bus_stop_area_polygons = + getBusStopAreaPolygons(planner_data, goal_planner_parameter); + for (const auto & bus_stop_area_polygon : bus_stop_area_polygons) { + plot_lanelet_polygon(ax1, bus_stop_area_polygon); + } + } + + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + 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); + for (const auto & path_point : filtered_path.full_path().points) { + const auto pose_footprint = transformVector( + footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); + plot_footprint(ax2, pose_footprint, "blue"); + } + } else if (i % 50 == 0) { + std::cout << "plotting " << i << "-th filtered path" << std::endl; + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + plot_path_with_lane_id(ax1, filtered_path.full_path(), color, "", 2.0); + } + } + ax2.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + + const auto centerline_path = calculate_centerline_path(planner_data, goal_planner_parameter); + if (centerline_path) { + // plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path"); + } + + ax1.set_aspect(Args("equal")); + ax2.set_aspect(Args("equal")); + ax2.legend(); + plt.show(); + + rclcpp::shutdown(); + return 0; +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp similarity index 66% rename from planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 40c2bd201abc7..102e5ef53b164 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include @@ -40,6 +42,8 @@ #include #include +#include +#include #include #include @@ -52,7 +56,6 @@ #include #include #include - using namespace std::chrono_literals; // NOLINT using autoware::behavior_path_planner::BehaviorModuleOutput; @@ -65,9 +68,28 @@ using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDec using autoware_planning_msgs::msg::LaneletRoute; using tier4_planning_msgs::msg::PathWithLaneId; +std::vector g_colors = { + "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", + "#0000FF", "#8A2BE2", "#A52A2A", "#DEB887", "#5F9EA0", "#7FFF00", "#D2691E", "#FF7F50", "#6495ED", + "#FFF8DC", "#DC143C", "#00FFFF", "#00008B", "#008B8B", "#B8860B", "#A9A9A9", "#006400", "#A9A9A9", + "#BDB76B", "#8B008B", "#556B2F", "#FF8C00", "#9932CC", "#8B0000", "#E9967A", "#8FBC8F", "#483D8B", + "#2F4F4F", "#2F4F4F", "#00CED1", "#9400D3", "#FF1493", "#00BFFF", "#696969", "#696969", "#1E90FF", + "#B22222", "#FFFAF0", "#228B22", "#FF00FF", "#DCDCDC", "#F8F8FF", "#FFD700", "#DAA520", "#808080", + "#008000", "#ADFF2F", "#808080", "#F0FFF0", "#FF69B4", "#CD5C5C", "#4B0082", "#FFFFF0", "#F0E68C", + "#E6E6FA", "#FFF0F5", "#7CFC00", "#FFFACD", "#ADD8E6", "#F08080", "#E0FFFF", "#FAFAD2", "#D3D3D3", + "#90EE90", "#D3D3D3", "#FFB6C1", "#FFA07A", "#20B2AA", "#87CEFA", "#778899", "#778899", "#B0C4DE", + "#FFFFE0", "#00FF00", "#32CD32", "#FAF0E6", "#FF00FF", "#800000", "#66CDAA", "#0000CD", "#BA55D3", + "#9370DB", "#3CB371", "#7B68EE", "#00FA9A", "#48D1CC", "#C71585", "#191970", "#F5FFFA", "#FFE4E1", + "#FFE4B5", "#FFDEAD", "#000080", "#FDF5E6", "#808000", "#6B8E23", "#FFA500", "#FF4500", "#DA70D6", + "#EEE8AA", "#98FB98", "#AFEEEE", "#DB7093", "#FFEFD5", "#FFDAB9", "#CD853F", "#FFC0CB", "#DDA0DD", + "#B0E0E6", "#800080", "#663399", "#FF0000", "#BC8F8F", "#4169E1", "#8B4513", "#FA8072", "#F4A460", + "#2E8B57", "#FFF5EE", "#A0522D", "#C0C0C0", "#87CEEB", "#6A5ACD", "#708090", "#708090", "#FFFAFA", + "#00FF7F", "#4682B4", "#D2B48C", "#008080", "#D8BFD8", "#FF6347", "#40E0D0", "#EE82EE", "#F5DEB3", + "#FFFFFF", "#F5F5F5", "#FFFF00", "#9ACD32"}; + void plot_footprint( matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint, - const std::string & color = "blue") + const std::string & color) { std::vector xs, ys; for (const auto & pt : footprint) { @@ -79,43 +101,77 @@ void plot_footprint( axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted")); } +void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon) +{ + std::vector xs, ys; + for (const auto & p : polygon) { + xs.push_back(p.x()); + ys.push_back(p.y()); + } + xs.push_back(xs.front()); + ys.push_back(ys.front()); + axes.fill(Args(xs, ys), Kwargs("color"_a = "grey", "alpha"_a = 0.5)); +} + +void plot_goal_candidate( + matplotlibcpp17::axes::Axes & axes, const GoalCandidate & goal, const size_t prio, + const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color) +{ + std::vector xs, ys; + std::vector yaw_cos, yaw_sin; + const auto goal_footprint = + transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); + plot_footprint(axes, goal_footprint, color); + xs.push_back(goal.goal_pose.position.x); + ys.push_back(goal.goal_pose.position.y); + axes.text(Args(xs.back(), ys.back(), std::to_string(prio))); + const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; + yaw_cos.push_back(std::cos(yaw)); + yaw_sin.push_back(std::sin(yaw)); + axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); +} + void plot_goal_candidates( matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals, + const std::map & goal_id2prio, const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color = "green") { - std::vector xs, ys; - std::vector yaw_cos, yaw_sin; for (const auto & goal : goals) { - const auto goal_footprint = - transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); - plot_footprint(axes, goal_footprint); - xs.push_back(goal.goal_pose.position.x); - ys.push_back(goal.goal_pose.position.y); - axes.text(Args(xs.back(), ys.back(), std::to_string(goal.id))); - const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; - yaw_cos.push_back(std::cos(yaw)); - yaw_sin.push_back(std::sin(yaw)); + const auto it = goal_id2prio.find(goal.id); + if (it != goal_id2prio.end()) { + plot_goal_candidate(axes, goal, it->second, local_footprint, color); + } } - axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); - axes.quiver( - Args(xs, ys, yaw_cos, yaw_sin), - Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 1.0)); } void plot_path_with_lane_id( matplotlibcpp17::axes::Axes & axes, const PathWithLaneId & path, - const std::string & color = "red", const std::string & label = "") + const std::string & color = "red", const std::string & label = "", const double linewidth = 1.0) { std::vector xs, ys; + std::vector yaw_cos, yaw_sin; 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)); } + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); + if (label == "") { - axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0)); + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth)); } else { - axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0, "label"_a = label)); + axes.plot( + Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth, "label"_a = label)); } } @@ -151,7 +207,8 @@ void plot_lanelet( } std::shared_ptr instantiate_planner_data( - rclcpp::Node::SharedPtr node, const std::string & map_path, const LaneletRoute & route_msg) + rclcpp::Node::SharedPtr node, const std::string & map_path, + const std::string & sample_planner_data_yaml_path) { lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; @@ -166,20 +223,21 @@ std::shared_ptr instantiate_planner_data( lanelet::utils::conversion::toBinMsg( lanelet_map_ptr, &map_bin); // TODO(soblin): pass lanelet_map_ptr to RouteHandler + YAML::Node config = YAML::LoadFile(sample_planner_data_yaml_path); + auto planner_data = std::make_shared(); planner_data->init_parameters(*node); planner_data->route_handler->setMap(map_bin); - planner_data->route_handler->setRoute(route_msg); + planner_data->route_handler->setRoute( + autoware::test_utils::parse(config["route"])); - nav_msgs::msg::Odometry odom; - odom.pose.pose = route_msg.start_pose; - auto odometry = std::make_shared(odom); + auto odometry = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse(config["self_odometry"])); planner_data->self_odometry = odometry; - geometry_msgs::msg::AccelWithCovarianceStamped accel; - accel.accel.accel.linear.x = 0.537018510955429; - accel.accel.accel.linear.y = -0.2435352815388478; - auto accel_ptr = std::make_shared(accel); + auto accel_ptr = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse( + config["self_acceleration"])); planner_data->self_acceleration = accel_ptr; return planner_data; } @@ -302,7 +360,7 @@ std::vector selectPullOverPaths( std::map 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]; + 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"); @@ -390,9 +448,9 @@ std::vector selectPullOverPaths( } std::optional calculate_centerline_path( - const geometry_msgs::msg::Pose & original_goal_pose, const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) { + const auto original_goal_pose = planner_data->route_handler->getOriginalGoalPose(); const auto refined_goal_opt = autoware::behavior_path_planner::goal_planner_utils::calcRefinedGoal( original_goal_pose, planner_data->route_handler, true, @@ -434,78 +492,45 @@ std::optional calculate_centerline_path( return center_line_path; } +std::vector getBusStopAreaPolygons( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + const auto pull_over_lanes = + autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes( + *(planner_data->route_handler), true, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + return autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons( + pull_over_lanes); +} + +struct SortByWeightedDistance +{ + double lateral_cost{0.0}; + bool prioritize_goals_before_objects{false}; + + SortByWeightedDistance(double cost, bool prioritize_goals_before_objects) + : lateral_cost(cost), prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < + b.distance_from_original_goal + lateral_cost * b.lateral_offset; + } +}; + int main(int argc, char ** argv) { using autoware::behavior_path_planner::utils::getReferencePath; rclcpp::init(argc, argv); - autoware_planning_msgs::msg::LaneletRoute route_msg; - route_msg.start_pose = - geometry_msgs::build() - .position(geometry_msgs::build().x(729.944).y(695.124).z(381.18)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.437138).w( - 0.899395)); - route_msg.goal_pose = - geometry_msgs::build() - .position(geometry_msgs::build().x(797.526).y(694.105).z(381.18)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(-0.658723).w( - 0.752386)); - - route_msg.segments = std::vector{ - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15214) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15214) - .primitive_type("lane"), - autoware_planning_msgs::build() - .id(15213) - .primitive_type("lane"), - }), - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15226) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15226) - .primitive_type("lane"), - autoware_planning_msgs::build() - .id(15225) - .primitive_type("lane"), - }), - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15228) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15228) - .primitive_type("lane"), - autoware_planning_msgs::build() - .id(15229) - .primitive_type("lane"), - }), - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15231) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15231) - .primitive_type("lane"), - }), - }; - route_msg.allow_modification = false; - auto node_options = rclcpp::NodeOptions{}; node_options.parameter_overrides( std::vector{{"launch_modules", std::vector{}}}); @@ -537,11 +562,12 @@ int main(int argc, char ** argv) node, ament_index_cpp::get_package_share_directory("autoware_test_utils") + "/test_map/road_shoulder/lanelet2_map.osm", - route_msg); + ament_index_cpp::get_package_share_directory("autoware_behavior_path_goal_planner_module") + + "/config/sample_planner_data_case2.yaml"); lanelet::ConstLanelet current_route_lanelet; planner_data->route_handler->getClosestLaneletWithinRoute( - route_msg.start_pose, ¤t_route_lanelet); + planner_data->self_odometry->pose.pose, ¤t_route_lanelet); const auto reference_path = getReferencePath(current_route_lanelet, planner_data); auto goal_planner_parameter = autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters( @@ -555,7 +581,16 @@ int main(int argc, char ** argv) lane_departure_checker.setParam(lane_departure_checker_params); const auto footprint = vehicle_info.createFootprint(); autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); - const auto goal_candidates = goal_searcher.search(planner_data); + auto goal_candidates = goal_searcher.search(planner_data); + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance( + goal_planner_parameter.minimum_weighted_distance_lateral_weight, + goal_planner_parameter.prioritize_goals_before_objects)); + std::map goal_id2prio{}; + for (auto i = 0; i < goal_candidates.size(); ++i) { + goal_id2prio[goal_candidates.at(i).id] = i; + } pybind11::scoped_interpreter guard{}; auto plt = matplotlibcpp17::pyplot::import(); @@ -563,42 +598,107 @@ int main(int argc, char ** argv) auto & ax1 = axes[0]; auto & ax2 = axes[1]; - const std::vector ids{15213, 15214, 15225, 15226, 15224, 15227, - 15228, 15229, 15230, 15231, 15232}; + // auto & ax3 = axes[2]; + const std::vector ids{759, 675, 676, 1303, 677, 678}; for (const auto & id : ids) { const auto lanelet = planner_data->route_handler->getLaneletMapPtr()->laneletLayer.get(id); plot_lanelet(ax1, lanelet); plot_lanelet(ax2, lanelet); + // plot_lanelet(ax3, lanelet); } - plot_goal_candidates(ax1, goal_candidates, footprint); + // plot_goal_candidates(ax1, goal_candidates, footprint); - plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + // plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + const auto start = std::chrono::steady_clock::now(); std::vector candidates; - for (const auto & goal_candidate : goal_candidates) { - auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( + for (auto i = 0; i < goal_candidates.size(); ++i) { + const auto & goal_candidate = goal_candidates.at(i); + auto shift_pull_over_planner = autoware::behavior_path_planner::BezierPullOver( *node, goal_planner_parameter, lane_departure_checker); - const auto pull_over_path_opt = - shift_pull_over_planner.plan(goal_candidate, 0, planner_data, reference_path); - if (pull_over_path_opt) { - const auto & pull_over_path = pull_over_path_opt.value(); - const auto & full_path = pull_over_path.full_path(); - candidates.push_back(pull_over_path); - plot_path_with_lane_id(ax1, full_path); + auto pull_over_paths = + shift_pull_over_planner.plans(goal_candidate, 0, planner_data, reference_path); + if (!pull_over_paths.empty()) { + std::copy( + std::make_move_iterator(pull_over_paths.begin()), + std::make_move_iterator(pull_over_paths.end()), std::back_inserter(candidates)); } } - [[maybe_unused]] const auto filtered_paths = selectPullOverPaths( + + const auto filtered_paths = selectPullOverPaths( candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path); + std::cout << filtered_paths.size() << std::endl; + const auto end = std::chrono::steady_clock::now(); + std::cout << "computed candidate bezier paths in " + << std::chrono::duration_cast(end - start).count() * 1.0 / + 1000000 + << "msecs" << std::endl; + std::cout << "filtered " << filtered_paths.size() << "/" << candidates.size() << " paths" + << std::endl; + + /* + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + const auto max_parking_curvature = filtered_path.parking_path_max_curvature(); + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + if (i == 0) { + plot_path_with_lane_id(ax1, filtered_path.parking_path(), color, "most prio", 2.0); + } else { + plot_path_with_lane_id( + ax1, filtered_path.full_path(), color, + std::to_string(prio) + "-th goal(id=" + std::to_string(goal_id) + + "): k_max=" + std::to_string(max_parking_curvature), + 0.5); + } + } + */ + const auto original_goal_pos = planner_data->route_handler->getOriginalGoalPose().position; + ax1.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + if (goal_planner_parameter.bus_stop_area.use_bus_stop_area) { + const auto bus_stop_area_polygons = + getBusStopAreaPolygons(planner_data, goal_planner_parameter); + for (const auto & bus_stop_area_polygon : bus_stop_area_polygons) { + plot_lanelet_polygon(ax1, bus_stop_area_polygon); + } + } - const auto centerline_path = - calculate_centerline_path(route_msg.goal_pose, planner_data, goal_planner_parameter); + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + 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); + for (const auto & path_point : filtered_path.full_path().points) { + const auto pose_footprint = transformVector( + footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); + plot_footprint(ax2, pose_footprint, "blue"); + } + } else if (i % 50 == 0) { + std::cout << "plotting " << i << "-th filtered path" << std::endl; + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + plot_path_with_lane_id(ax1, filtered_path.full_path(), color, "", 2.0); + } + } + ax2.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + + const auto centerline_path = calculate_centerline_path(planner_data, goal_planner_parameter); if (centerline_path) { - plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path"); + // plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path"); } + ax1.set_aspect(Args("equal")); ax2.set_aspect(Args("equal")); - ax1.legend(); ax2.legend(); plt.show(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp new file mode 100644 index 0000000000000..e1e91a1bd8af6 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp @@ -0,0 +1,66 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__BEZIER_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__BEZIER_PULL_OVER_HPP_ + +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" + +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::lane_departure_checker::LaneDepartureChecker; + +class BezierPullOver : public PullOverPlannerBase +{ +public: + BezierPullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const LaneDepartureChecker & lane_departure_checker); + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::BEZIER; } + std::optional plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; + std::vector plans( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output); + +private: + const LaneDepartureChecker lane_departure_checker_; + + const bool left_side_parking_; + + std::vector generateBezierPath( + const GoalCandidate & goal_candidate, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; + + PathWithLaneId generateReferencePath( + const std::shared_ptr planner_data, + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + + static double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__BEZIER_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index e061a0203602f..5b336a7de6acc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -33,6 +33,7 @@ namespace autoware::behavior_path_planner { enum class PullOverPlannerType { SHIFT, + BEZIER, ARC_FORWARD, ARC_BACKWARD, FREESPACE, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index dead1e4fe0f37..232c548b28da3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -22,8 +22,10 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_bezier_sampler autoware_motion_utils autoware_rtc_interface + autoware_test_utils autoware_universe_utils pluginlib rclcpp @@ -34,7 +36,6 @@ ament_index_cpp ament_lint_auto autoware_lint_common - autoware_test_utils ament_cmake diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp new file mode 100644 index 0000000000000..0da5ab5dae38b --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -0,0 +1,404 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp" + +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +namespace autoware::behavior_path_planner +{ +BezierPullOver::BezierPullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const LaneDepartureChecker & lane_departure_checker) +: PullOverPlannerBase{node, parameters}, + lane_departure_checker_{lane_departure_checker}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} +{ +} + +std::optional BezierPullOver::plan( + [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, + [[maybe_unused]] const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) +{ + const auto & route_handler = planner_data->route_handler; + const double min_jerk = parameters_.minimum_lateral_jerk; + const double max_jerk = parameters_.maximum_lateral_jerk; + const double backward_search_length = parameters_.backward_goal_search_length; + const double forward_search_length = parameters_.forward_goal_search_length; + const int shift_sampling_num = parameters_.shift_sampling_num; + [[maybe_unused]] const double jerk_resolution = + std::abs(max_jerk - min_jerk) / shift_sampling_num; + + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + previous_module_output.path, planner_data, backward_search_length, forward_search_length, + /*forward_only_in_route*/ false); + + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + + // find safe one from paths with different jerk + for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { + const auto pull_over_path = generateBezierPath( + modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + lateral_jerk); + if (!pull_over_path.empty()) { + return pull_over_path.at(0); + } + } + + return std::nullopt; +} + +std::vector BezierPullOver::plans( + [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, + [[maybe_unused]] const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) +{ + const auto & route_handler = planner_data->route_handler; + const double min_jerk = parameters_.minimum_lateral_jerk; + const double max_jerk = parameters_.maximum_lateral_jerk; + const double backward_search_length = parameters_.backward_goal_search_length; + const double forward_search_length = parameters_.forward_goal_search_length; + const int shift_sampling_num = parameters_.shift_sampling_num; + [[maybe_unused]] const double jerk_resolution = + std::abs(max_jerk - min_jerk) / shift_sampling_num; + + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + previous_module_output.path, planner_data, backward_search_length, forward_search_length, + /*forward_only_in_route*/ false); + + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + + // find safe one from paths with different jerk + std::vector paths; + for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { + auto pull_over_paths = generateBezierPath( + modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + lateral_jerk); + std::copy( + std::make_move_iterator(pull_over_paths.begin()), + std::make_move_iterator(pull_over_paths.end()), std::back_inserter(paths)); + } + + return paths; +} + +std::vector BezierPullOver::generateBezierPath( + const GoalCandidate & goal_candidate, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const +{ + const double pull_over_velocity = parameters_.pull_over_velocity; + + const auto & goal_pose = goal_candidate.goal_pose; + + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy + const Pose shift_end_pose = goal_pose; + + // calculate lateral shift of previous module path terminal pose from road lane reference path + const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( + generateReferencePath(planner_data, road_lanes, shift_end_pose), + parameters_.center_line_path_interval); + const auto prev_module_path = utils::resamplePathWithSpline( + previous_module_output.path, parameters_.center_line_path_interval); + const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; + + // process previous module path for path shifter input path + // case1) extend path if shift end pose is behind of previous module path terminal pose + // case2) crop path if shift end pose is ahead of previous module path terminal pose + const auto processed_prev_module_path = std::invoke([&]() -> std::optional { + const bool extend_previous_module_path = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > + lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; + if (extend_previous_module_path) { // case1 + // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it + // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is + // the role of the goal planner, and the intermediate zero velocity after extension is + // unnecessary. + return goal_planner_utils::extendPath( + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose, true); + } else { // case2 + return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); + } + }); + if (!processed_prev_module_path || processed_prev_module_path->points.empty()) { + return {}; + } + + // calculate shift length + const Pose & shift_end_pose_prev_module_path = + processed_prev_module_path->points.back().point.pose; + const double shift_end_road_to_target_distance = + autoware::universe_utils::inverseTransformPoint( + shift_end_pose.position, shift_end_pose_prev_module_path) + .y; + + // calculate shift start pose on road lane + const double pull_over_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( + shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); + const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( + processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); + const auto shift_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( + processed_prev_module_path->points, shift_end_pose_prev_module_path.position, + -before_shifted_pull_over_distance); + + std::vector> params; + const size_t n_sample_v_init = 4; + const size_t n_sample_v_final = 4; + const size_t n_sample_acc = 3; + for (unsigned i = 0; i <= n_sample_v_init; ++i) { + for (unsigned j = 0; j <= n_sample_v_final; j++) { + for (unsigned k = 0; k <= n_sample_acc; k++) { + const double v_init_coeff = i * 0.25; + const double v_final_coeff = j * 0.25; + const double acc_coeff = k * (10.0 / 3); + params.emplace_back(v_init_coeff, v_final_coeff, acc_coeff); + } + } + } + + std::vector bezier_paths; + for (const auto & [v_init_coeff, v_final_coeff, acc_coeff] : params) { + // set path shifter and generate shifted path + PathShifter path_shifter{}; + path_shifter.setPath(processed_prev_module_path.value()); + ShiftLine shift_line{}; + shift_line.start = *shift_start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_end_road_to_target_distance; + path_shifter.addShiftLine(shift_line); + ShiftedPath shifted_path{}; + const bool offset_back = true; // offset front side from reference path + if (!path_shifter.generate(&shifted_path, offset_back)) { + continue; + } + const auto from_idx_opt = + autoware::motion_utils::findNearestIndex(shifted_path.path.points, *shift_start_pose); + const auto to_idx_opt = + autoware::motion_utils::findNearestIndex(shifted_path.path.points, shift_end_pose); + if (!from_idx_opt || !to_idx_opt) { + continue; + } + const auto from_idx = from_idx_opt.value(); + const auto to_idx = to_idx_opt.value(); + const auto span = static_cast( + std::max(static_cast(to_idx) - static_cast(from_idx), 0)); + const auto reference_curvature = + autoware::motion_utils::calcCurvature(processed_prev_module_path->points); + const auto & from_pose = shifted_path.path.points[from_idx].point.pose; + const auto & to_pose = shifted_path.path.points[to_idx].point.pose; + const autoware::sampler_common::State initial{ + {from_pose.position.x, from_pose.position.y}, + {0.0, 0.0}, + reference_curvature.at(from_idx), + tf2::getYaw(from_pose.orientation)}; + const autoware::sampler_common::State final{ + {to_pose.position.x, to_pose.position.y}, {0.0, 0.0}, 0.0, tf2::getYaw(to_pose.orientation)}; + // setting the initial velocity to higher gives straight forward path (the steering does not + // change) + const auto bezier_path = + bezier_sampler::sample(initial, final, v_init_coeff, v_final_coeff, acc_coeff); + const auto bezier_points = bezier_path.cartesianWithHeading(span); + for (unsigned i = 0; i + 1 < span; ++i) { + auto & p = shifted_path.path.points[from_idx + i]; + p.point.pose.position.x = bezier_points[i].x(); + p.point.pose.position.y = bezier_points[i].y(); + p.point.pose.orientation = + universe_utils::createQuaternionFromRPY(0.0, 0.0, bezier_points[i].z()); + } + shifted_path.path.points = + autoware::motion_utils::removeOverlapPoints(shifted_path.path.points); + + autoware::motion_utils::insertOrientation(shifted_path.path.points, true); + + // combine road lanes and shoulder lanes to find closest lanelet id + const auto lanes = std::invoke([&]() -> lanelet::ConstLanelets { + auto lanes = road_lanes; + lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); + return lanes; // not copy the value (Return Value Optimization) + }); + + // set goal pose with velocity 0 + { + PathPointWithLaneId p{}; + p.point.longitudinal_velocity_mps = 0.0; + p.point.pose = goal_pose; + lanelet::Lanelet goal_lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lanelet)) { + p.lane_ids = {goal_lanelet.id()}; + } else { + p.lane_ids = shifted_path.path.points.back().lane_ids; + } + shifted_path.path.points.push_back(p); + } + + // set lane_id and velocity to shifted_path + for (size_t i = path_shifter.getShiftLines().front().start_idx; + i < shifted_path.path.points.size() - 1; ++i) { + auto & point = shifted_path.path.points.at(i); + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); + lanelet::Lanelet lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, point.point.pose, &lanelet)) { + point.lane_ids = {lanelet.id()}; // overwrite lane_ids + } else { + point.lane_ids = shifted_path.path.points.at(i - 1).lane_ids; + } + } + + // set pull over path + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + goal_candidate, {std::make_pair(pull_over_velocity, 0)}); + + if (!pull_over_path_opt) { + continue; + } + auto & pull_over_path = pull_over_path_opt.value(); + + // check if the parking path will leave drivable area and lanes + const bool is_in_parking_lots = std::invoke([&]() -> bool { + const auto & p = planner_data->parameters; + const auto parking_lot_polygons = + lanelet::utils::query::getAllParkingLots(planner_data->route_handler->getLaneletMapPtr()); + const auto path_footprints = goal_planner_utils::createPathFootPrints( + pull_over_path.parking_path(), p.base_link2front, p.base_link2rear, p.vehicle_width); + const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { + return std::any_of( + parking_lot_polygons.begin(), parking_lot_polygons.end(), + [&footprint](const auto & polygon) { + return lanelet::geometry::within( + footprint, lanelet::utils::to2D(polygon).basicPolygon()); + }); + }; + return std::all_of( + path_footprints.begin(), path_footprints.end(), + [&is_footprint_in_any_polygon](const auto & footprint) { + return is_footprint_in_any_polygon(footprint); + }); + }); + const bool is_in_lanes = std::invoke([&]() -> bool { + const auto drivable_lanes = + utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes); + const auto & dp = planner_data->drivable_area_expansion_parameters; + const auto expanded_lanes = utils::expandLanelets( + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output.drivable_area_info.drivable_lanes); + return !lane_departure_checker_.checkPathWillLeaveLane( + utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); + }); + if (!is_in_parking_lots && !is_in_lanes) { + continue; + } + bezier_paths.push_back(std::move(pull_over_path)); + } + + return bezier_paths; +} + +PathWithLaneId BezierPullOver::generateReferencePath( + const std::shared_ptr planner_data, const lanelet::ConstLanelets & road_lanes, + const Pose & end_pose) const +{ + const auto & route_handler = planner_data->route_handler; + const Pose & current_pose = planner_data->self_odometry->pose.pose; + const double backward_path_length = planner_data->parameters.backward_path_length; + const double pull_over_velocity = parameters_.pull_over_velocity; + const double deceleration_interval = parameters_.deceleration_interval; + + const auto current_road_arc_coords = lanelet::utils::getArcCoordinates(road_lanes, current_pose); + const double s_start = current_road_arc_coords.length - backward_path_length; + const double s_end = std::max( + lanelet::utils::getArcCoordinates(road_lanes, end_pose).length, + s_start + std::numeric_limits::epsilon()); + auto road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + + // decelerate velocity linearly to minimum pull over velocity + // (or keep original velocity if it is lower than pull over velocity) + for (auto & point : road_lane_reference_path.points) { + const auto arclength = lanelet::utils::getArcCoordinates(road_lanes, point.point.pose).length; + const double distance_to_pull_over_start = + std::clamp(s_end - arclength, 0.0, deceleration_interval); + const auto decelerated_velocity = static_cast( + distance_to_pull_over_start / deceleration_interval * + (point.point.longitudinal_velocity_mps - pull_over_velocity) + + pull_over_velocity); + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, decelerated_velocity); + } + return road_lane_reference_path; +} + +double BezierPullOver::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) +{ + // reverse path for checking from the end point + // note that the sign of curvature is also reversed + PathWithLaneId reversed_path{}; + std::reverse_copy( + path.points.begin(), path.points.end(), std::back_inserter(reversed_path.points)); + + double after_arc_length{0.0}; + + const auto curvature_and_segment_length = + autoware::motion_utils::calcCurvatureAndSegmentLength(reversed_path.points); + + for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) { + const auto & [k, segment_length_pair] = curvature_and_segment_length[i]; + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvature_and_segment_length.size() - 1 + ? segment_length_pair.first + : segment_length_pair.first + segment_length_pair.second; + + // after shifted segment length + const double after_segment_length = + k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + after_arc_length += k > 0 ? offset * (1 + k * dr) : offset / (1 - k * dr); + return after_arc_length; + } + after_arc_length += after_segment_length; + } + + return after_arc_length; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp index a832ef1f5cd39..7c0db035d29e4 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp @@ -40,6 +40,9 @@ struct SamplingParameters std::vector sample( const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params); +Bezier sample( + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, + const double initial_velocity, const double final_velocity, const double acceleration); /// @brief generate a Bezier curve for the given states, velocities, and accelerations Bezier generate( const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp index c36b8f88c8486..637cb0482e033 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp @@ -58,6 +58,33 @@ std::vector sample( } return curves; } +Bezier sample( + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, + const double initial_velocity, const double final_velocity, const double acceleration) +{ + const double distance_initial_to_final = std::sqrt( + (initial.pose.x() - final.pose.x()) * (initial.pose.x() - final.pose.x()) + + (initial.pose.y() - final.pose.y()) * (initial.pose.y() - final.pose.y())); + // Tangent vectors + const Eigen::Vector2d initial_tangent_unit(std::cos(initial.heading), std::sin(initial.heading)); + const Eigen::Vector2d final_tangent_unit(std::cos(final.heading), std::sin(final.heading)); + // Unit vectors + const Eigen::Vector2d initial_normal_unit = initial_tangent_unit.unitOrthogonal(); + const Eigen::Vector2d final_normal_unit = final_tangent_unit.unitOrthogonal(); + const double initial_tangent_length = initial_velocity * distance_initial_to_final; + const double final_tangent_length = final_velocity * distance_initial_to_final; + const double acceleration_length = acceleration * distance_initial_to_final; + const Eigen::Vector2d initial_acceleration = + acceleration_length * initial_tangent_unit + + initial.curvature * initial_tangent_length * initial_tangent_length * initial_normal_unit; + const Eigen::Vector2d final_acceleration = + acceleration_length * final_tangent_unit + + final.curvature * final_tangent_length * final_tangent_length * final_normal_unit; + return generate( + {initial.pose.x(), initial.pose.y()}, {final.pose.x(), final.pose.y()}, + initial_tangent_unit * initial_tangent_length, initial_acceleration, + final_tangent_unit * final_tangent_length, final_acceleration); +} Bezier generate( const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration,