From 425eed84a390aab143d822fee704f079025518dd Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 20 Dec 2024 15:53:58 +0900 Subject: [PATCH] migrating Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 11 +- .../README.md | 41 +- .../config/goal_planner.param.yaml | 4 + .../decision_state.hpp | 86 + .../goal_planner_module.hpp | 558 ++--- .../goal_planner_parameters.hpp | 11 + .../goal_searcher.hpp | 5 +- .../goal_searcher_base.hpp | 11 +- .../manager.hpp | 15 +- .../pull_over_planner/bezier_pull_over.hpp | 66 + .../pull_over_planner/freespace_pull_over.hpp | 54 + .../pull_over_planner/geometric_pull_over.hpp | 71 + .../pull_over_planner_base.hpp | 140 ++ .../pull_over_planner/shift_pull_over.hpp | 68 + .../thread_data.hpp | 140 ++ .../util.hpp | 63 +- .../package.xml | 5 +- .../src/decision_state.cpp | 190 ++ .../src/default_fixed_goal_planner.cpp | 2 - .../src/goal_planner_module.cpp | 2056 +++++++---------- .../src/goal_searcher.cpp | 102 +- .../src/manager.cpp | 72 +- .../pull_over_planner/freespace_pull_over.cpp | 156 ++ .../pull_over_planner/geometric_pull_over.cpp | 83 + .../pull_over_planner_base.cpp | 156 ++ .../src/pull_over_planner/shift_pull_over.cpp | 347 +++ .../src/thread_data.cpp | 66 + .../src/util.cpp | 282 ++- .../path_safety_checker/safety_check.hpp | 21 + .../path_safety_checker/safety_check.cpp | 66 + 30 files changed, 3253 insertions(+), 1695 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp create mode 100644 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 create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp 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 670a61479e875..0de40d6203a2e 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 @@ -1,19 +1,24 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_path_goal_planner_module) +set(ament_cmake_lint_cmake_FOUND TRUE) + find_package(autoware_cmake REQUIRED) autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED + src/pull_over_planner/pull_over_planner_base.cpp + 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/default_fixed_goal_planner.cpp - src/freespace_pull_over.cpp - src/geometric_pull_over.cpp src/goal_searcher.cpp - src/shift_pull_over.cpp src/util.cpp src/goal_planner_module.cpp src/manager.cpp + src/thread_data.cpp + src/decision_state.cpp ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 485ec4cb2a359..928e53b1ccff5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -173,20 +173,19 @@ The gray numbers represent objects to avoid, and you can see that the goal in fr ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | -| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | -| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | -| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | This parameter ensures that the distance between the start of the shoulder lane and the goal is not less than the specified value. It's used to prevent setting goals too close to the beginning of the shoulder lane, which might lead to unsafe or impractical pull-over maneuvers. Increasing this value will force the system to ignore potential goal positions near the start of the shoulder lane, potentially leading to safer and more comfortable pull-over locations. | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** @@ -340,13 +339,13 @@ Then there is the concept of soft and hard margins. Although not currently param #### Parameters for object recognition based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------------------------- | :--- | :------------- | :------------------------------------------------------------------------------------------------------- | :-------------- | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation | [2.0, 1.5, 1.0] | -| object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] | -| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | -| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :------------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------------------------- | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation. It is not strictly the distance between footprints, but the maximum distance when ego and objects are oriented. | [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] | +| object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | ## **safety check** diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 156483df65f20..e31d75d6d78db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -23,6 +23,10 @@ ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 high_curvature_threshold: 0.1 + bus_stop_area: + use_bus_stop_area: false + goal_search_interval: 0.5 + lateral_offset_interval: 0.25 # occupancy grid map occupancy_grid: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp new file mode 100644 index 0000000000000..1092047e65030 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -0,0 +1,86 @@ +// 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__DECISION_STATE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ + +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include +#include + +namespace autoware::behavior_path_planner +{ + +class PathDecisionState +{ +public: + enum class DecisionKind { + NOT_DECIDED, + DECIDING, + DECIDED, + }; + + DecisionKind state{DecisionKind::NOT_DECIDED}; + std::optional deciding_start_time{std::nullopt}; + bool is_stable_safe{false}; + std::optional safe_start_time{std::nullopt}; +}; + +class PathDecisionStateController +{ +public: + explicit PathDecisionStateController(rclcpp::Logger logger) : logger_(logger) {} + + /** + * @brief update current state and save old current state to prev state + */ + void transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const autoware_perception_msgs::msg::PredictedObjects & static_target_objects, + const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded); + + PathDecisionState get_current_state() const { return current_state_; } + +private: + rclcpp::Logger logger_; + + PathDecisionState current_state_{}; + + /** + * @brief update current state and save old current state to prev state + */ + PathDecisionState get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const; +}; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 98ae9e2a941d3..057fbfe0cc566 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -15,24 +15,15 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ -#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "autoware/behavior_path_goal_planner_module/freespace_pull_over.hpp" -#include "autoware/behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/decision_state.hpp" +#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" -#include "autoware/behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/thread_data.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include -#include #include -#include -#include #include #include @@ -41,7 +32,6 @@ #include #include -#include #include #include #include @@ -61,8 +51,6 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; using autoware::freespace_planning_algorithms::AstarParam; using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::PlannerCommonParam; -using autoware::freespace_planning_algorithms::RRTStar; -using autoware::freespace_planning_algorithms::RRTStarParam; using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; @@ -71,189 +59,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckPa using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using autoware::universe_utils::Polygon2d; -#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) - -enum class DecidingPathStatus { - NOT_DECIDED, - DECIDING, - DECIDED, -}; -using DecidingPathStatusWithStamp = std::pair; - -struct PreviousPullOverData -{ - struct SafetyStatus - { - std::optional safe_start_time{}; - bool is_safe{false}; - }; - - void reset() - { - found_path = false; - safety_status = SafetyStatus{}; - deciding_path_status = DecidingPathStatusWithStamp{}; - } - - bool found_path{false}; - SafetyStatus safety_status{}; - DecidingPathStatusWithStamp deciding_path_status{}; -}; - -class ThreadSafeData -{ -public: - ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) - : mutex_(mutex), clock_(clock) - { - } - - bool incrementPathIndex() - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - if (pull_over_path_->incrementPathIndex()) { - last_path_idx_increment_time_ = clock_->now(); - return true; - } - return false; - } - - void set_pull_over_path(const PullOverPath & path) - { - const std::lock_guard lock(mutex_); - pull_over_path_ = std::make_shared(path); - if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } - - last_path_update_time_ = clock_->now(); - } - - void set_pull_over_path(const std::shared_ptr & path) - { - const std::lock_guard lock(mutex_); - pull_over_path_ = path; - if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } - last_path_update_time_ = clock_->now(); - } - - template - void set(Args... args) - { - std::lock_guard lock(mutex_); - (..., set(args)); - } - void set(const GoalCandidates & arg) { set_goal_candidates(arg); } - void set(const std::vector & arg) { set_pull_over_path_candidates(arg); } - void set(const std::shared_ptr & arg) { set_pull_over_path(arg); } - void set(const PullOverPath & arg) { set_pull_over_path(arg); } - void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); } - void set(const BehaviorModuleOutput & arg) { set_last_previous_module_output(arg); } - void set(const PreviousPullOverData & arg) { set_prev_data(arg); } - void set(const CollisionCheckDebugMap & arg) { set_collision_check(arg); } - - void clearPullOverPath() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - } - - bool foundPullOverPath() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - return pull_over_path_->isValidPath(); - } - - PullOverPlannerType getPullOverPlannerType() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return PullOverPlannerType::NONE; - } - - return pull_over_path_->type; - }; - - void reset() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - pull_over_path_candidates_.clear(); - goal_candidates_.clear(); - modified_goal_pose_ = std::nullopt; - last_path_update_time_ = std::nullopt; - last_path_idx_increment_time_ = std::nullopt; - closest_start_pose_ = std::nullopt; - last_previous_module_output_ = std::nullopt; - prev_data_.reset(); - } - - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) - - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) - DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data) - DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) - -private: - std::shared_ptr pull_over_path_{nullptr}; - std::shared_ptr lane_parking_pull_over_path_{nullptr}; - std::vector pull_over_path_candidates_; - GoalCandidates goal_candidates_{}; - std::optional modified_goal_pose_; - std::optional last_path_update_time_; - std::optional last_path_idx_increment_time_; - std::optional closest_start_pose_{}; - std::optional last_previous_module_output_{}; - PreviousPullOverData prev_data_{}; - CollisionCheckDebugMap collision_check_{}; - PredictedObjects static_target_objects_{}; - PredictedObjects dynamic_target_objects_{}; - - std::recursive_mutex & mutex_; - rclcpp::Clock::SharedPtr clock_; -}; - -#undef DEFINE_SETTER_WITH_MUTEX -#undef DEFINE_GETTER_WITH_MUTEX -#undef DEFINE_SETTER_GETTER_WITH_MUTEX - struct FreespacePlannerDebugData { bool is_planning{false}; @@ -267,6 +72,7 @@ struct GoalPlannerDebugData std::vector ego_polygons_expanded{}; lanelet::ConstLanelet expanded_pull_over_lane_between_ego{}; Polygon2d objects_extraction_polygon{}; + utils::path_safety_checker::CollisionCheckDebugMap collision_check{}; }; struct LastApprovalData @@ -300,6 +106,155 @@ struct PoseWithString } }; +struct PullOverContextData +{ + PullOverContextData() = delete; + explicit PullOverContextData( + const bool is_stable_safe_path, const PredictedObjects & static_objects, + const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state, + const bool is_stopped, const LaneParkingResponse & lane_parking_response, + const FreespaceParkingResponse & freespace_parking_response) + : is_stable_safe_path(is_stable_safe_path), + static_target_objects(static_objects), + dynamic_target_objects(dynamic_objects), + prev_state_for_debug(prev_state), + is_stopped(is_stopped), + lane_parking_response(lane_parking_response), + freespace_parking_response(freespace_parking_response) + { + } + // TODO(soblin): make following variables private + bool is_stable_safe_path; + PredictedObjects static_target_objects; + PredictedObjects dynamic_target_objects; + PathDecisionState prev_state_for_debug; + bool is_stopped; + LaneParkingResponse lane_parking_response; + FreespaceParkingResponse freespace_parking_response; + std::optional pull_over_path_opt; + std::optional last_path_update_time; + std::optional last_path_idx_increment_time; + + void update( + const bool is_stable_safe_path_, const PredictedObjects static_target_objects_, + const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_, + const bool is_stopped_, const LaneParkingResponse & lane_parking_response_, + const FreespaceParkingResponse & freespace_parking_response_) + { + is_stable_safe_path = is_stable_safe_path_; + static_target_objects = static_target_objects_; + dynamic_target_objects = dynamic_target_objects_; + prev_state_for_debug = prev_state_for_debug_; + is_stopped = is_stopped_; + lane_parking_response = lane_parking_response_; + freespace_parking_response = freespace_parking_response_; + } +}; + +bool isOnModifiedGoal( + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters); + +bool hasPreviousModulePathShapeChanged( + const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & last_previous_module_output); +bool hasDeviatedFromLastPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output); +bool hasDeviatedFromCurrentPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output); + +bool needPathUpdate( + const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, + const std::optional & modified_goal, + const std::optional & selected_time, const GoalPlannerParameters & parameters); + +bool checkOccupancyGridCollision( + const PathWithLaneId & path, + const std::shared_ptr occupancy_grid_map); + +// freespace parking +std::optional planFreespacePath( + const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects, + std::shared_ptr freespace_planner); + +bool isStopped( + std::deque & odometry_buffer, + const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, + const double velocity_upper); + +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices); + +// Flag class for managing whether a certain callback is running in multi-threading +class ScopedFlag +{ +public: + explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } + + ~ScopedFlag() { flag_.store(false); } + +private: + std::atomic & flag_; +}; + +class LaneParkingPlanner +{ +public: + LaneParkingPlanner( + rclcpp::Node & node, std::mutex & lane_parking_mutex, + const std::optional & request, LaneParkingResponse & response, + std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, + const GoalPlannerParameters & parameters); + rclcpp::Logger getLogger() const { return logger_; } + void onTimer(); + +private: + std::mutex & mutex_; + const std::optional & request_; + LaneParkingResponse & response_; + std::atomic & is_lane_parking_cb_running_; + rclcpp::Logger logger_; + + std::vector> pull_over_planners_; +}; + +class FreespaceParkingPlanner +{ +public: + FreespaceParkingPlanner( + std::mutex & freespace_parking_mutex, const std::optional & request, + FreespaceParkingResponse & response, std::atomic & is_freespace_parking_cb_running, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr freespace_planner) + : mutex_(freespace_parking_mutex), + request_(request), + response_(response), + is_freespace_parking_cb_running_(is_freespace_parking_cb_running), + logger_(logger), + clock_(clock), + freespace_planner_(freespace_planner) + { + } + void onTimer(); + +private: + std::mutex & mutex_; + const std::optional & request_; + FreespaceParkingResponse & response_; + std::atomic & is_freespace_parking_cb_running_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + std::shared_ptr freespace_planner_; + + bool isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const FreespaceParkingRequest & req) const; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -359,18 +314,6 @@ class GoalPlannerModule : public SceneModuleInterface void processOnExit() override; void updateData() override; - void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params); - - void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params); - - void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params); - void postProcess() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( @@ -380,61 +323,16 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - /** - * @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this) - */ - struct GoalPlannerData - { - GoalPlannerData(const PlannerData & planner_data, const GoalPlannerParameters & parameters) - { - initializeOccupancyGridMap(planner_data, parameters); - }; - GoalPlannerParameters parameters; - std::shared_ptr ego_predicted_path_params; - std::shared_ptr objects_filtering_params; - std::shared_ptr safety_check_params; - autoware::universe_utils::LinearRing2d vehicle_footprint; - - PlannerData planner_data; - ModuleStatus current_status; - BehaviorModuleOutput previous_module_output; - // collision detector - // need to be shared_ptr to be used in planner and goal searcher - std::shared_ptr occupancy_grid_map; - std::shared_ptr goal_searcher; - - const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; } - const ModuleStatus & getCurrentStatus() const { return current_status; } - void updateOccupancyGrid(); - GoalPlannerData clone() const; - void update( - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params_, - const std::shared_ptr & objects_filtering_params_, - const std::shared_ptr & safety_check_params_, - const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, - const std::shared_ptr goal_searcher_, - const autoware::universe_utils::LinearRing2d & vehicle_footprint); - - private: - void initializeOccupancyGridMap( - const PlannerData & planner_data, const GoalPlannerParameters & parameters); - }; - std::optional gp_planner_data_{std::nullopt}; - std::mutex gp_planner_data_mutex_; - - // Flag class for managing whether a certain callback is running in multi-threading - class ScopedFlag - { - public: - explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } + std::pair syncWithThreads(); - ~ScopedFlag() { flag_.store(false); } + // NOTE: never access to following variables except in updateData()!!! + std::mutex lane_parking_mutex_; + std::optional lane_parking_request_; + LaneParkingResponse lane_parking_response_; - private: - std::atomic & flag_; - }; + std::mutex freespace_parking_mutex_; + std::optional freespace_parking_request_; + FreespaceParkingResponse freespace_parking_response_; /* * state transitions and plan function used in each state @@ -473,13 +371,11 @@ class GoalPlannerModule : public SceneModuleInterface autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; - // planner - std::vector> pull_over_planners_; - std::unique_ptr freespace_planner_; std::unique_ptr fixed_goal_planner_; // goal searcher std::shared_ptr goal_searcher_; + GoalCandidates goal_candidates_{}; // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on // onFreespaceParkingTimer thread storage may point to while calculation. @@ -492,10 +388,9 @@ class GoalPlannerModule : public SceneModuleInterface autoware::universe_utils::LinearRing2d vehicle_footprint_; - std::recursive_mutex mutex_; - // TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable - mutable ThreadSafeData thread_safe_data_; - + std::optional context_data_{std::nullopt}; + // path_decision_controller is updated in updateData(), and used in plan() + PathDecisionStateController path_decision_controller_{getLogger()}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. @@ -521,18 +416,7 @@ class GoalPlannerModule : public SceneModuleInterface mutable GoalPlannerDebugData debug_data_; mutable PoseWithString debug_stop_pose_with_info_; - // collision check - bool checkOccupancyGridCollision( - const PathWithLaneId & path, - const std::shared_ptr occupancy_grid_map) const; - bool checkObjectsCollision( - const PathWithLaneId & path, const std::vector & curvatures, - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, - const double collision_check_margin, const bool extract_static_objects, - const bool update_debug_data = false) const; - // goal seach - Pose calcRefinedGoal(const Pose & goal_pose) const; GoalCandidates generateGoalCandidates() const; // stop or decelerate @@ -540,52 +424,17 @@ class GoalPlannerModule : public SceneModuleInterface void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; - PathWithLaneId generateStopPath() const; + PathWithLaneId generateStopPath(const PullOverContextData & context_data) const; PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const; - void keepStoppedWithCurrentPath(PathWithLaneId & path) const; + void keepStoppedWithCurrentPath( + const PullOverContextData & ctx_data, PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status - bool isStopped(); - bool isStopped( - std::deque & odometry_buffer, const double time); - bool hasFinishedCurrentPath(); - bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const; + bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); double calcModuleRequestLength() const; - bool needPathUpdate( - const Pose & current_pose, const double path_update_duration, - const GoalPlannerParameters & parameters) const; - bool isStuck( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters); - bool hasDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, - const std::shared_ptr goal_searcher) const; - bool hasNotDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, - const std::shared_ptr goal_searcher) const; - DecidingPathStatusWithStamp checkDecidingPathStatus( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, - const std::shared_ptr goal_searcher) const; - void decideVelocity(); - bool foundPullOverPath() const; + void decideVelocity(PullOverPath & pull_over_path); void updateStatus(const BehaviorModuleOutput & output); // validation @@ -596,58 +445,43 @@ class GoalPlannerModule : public SceneModuleInterface bool isCrossingPossible( const Pose & start_pose, const Pose & end_pose, const lanelet::ConstLanelets lanes) const; bool isCrossingPossible(const PullOverPath & pull_over_path) const; - bool hasEnoughTimePassedSincePathUpdate(const double duration) const; - - // freespace parking - bool planFreespacePath( - std::shared_ptr planner_data, - const std::shared_ptr goal_searcher, - const std::shared_ptr occupancy_grid_map); - bool canReturnToLaneParking(); + bool canReturnToLaneParking(const PullOverContextData & context_data); // plan pull over path - BehaviorModuleOutput planPullOver(); - BehaviorModuleOutput planPullOverAsOutput(); - BehaviorModuleOutput planPullOverAsCandidate(); - std::optional> selectPullOverPath( - const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const double collision_check_margin) const; - std::vector sortPullOverPathCandidatesByGoalPriority( + BehaviorModuleOutput planPullOver(PullOverContextData & context_data); + BehaviorModuleOutput planPullOverAsOutput(PullOverContextData & context_data); + BehaviorModuleOutput planPullOverAsCandidate(PullOverContextData & context_data); + std::optional selectPullOverPath( + const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; // lanes and drivable area std::vector generateDrivableLanes() const; - void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + void setDrivableAreaInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const; // output setter - void setOutput(BehaviorModuleOutput & output); - void updatePreviousData(); + void setOutput( + const std::optional selected_pull_over_path_with_velocity_opt, + const PullOverContextData & context_data, BehaviorModuleOutput & output); - void setModifiedGoal(BehaviorModuleOutput & output) const; - void setTurnSignalInfo(BehaviorModuleOutput & output); + void setModifiedGoal( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const; + void setTurnSignalInfo(const PullOverContextData & context_data, BehaviorModuleOutput & output); // new turn signal - TurnSignalInfo calcTurnSignalInfo(); + TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data); std::optional ignore_signal_{std::nullopt}; - bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const; - bool hasDeviatedFromLastPreviousModulePath( - const std::shared_ptr planner_data) const; - bool hasDeviatedFromCurrentPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) const; - - // timer for generating pull over path candidates in a separate thread - void onTimer(); - void onFreespaceParkingTimer(); - // steering factor void updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type); + const PullOverContextData & context_data, const std::array & pose, + const std::array distance, const uint16_t type); // rtc - std::pair calcDistanceToPathChange() const; + std::pair calcDistanceToPathChange( + const PullOverContextData & context_data) const; // safety check void initializeSafetyCheckParameters(); @@ -659,18 +493,18 @@ class GoalPlannerModule : public SceneModuleInterface */ /** * @brief Checks if the current path is safe. - * @return std::pair - * first: If the path is safe for a certain period of time, true. - * second: If the path is safe in the current state, true. + * @return If the path is safe in the current state, true. */ - std::pair isSafePath( - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + std::pair isSafePath( + const std::shared_ptr planner_data, const bool found_pull_over_path, + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, + const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const; // debug - void setDebugData(); + void setDebugData(const PullOverContextData & context_data); void printParkingPositionError() const; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 1272ea38451ee..299e0a5892e1d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -61,6 +61,12 @@ struct GoalPlannerParameters double ignore_distance_from_lane_start{0.0}; double margin_from_boundary{0.0}; double high_curvature_threshold{0.0}; + struct BusStopArea + { + bool use_bus_stop_area{false}; + double goal_search_interval{0.0}; + double lateral_offset_interval{0.0}; + } bus_stop_area; // occupancy grid map bool use_occupancy_grid_for_goal_search{false}; @@ -113,6 +119,11 @@ struct GoalPlannerParameters AstarParam astar_parameters{}; RRTStarParam rrt_star_parameters{}; + struct BezierParking + { + double pull_over_azimuth_threshold; + } bezier_parking; + // stop condition double maximum_deceleration_for_stop{0.0}; double maximum_jerk_for_stop{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 6bdad3a3063ef..ac4ffe97d6f2d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include @@ -52,7 +51,8 @@ class GoalSearcher : public GoalSearcherBase private: void countObjectsToAvoid( GoalCandidates & goal_candidates, const PredictedObjects & objects, - const std::shared_ptr & planner_data) const; + const std::shared_ptr & planner_data, + const Pose & reference_goal_pose) const; void createAreaPolygons( std::vector original_search_poses, const std::shared_ptr & planner_data); @@ -65,7 +65,6 @@ class GoalSearcher : public GoalSearcherBase const std::shared_ptr & planner_data) const; BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; - bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; LinearRing2d vehicle_footprint_{}; bool left_side_parking_{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp index b7d6ffe0710fe..23a51d1f8c86a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -43,15 +43,9 @@ using GoalCandidates = std::vector; class GoalSearcherBase { public: - explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; } + explicit GoalSearcherBase(const GoalPlannerParameters & parameters) : parameters_{parameters} {} virtual ~GoalSearcherBase() = default; - void setReferenceGoal(const Pose & reference_goal_pose) - { - reference_goal_pose_ = reference_goal_pose; - } - const Pose & getReferenceGoal() const { return reference_goal_pose_; } - MultiPolygon2d getAreaPolygons() const { return area_polygons_; } virtual GoalCandidates search(const std::shared_ptr & planner_data) = 0; virtual void update( @@ -72,8 +66,7 @@ class GoalSearcherBase const PredictedObjects & objects) const = 0; protected: - GoalPlannerParameters parameters_{}; - Pose reference_goal_pose_{}; + const GoalPlannerParameters parameters_; MultiPolygon2d area_polygons_{}; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 1ab7b6cb265a8..6e2aaedd98b0e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -29,22 +29,19 @@ namespace autoware::behavior_path_planner class GoalPlannerModuleManager : public SceneModuleManagerInterface { +public: + static GoalPlannerParameters initGoalPlannerParameters( + rclcpp::Node * node, const std::string & base_ns); + public: GoalPlannerModuleManager() : SceneModuleManagerInterface{"goal_planner"} {} void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override - { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); - } + std::unique_ptr createNewSceneModuleInstance() override; void updateModuleParams(const std::vector & parameters) override; - bool isAlwaysExecutableModule() const override; - bool isSimultaneousExecutableAsApprovedModule() const override; bool isSimultaneousExecutableAsCandidateModule() const override; 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/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp new file mode 100644 index 0000000000000..e32488965f69a --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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__FREESPACE_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__FREESPACE_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::freespace_planning_algorithms::AbstractPlanningAlgorithm; + +class FreespacePullOver : public PullOverPlannerBase +{ +public: + FreespacePullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } + + void setMap(const nav_msgs::msg::OccupancyGrid & costmap) { planner_->setMap(costmap); } + + std::optional plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; + +protected: + const double velocity_; + const bool left_side_parking_; + const bool use_back_; + std::unique_ptr planner_; +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__FREESPACE_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/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp new file mode 100644 index 0000000000000..89181b258fbea --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -0,0 +1,71 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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__GEOMETRIC_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__GEOMETRIC_PULL_OVER_HPP_ + +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" + +#include + +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::lane_departure_checker::LaneDepartureChecker; +class GeometricPullOver : public PullOverPlannerBase +{ +public: + GeometricPullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const LaneDepartureChecker & lane_departure_checker, const bool is_forward); + + PullOverPlannerType getPlannerType() const override + { + return is_forward_ ? PullOverPlannerType::ARC_FORWARD : PullOverPlannerType::ARC_BACKWARD; + } + Pose getCr() const { return planner_.getCr(); } + Pose getCl() const { return planner_.getCl(); } + + 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 generatePullOverPaths( + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const Pose & goal_pose) const; + std::vector selectValidPaths( + const std::vector & paths, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const bool is_in_goal_route_section, + const Pose & goal_pose) const; + bool hasEnoughDistance( + const PullOverPath & path, const lanelet::ConstLanelets & road_lanes, + const bool is_in_goal_route_section, const Pose & goal_pose) const; + +protected: + const ParallelParkingParameters parallel_parking_parameters_; + const LaneDepartureChecker lane_departure_checker_; + const bool is_forward_; + const bool left_side_parking_; + + GeometricParallelParking planner_; +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__GEOMETRIC_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 new file mode 100644 index 0000000000000..5b336a7de6acc --- /dev/null +++ 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 @@ -0,0 +1,140 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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. + +#pragma once + +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" + +#include +#include + +#include +#include +#include + +using autoware::universe_utils::LinearRing2d; +using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; + +namespace autoware::behavior_path_planner +{ +enum class PullOverPlannerType { + SHIFT, + BEZIER, + ARC_FORWARD, + ARC_BACKWARD, + FREESPACE, +}; + +struct PullOverPath +{ +public: + static std::optional create( + const PullOverPlannerType & type, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, + const std::vector> & pairs_terminal_velocity_and_accel); + + PullOverPath(const PullOverPath & other); + PullOverPath & operator=(const PullOverPath & other) = default; + + PullOverPlannerType type() const { return type_; } + size_t goal_id() const { return modified_goal_pose_.id; } + size_t id() const { return id_; } + Pose start_pose() const { return start_pose_; } + Pose modified_goal_pose() const { return modified_goal_pose_.goal_pose; } + const GoalCandidate & modified_goal() const { return modified_goal_pose_; } + + std::vector & partial_paths() { return partial_paths_; } + const std::vector & partial_paths() const { return partial_paths_; } + + // TODO(soblin): use reference to avoid copy once thread-safe design is finished + const PathWithLaneId & full_path() const { return full_path_; } + PathWithLaneId parking_path() const { return parking_path_; } + const std::vector & full_path_curvatures() const { return full_path_curvatures_; } + const std::vector & parking_path_curvatures() const { return parking_path_curvatures_; } + double full_path_max_curvature() const { return full_path_max_curvature_; } + double parking_path_max_curvature() const { return parking_path_max_curvature_; } + size_t path_idx() const { return path_idx_; } + + bool incrementPathIndex(); + + // TODO(soblin): this cannot be const due to decelerateBeforeSearchStart + PathWithLaneId & getCurrentPath(); + + const PathWithLaneId & getCurrentPath() const; + + std::pair getPairsTerminalVelocityAndAccel() const + { + if (pairs_terminal_velocity_and_accel_.size() <= path_idx_) { + return std::make_pair(0.0, 0.0); + } + return pairs_terminal_velocity_and_accel_.at(path_idx_); + } + + std::vector debug_poses{}; + +private: + PullOverPath( + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel); + + PullOverPlannerType type_; + GoalCandidate modified_goal_pose_; + size_t id_; + Pose start_pose_; + + std::vector partial_paths_; + PathWithLaneId full_path_; + PathWithLaneId parking_path_; + std::vector full_path_curvatures_; + std::vector parking_path_curvatures_; + double full_path_max_curvature_; + double parking_path_max_curvature_; + + // accelerate with constant acceleration to the target velocity + size_t path_idx_; + std::vector> pairs_terminal_velocity_and_accel_; +}; + +class PullOverPlannerBase +{ +public: + PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) + : vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, + parameters_{parameters} + { + } + virtual ~PullOverPlannerBase() = default; + + virtual PullOverPlannerType getPlannerType() const = 0; + virtual std::optional plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) = 0; + +protected: + const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + const LinearRing2d vehicle_footprint_; + const GoalPlannerParameters parameters_; +}; +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp new file mode 100644 index 0000000000000..08e0b8508c991 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -0,0 +1,68 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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__SHIFT_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__SHIFT_PULL_OVER_HPP_ + +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" + +#include + +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::lane_departure_checker::LaneDepartureChecker; + +class ShiftPullOver : public PullOverPlannerBase +{ +public: + ShiftPullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const LaneDepartureChecker & lane_departure_checker); + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; + std::optional plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; + +protected: + PathWithLaneId generateReferencePath( + const std::shared_ptr planner_data, + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + std::optional cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; + std::optional generatePullOverPath( + 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; + static double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); + static std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s); + static std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval); + + const LaneDepartureChecker lane_departure_checker_; + + const bool left_side_parking_; +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__SHIFT_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp new file mode 100644 index 0000000000000..1e77cd3bcf191 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -0,0 +1,140 @@ +// 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__THREAD_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ + +#include "autoware/behavior_path_goal_planner_module/decision_state.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +class LaneParkingRequest +{ +public: + LaneParkingRequest( + const GoalPlannerParameters & parameters, + const autoware::universe_utils::LinearRing2d & vehicle_footprint, + const GoalCandidates & goal_candidates, const BehaviorModuleOutput & previous_module_output) + : parameters_(parameters), + vehicle_footprint_(vehicle_footprint), + goal_candidates_(goal_candidates), + previous_module_output_(previous_module_output), + last_previous_module_output_(previous_module_output) + { + } + + void update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const BehaviorModuleOutput & previous_module_output, + const std::optional & pull_over_path, const PathDecisionState & prev_data); + + const GoalPlannerParameters parameters_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; + + const std::shared_ptr & get_planner_data() const { return planner_data_; } + const ModuleStatus & get_current_status() const { return current_status_; } + const BehaviorModuleOutput & get_previous_module_output() const + { + return previous_module_output_; + } + const BehaviorModuleOutput & get_last_previous_module_output() const + { + return last_previous_module_output_; + } + const std::optional & get_pull_over_path() const { return pull_over_path_; } + const PathDecisionState & get_prev_data() const { return prev_data_; } + +private: + std::shared_ptr planner_data_; + ModuleStatus current_status_; + BehaviorModuleOutput previous_module_output_; + BehaviorModuleOutput last_previous_module_output_; + std::optional pull_over_path_; // pull_over_path_candidates; + std::optional closest_start_pose; + std::optional> sorted_bezier_indices_opt; +}; + +class FreespaceParkingRequest +{ +public: + FreespaceParkingRequest( + const GoalPlannerParameters & parameters, + const autoware::universe_utils::LinearRing2d & vehicle_footprint, + const GoalCandidates & goal_candidates, const PlannerData & planner_data) + : parameters_(parameters), + vehicle_footprint_(vehicle_footprint), + goal_candidates_(goal_candidates) + { + initializeOccupancyGridMap(planner_data, parameters_); + }; + + const ModuleStatus & getCurrentStatus() const { return current_status_; } + void update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped); + + const GoalPlannerParameters parameters_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; + + const std::shared_ptr & get_planner_data() const { return planner_data_; } + const ModuleStatus & get_current_status() const { return current_status_; } + std::shared_ptr get_occupancy_grid_map() const + { + return occupancy_grid_map_; + } + const std::optional & get_pull_over_path() const { return pull_over_path_; } + const std::optional & get_last_path_update_time() const + { + return last_path_update_time_; + } + bool is_stopped() const { return is_stopped_; } + +private: + std::shared_ptr planner_data_; + ModuleStatus current_status_; + std::shared_ptr occupancy_grid_map_; + std::optional pull_over_path_; + std::optional last_path_update_time_; + bool is_stopped_; + + void initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters); +}; + +struct FreespaceParkingResponse +{ + std::optional freespace_pull_over_path; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index c4491753cb0a3..f00d9d41622e9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -16,19 +16,20 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include -#include "visualization_msgs/msg/detail/marker_array__struct.hpp" #include #include #include #include #include +#include #include +#include #include #include #include @@ -49,14 +50,6 @@ lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); -/* - * @brief expand pull_over_lanes to the opposite side of drivable roads by bound_offset. - * bound_offset must be positive regardless of left_side is true/false - */ -lanelet::ConstLanelets generateExpandedPullOverLanes( - const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, const double bound_offset); - lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, const geometry_msgs::msg::Pose ego_pose, @@ -75,16 +68,9 @@ std::optional generateObjectExtractionPolygon( const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, const double outer_offset, const double inner_offset); -PredictedObjects extractObjectsInExpandedPullOverLanes( - const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, double bound_offset, const PredictedObjects & objects); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -PredictedObjects extractStaticObjectsInExpandedPullOverLanes( - const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, double bound_offset, const PredictedObjects & objects, - const double velocity_thresh); double calcLateralDeviationBetweenPaths( const PathWithLaneId & reference_path, const PathWithLaneId & target_path); @@ -125,6 +111,32 @@ std::vector createPathFootPrints( const PathWithLaneId & path, const double base_to_front, const double base_to_rear, const double width); +/** + * @brief check if footprint intersects with given areas + */ +bool isIntersectingAreas( + const LinearRing2d & footprint, const std::vector & areas); + +/** + * @brief check if footprint is within one of the areas + */ +bool isWithinAreas( + const LinearRing2d & footprint, const std::vector & areas); + +/** + * @brief query BusStopArea polygons associated with given lanes + */ +std::vector getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes); + +bool checkObjectsCollision( + const PathWithLaneId & path, const std::vector & curvatures, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const BehaviorPathPlannerParameters & behavior_path_parameters, + const double collision_check_margin, const bool extract_static_objects, + const double maximum_deceleration, + const double object_recognition_collision_check_max_extra_stopping_margin, + std::vector & ego_polygons_expanded, const bool update_debug_data = false); + // debug MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, @@ -141,7 +153,13 @@ MarkerArray createLaneletPolygonMarkerArray( MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); - +std::string makePathPriorityDebugMessage( + const std::vector & sorted_path_indices, + const std::vector & pull_over_path_candidates, + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_rough_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature); /** * @brief combine two points * @param points lane points @@ -158,6 +176,15 @@ lanelet::Points3d combineLanePoints( lanelet::Lanelet createDepartureCheckLanelet( const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, const bool left_side_parking); + +std::optional calcRefinedGoal( + const Pose & goal_pose, const std::shared_ptr route_handler, + const bool left_side_parking, const double vehicle_width, const double base_link2front, + const double base_link2rear, const GoalPlannerParameters & parameters); + +std::optional calcClosestPose( + const lanelet::ConstLineString3d line, const Point & query_point); + } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ 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 2f107c5644f2e..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 @@ -2,7 +2,7 @@ autoware_behavior_path_goal_planner_module - 0.1.0 + 0.40.0 The autoware_behavior_path_goal_planner_module package Kosuke Takeuchi @@ -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 @@ -31,6 +33,7 @@ visualization_msgs ament_cmake_ros + ament_index_cpp ament_lint_auto autoware_lint_common diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp new file mode 100644 index 0000000000000..1a4a6e5a25589 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -0,0 +1,190 @@ +// 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/util.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ + +using autoware::motion_utils::calcSignedArcLength; + +void PathDecisionStateController::transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded) +{ + const auto next_state = get_next_state( + found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, + planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, + pull_over_path, ego_polygons_expanded); + current_state_ = next_state; +} + +PathDecisionState PathDecisionStateController::get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const +{ + auto next_state = current_state_; + + // update safety + if (!parameters.safety_check_params.enable_safety_check) { + next_state.is_stable_safe = true; + } else { + if (is_current_safe) { + if (!next_state.safe_start_time) { + next_state.safe_start_time = now; + next_state.is_stable_safe = false; + } else { + next_state.is_stable_safe = + ((now - next_state.safe_start_time.value()).seconds() > + parameters.safety_check_params.keep_unsafe_time); + } + } else { + next_state.safe_start_time = std::nullopt; + next_state.is_stable_safe = false; + } + } + + // Once this function returns true, it will continue to return true thereafter + if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { + return next_state; + } + + // if path is not safe, not decided + if (!found_pull_over_path || !pull_over_path_opt) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & pull_over_path = pull_over_path_opt.value(); + const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; + // If it is dangerous against dynamic objects before approval, do not determine the path. + // This eliminates a unsafe path to be approved + if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & current_path = pull_over_path.getCurrentPath(); + if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { + const double hysteresis_factor = 0.9; + + // check goal pose collision + if ( + modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, + planner_data, static_target_objects)) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; + return next_state; + } + + // check current parking path collision + const auto & parking_path = pull_over_path.parking_path(); + const auto & parking_path_curvatures = pull_over_path.parking_path_curvatures(); + const double margin = + parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; + if (goal_planner_utils::checkObjectsCollision( + parking_path, parking_path_curvatures, static_target_objects, dynamic_target_objects, + planner_data->parameters, margin, + /*extract_static_objects=*/false, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + ego_polygons_expanded, true)) { + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; + return next_state; + } + + if (enable_safety_check && !next_state.is_stable_safe) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if enough time has passed since deciding status starts, transition to DECIDED + constexpr double check_collision_duration = 1.0; + const double elapsed_time_from_deciding = + (now - current_state_.deciding_start_time.value()).seconds(); + if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); + next_state.state = PathDecisionState::DecisionKind::DECIDED; + next_state.deciding_start_time = std::nullopt; + return next_state; + } + + // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); + return next_state; + } + + // if ego is sufficiently close to the start of the nearest candidate path, the path is decided + const auto & current_pose = planner_data->self_odometry->pose.pose; + const size_t ego_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); + + const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + current_path.points, pull_over_path.start_pose().position); + const double dist_to_parking_start_pose = calcSignedArcLength( + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path.start_pose().position, start_pose_segment_idx); + if (dist_to_parking_start_pose > parameters.decide_path_distance) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if object recognition for path collision check is enabled, transition to DECIDING to check + // collision for a certain period of time. Otherwise, transition to DECIDED directly. + if (parameters.use_object_recognition) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + next_state.state = PathDecisionState::DecisionKind::DECIDING; + next_state.deciding_start_time = now; + return next_state; + } + return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 10e0137978bd6..70ad06f0b060b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -14,8 +14,6 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 92aa83ede9f4e..44c3c9ba632cf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -14,16 +14,20 @@ #include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_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/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -32,10 +36,16 @@ #include #include +#include +#include +#include #include +#include #include #include #include +#include +#include #include #include @@ -47,7 +57,6 @@ using autoware::motion_utils::insertDecelPoint; using autoware::universe_utils::calcDistance2d; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::inverseTransformPose; using nav_msgs::msg::OccupancyGrid; namespace autoware::behavior_path_planner @@ -62,18 +71,10 @@ GoalPlannerModule::GoalPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, - thread_safe_data_{mutex_, clock_}, is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false}, debug_stop_pose_with_info_{&stop_pose_} { - LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info_); - lane_departure_checker::Param lane_departure_checker_params; - lane_departure_checker_params.footprint_extra_margin = - parameters->lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); - occupancy_grid_map_ = std::make_shared(); left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; @@ -81,23 +82,6 @@ GoalPlannerModule::GoalPlannerModule( // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); - for (const std::string & planner_type : parameters_->efficient_path_order) { - if (planner_type == "SHIFT" && parameters_->enable_shift_parking) { - pull_over_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker)); - } else if (planner_type == "ARC_FORWARD" && parameters_->enable_arc_forward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, /*is_forward*/ true)); - } else if (planner_type == "ARC_BACKWARD" && parameters_->enable_arc_backward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, /*is_forward*/ false)); - } - } - - if (pull_over_planners_.empty()) { - RCLCPP_ERROR(getLogger(), "Not found enabled planner"); - } - // set selected goal searcher // currently there is only one goal_searcher_type const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); @@ -109,18 +93,27 @@ GoalPlannerModule::GoalPlannerModule( lane_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); lane_parking_timer_ = rclcpp::create_timer( - &node, clock_, lane_parking_period_ns, std::bind(&GoalPlannerModule::onTimer, this), + &node, clock_, lane_parking_period_ns, + [lane_parking_executor = std::make_unique( + node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_, + is_lane_parking_cb_running_, getLogger(), *parameters_)]() { + lane_parking_executor->onTimer(); + }, lane_parking_timer_cb_group_); // freespace parking if (parameters_->enable_freespace_parking) { - freespace_planner_ = std::make_unique(node, *parameters, vehicle_info); + auto freespace_planner = std::make_shared(node, *parameters, vehicle_info); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); freespace_parking_timer_ = rclcpp::create_timer( &node, clock_, freespace_parking_period_ns, - std::bind(&GoalPlannerModule::onFreespaceParkingTimer, this), + [freespace_parking_executor = std::make_unique( + freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_, + is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner)]() { + freespace_parking_executor->onTimer(); + }, freespace_parking_timer_cb_group_); } @@ -137,22 +130,36 @@ GoalPlannerModule::GoalPlannerModule( // time_keeper_->add_reporter(&std::cerr); } -bool GoalPlannerModule::hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output) const +bool isOnModifiedGoal( + const Pose & current_pose, const GoalCandidate & modified_goal, + const GoalPlannerParameters & parameters) { - const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); - if (!last_previous_module_output) { - return true; + return calcDistance2d(current_pose, modified_goal.goal_pose) < parameters.th_arrived_distance; +} + +bool isOnModifiedGoal( + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters) +{ + if (!modified_goal_opt) { + return false; } + return isOnModifiedGoal(current_pose, modified_goal_opt.value(), parameters); +} + +bool hasPreviousModulePathShapeChanged( + const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & last_previous_module_output) +{ // Calculate the lateral distance between each point of the current path and the nearest point of // the last path constexpr double LATERAL_DEVIATION_THRESH = 0.3; for (const auto & p : previous_module_output.path.points) { const size_t nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( - last_previous_module_output->path.points, p.point.pose.position); - const auto seg_front = last_previous_module_output->path.points.at(nearest_seg_idx); - const auto seg_back = last_previous_module_output->path.points.at(nearest_seg_idx + 1); + last_previous_module_output.path.points, p.point.pose.position); + const auto seg_front = last_previous_module_output.path.points.at(nearest_seg_idx); + const auto seg_back = last_previous_module_output.path.points.at(nearest_seg_idx + 1); // Check if the target point is within the segment const Eigen::Vector3d segment_vec{ seg_back.point.pose.position.x - seg_front.point.pose.position.x, @@ -168,7 +175,7 @@ bool GoalPlannerModule::hasPreviousModulePathShapeChanged( continue; } const double lateral_distance = std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output->path.points, p.point.pose.position, nearest_seg_idx)); + last_previous_module_output.path.points, p.point.pose.position, nearest_seg_idx)); if (lateral_distance > LATERAL_DEVIATION_THRESH) { return true; } @@ -176,82 +183,172 @@ bool GoalPlannerModule::hasPreviousModulePathShapeChanged( return false; } -bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath( - const std::shared_ptr planner_data) const +bool hasDeviatedFromLastPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output) { - const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); - if (!last_previous_module_output) { - return true; - } return std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output->path.points, - planner_data->self_odometry->pose.pose.position)) > 0.3; + last_previous_module_output.path.points, + planner_data.self_odometry->pose.pose.position)) > 0.3; } -bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) const +bool hasDeviatedFromCurrentPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output) { constexpr double LATERAL_DEVIATION_THRESH = 0.3; return std::abs(autoware::motion_utils::calcLateralOffset( - previous_module_output.path.points, planner_data->self_odometry->pose.pose.position)) > + previous_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > LATERAL_DEVIATION_THRESH; } +bool needPathUpdate( + const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, + const std::optional & modified_goal, + const std::optional & selected_time, const GoalPlannerParameters & parameters) +{ + const bool has_enough_time_passed = + selected_time ? (now - selected_time.value()).seconds() > path_update_duration : true; + return !isOnModifiedGoal(current_pose, modified_goal, parameters) && has_enough_time_passed; +} + +bool checkOccupancyGridCollision( + const PathWithLaneId & path, + const std::shared_ptr occupancy_grid_map) +{ + if (!occupancy_grid_map) { + return false; + } + const bool check_out_of_range = false; + return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); +} + +std::optional planFreespacePath( + const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects, + std::shared_ptr freespace_planner) +{ + auto goal_candidates = req.goal_candidates_; + auto goal_searcher = std::make_shared(req.parameters_, req.vehicle_footprint_); + goal_searcher->update( + goal_candidates, req.get_occupancy_grid_map(), req.get_planner_data(), static_target_objects); + + for (size_t i = 0; i < goal_candidates.size(); i++) { + const auto goal_candidate = goal_candidates.at(i); + + if (!goal_candidate.is_safe) { + continue; + } + + freespace_planner->setMap(*(req.get_planner_data()->costmap)); + const auto freespace_path = freespace_planner->plan( + goal_candidate, 0, req.get_planner_data(), BehaviorModuleOutput{} + // NOTE: not used so passing {} is OK + ); + if (!freespace_path) { + continue; + } + return freespace_path; + } + return std::nullopt; +} + +bool isStopped( + std::deque & odometry_buffer, + const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, + const double velocity_upper) +{ + odometry_buffer.push_back(self_odometry); + // Delete old data in buffer_stuck_ + while (rclcpp::ok()) { + const auto time_diff = rclcpp::Time(odometry_buffer.back()->header.stamp) - + rclcpp::Time(odometry_buffer.front()->header.stamp); + if (time_diff.seconds() < duration_lower) { + break; + } + odometry_buffer.pop_front(); + } + bool is_stopped = true; + for (const auto & odometry : odometry_buffer) { + const double ego_vel = utils::l2Norm(odometry->twist.twist.linear); + if (ego_vel > velocity_upper) { + is_stopped = false; + break; + } + } + return is_stopped; +} + +LaneParkingPlanner::LaneParkingPlanner( + rclcpp::Node & node, std::mutex & lane_parking_mutex, + const std::optional & request, LaneParkingResponse & response, + std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, + const GoalPlannerParameters & parameters) +: mutex_(lane_parking_mutex), + request_(request), + response_(response), + is_lane_parking_cb_running_(is_lane_parking_cb_running), + logger_(logger) +{ + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + LaneDepartureChecker lane_departure_checker{}; + lane_departure_checker.setVehicleInfo(vehicle_info); + lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + lane_departure_checker.setParam(lane_departure_checker_params); + + for (const std::string & planner_type : parameters.efficient_path_order) { + if (planner_type == "SHIFT" && parameters.enable_shift_parking) { + pull_over_planners_.push_back( + std::make_shared(node, parameters, lane_departure_checker)); + } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) { + pull_over_planners_.push_back(std::make_shared( + node, parameters, lane_departure_checker, /*is_forward*/ true)); + } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) { + pull_over_planners_.push_back(std::make_shared( + node, parameters, lane_departure_checker, /*is_forward*/ false)); + } + } + + if (pull_over_planners_.empty()) { + RCLCPP_ERROR(logger_, "Not found enabled planner"); + } +} + // generate pull over candidate paths -void GoalPlannerModule::onTimer() +void LaneParkingPlanner::onTimer() { const ScopedFlag flag(is_lane_parking_cb_running_); - std::shared_ptr local_planner_data{nullptr}; - std::optional current_status_opt{std::nullopt}; - std::optional previous_module_output_opt{std::nullopt}; - std::shared_ptr occupancy_grid_map{nullptr}; - std::optional parameters_opt{std::nullopt}; - std::shared_ptr ego_predicted_path_params{nullptr}; - std::shared_ptr objects_filtering_params{nullptr}; - std::shared_ptr safety_check_params{nullptr}; - std::shared_ptr goal_searcher{nullptr}; + std::optional local_request_opt; // begin of critical section { - std::lock_guard guard(gp_planner_data_mutex_); - if (gp_planner_data_) { - const auto & gp_planner_data = gp_planner_data_.value(); - local_planner_data = std::make_shared(gp_planner_data.planner_data); - current_status_opt = gp_planner_data.current_status; - previous_module_output_opt = gp_planner_data.previous_module_output; - occupancy_grid_map = gp_planner_data.occupancy_grid_map; - parameters_opt = gp_planner_data.parameters; - ego_predicted_path_params = gp_planner_data.ego_predicted_path_params; - objects_filtering_params = gp_planner_data.objects_filtering_params; - safety_check_params = gp_planner_data.safety_check_params; - goal_searcher = gp_planner_data.goal_searcher; + std::lock_guard guard(mutex_); + if (request_) { + auto & request = request_.value(); + local_request_opt.emplace(request); } } // end of critical section - if ( - !local_planner_data || !current_status_opt || !previous_module_output_opt || - !occupancy_grid_map || !parameters_opt || !ego_predicted_path_params || - !objects_filtering_params || !safety_check_params || !goal_searcher) { - RCLCPP_ERROR( - getLogger(), - "failed to get valid " - "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt/" - "ego_predicted_path_params/objects_filtering_params/safety_check_params/goal_searcher in " - "onTimer"); + if (!local_request_opt) { + RCLCPP_ERROR(logger_, "main thread has not yet set request for LaneParkingPlanner"); return; } - const auto & current_status = current_status_opt.value(); - const auto & previous_module_output = previous_module_output_opt.value(); - const auto & parameters = parameters_opt.value(); + const auto & local_request = local_request_opt.value(); + const auto & parameters = local_request.parameters_; + const auto & goal_candidates = local_request.goal_candidates_; + const auto & local_planner_data = local_request.get_planner_data(); + const auto & current_status = local_request.get_current_status(); + const auto & previous_module_output = local_request.get_previous_module_output(); + const auto & last_previous_module_output = local_request.get_last_previous_module_output(); + const auto & pull_over_path_opt = local_request.get_pull_over_path(); + const auto & prev_data = local_request.get_prev_data(); if (current_status == ModuleStatus::IDLE) { return; } // goals are not yet available. - if (thread_safe_data_.get_goal_candidates().empty()) { + if (goal_candidates.empty()) { return; } @@ -260,26 +357,33 @@ void GoalPlannerModule::onTimer() } // check if new pull over path candidates are needed to be generated + const auto current_state = prev_data.state; const bool need_update = std::invoke([&]() { - if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { + { + std::lock_guard guard(mutex_); + if (response_.pull_over_path_candidates.empty()) { + return true; + } + } + const std::optional modified_goal_opt = + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) + : std::nullopt; + if (isOnModifiedGoal( + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(local_planner_data, previous_module_output)) { + if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, previous_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { - return true; - } - if (hasPreviousModulePathShapeChanged(previous_module_output)) { + if (hasPreviousModulePathShapeChanged(previous_module_output, last_previous_module_output)) { RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } if ( - hasDeviatedFromLastPreviousModulePath(local_planner_data) && - !hasDecidedPath( - local_planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params, goal_searcher)) { + hasDeviatedFromLastPreviousModulePath(*local_planner_data, last_previous_module_output) && + current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } @@ -293,8 +397,6 @@ void GoalPlannerModule::onTimer() return; } - const auto goal_candidates = thread_safe_data_.get_goal_candidates(); - // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( local_planner_data, parameters.backward_goal_search_length, @@ -306,23 +408,19 @@ void GoalPlannerModule::onTimer() const auto planCandidatePaths = [&]( const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { - planner->setPlannerData(local_planner_data); - planner->setPreviousModuleOutput(previous_module_output); - auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { - pull_over_path->goal_id = goal_candidate.id; - pull_over_path->id = path_candidates.size(); - + const auto pull_over_path = planner->plan( + goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); + if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = - lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose).length; + lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose()).length; if (start_arc_length < min_start_arc_length) { min_start_arc_length = start_arc_length; // closest start pose is stop point when not finding safe path - closest_start_pose = pull_over_path->start_pose; + closest_start_pose = pull_over_path->start_pose(); } } }; @@ -362,47 +460,42 @@ void GoalPlannerModule::onTimer() throw std::domain_error("[pull_over] invalid path_priority"); } - // set member variables - thread_safe_data_.set_pull_over_path_candidates(path_candidates); - thread_safe_data_.set_closest_start_pose(closest_start_pose); - RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); - - thread_safe_data_.set_last_previous_module_output(previous_module_output); + // set response + { + std::lock_guard guard(mutex_); + response_.pull_over_path_candidates = path_candidates; + response_.closest_start_pose = closest_start_pose; + RCLCPP_INFO( + getLogger(), "generated %lu pull over path candidates", + response_.pull_over_path_candidates.size()); + } } -void GoalPlannerModule::onFreespaceParkingTimer() +void FreespaceParkingPlanner::onTimer() { const ScopedFlag flag(is_freespace_parking_cb_running_); - std::shared_ptr local_planner_data{nullptr}; - std::optional current_status_opt{std::nullopt}; - std::shared_ptr occupancy_grid_map{nullptr}; - std::optional parameters_opt{std::nullopt}; - std::shared_ptr goal_searcher{nullptr}; + std::optional local_request_opt; // begin of critical section { - std::lock_guard guard(gp_planner_data_mutex_); - if (gp_planner_data_) { - const auto & gp_planner_data = gp_planner_data_.value(); - local_planner_data = std::make_shared(gp_planner_data.planner_data); - current_status_opt = gp_planner_data.current_status; - occupancy_grid_map = gp_planner_data.occupancy_grid_map; - parameters_opt = gp_planner_data.parameters; - goal_searcher = gp_planner_data.goal_searcher; + std::lock_guard guard(mutex_); + if (request_) { + auto & request = request_.value(); + local_request_opt.emplace(request); } } // end of critical section - if (!local_planner_data || !current_status_opt || !parameters_opt || !goal_searcher) { - RCLCPP_ERROR( - getLogger(), - "failed to get valid planner_data/current_status/parameters/goal_searcher in " - "onFreespaceParkingTimer"); + if (!local_request_opt) { + RCLCPP_ERROR(logger_, "main thread has not yet set request for FreespaceParkingPlanner"); return; } - - const auto & current_status = current_status_opt.value(); - const auto & parameters = parameters_opt.value(); + const auto & local_request = local_request_opt.value(); + const auto & parameters = local_request.parameters_; + const auto & local_planner_data = local_request.get_planner_data(); + const auto & current_status = local_request.get_current_status(); + const auto & pull_over_path_opt = local_request.get_pull_over_path(); + const auto & last_path_update_time = local_request.get_last_path_update_time(); if (current_status == ModuleStatus::IDLE) { return; @@ -416,107 +509,86 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } - if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { + const std::optional modified_goal_opt = + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) + : std::nullopt; + if (isOnModifiedGoal( + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return; } + const double vehicle_width = local_planner_data->parameters.vehicle_width; + const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(local_planner_data->route_handler), left_side_parking, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( + pull_over_lanes, left_side_parking, parameters.detection_bound_offset, + parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width); + + PredictedObjects dynamic_target_objects{}; + for (const auto & object : local_planner_data->dynamic_object->objects) { + const auto object_polygon = universe_utils::toPolygon2d(object); + if ( + objects_extraction_polygon.has_value() && + boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { + dynamic_target_objects.objects.push_back(object); + } + } + const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_target_objects, parameters.th_moving_object_velocity); + const bool is_new_costmap = (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; if ( - isStuck(local_planner_data, occupancy_grid_map, parameters) && is_new_costmap && + isStuck(static_target_objects, dynamic_target_objects, local_request) && is_new_costmap && needPathUpdate( - local_planner_data->self_odometry->pose.pose, path_update_duration, parameters)) { - planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); + local_planner_data->self_odometry->pose.pose, path_update_duration, clock_->now(), + modified_goal_opt, last_path_update_time, parameters)) { + const auto freespace_path_opt = + planFreespacePath(local_request, static_target_objects, freespace_planner_); + if (freespace_path_opt) { + std::lock_guard guard(mutex_); + response_.freespace_pull_over_path = freespace_path_opt.value(); + } } } -void GoalPlannerModule::updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params) -{ - ego_predicted_path_params = - std::make_shared(goal_planner_params->ego_predicted_path_params); -} - -void GoalPlannerModule::updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params) -{ - safety_check_params = - std::make_shared(goal_planner_params->safety_check_params); -} - -void GoalPlannerModule::updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params) +std::pair GoalPlannerModule::syncWithThreads() { - objects_filtering_params = - std::make_shared(goal_planner_params->objects_filtering_params); -} - -void GoalPlannerModule::updateData() -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - // extract static and dynamic objects in extraction polygon for path coliision check - { - const auto & p = parameters_; - const auto & rh = *(planner_data_->route_handler); - const auto objects = *(planner_data_->dynamic_object); - const double vehicle_width = planner_data_->parameters.vehicle_width; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); - const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( - pull_over_lanes, left_side_parking_, p->detection_bound_offset, - p->margin_from_boundary + p->max_lateral_offset + vehicle_width); - if (objects_extraction_polygon.has_value()) { - debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); - } - PredictedObjects dynamic_target_objects{}; - for (const auto & object : objects.objects) { - const auto object_polygon = universe_utils::toPolygon2d(object); - if ( - objects_extraction_polygon.has_value() && - boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { - dynamic_target_objects.objects.push_back(object); - } - } - const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_target_objects, p->th_moving_object_velocity); - - // these objects are used for path collision check not for safety check - thread_safe_data_.set_static_target_objects(static_target_objects); - thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects); - } - // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to - // gp_planner_data_ here + // lane_parking_request/freespace_parking_request - // NOTE: onTimer/onFreespaceParkingTimer copies gp_planner_data_ to their local clone, so we need - // to lock gp_planner_data_ here to avoid data race. But the following clone process is - // lightweight because most of the member variables of PlannerData/RouteHandler is - // shared_ptrs/bool + std::optional pull_over_path = + context_data_ ? context_data_.value().pull_over_path_opt : std::nullopt; + std::optional last_path_update_time = + context_data_ ? context_data_.value().last_path_update_time : std::nullopt; + + // NOTE: Following clone process is rather lightweight because most of the member variables of + // PlannerData/RouteHandler is shared_ptrs // begin of critical section + LaneParkingResponse lane_parking_response; { - std::lock_guard guard(gp_planner_data_mutex_); - if (!gp_planner_data_) { - gp_planner_data_ = GoalPlannerData(*planner_data_, *parameters_); + std::lock_guard guard(lane_parking_mutex_); + if (!lane_parking_request_) { + lane_parking_request_.emplace( + *parameters_, vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); } - auto & gp_planner_data = gp_planner_data_.value(); // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that // planner_data_ is not nullptr, so it is OK to copy as value // By copying PlannerData as value, the internal shared member variables are also copied - // (reference count is incremented), so `gp_planner_data_.foo` is now thread-safe from the - // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) - // and if these two coincided, only the reference count is affected - gp_planner_data.update( - *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_, - *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), goal_searcher_, - vehicle_footprint_); + // (reference count is incremented), so `lane_parking_request_local.planner_data_.foo` is now + // thread-safe from the **re-pointing** by `planner_data_->foo = msg` in + // behavior_path_planner::onCallbackFor(msg) and if these two coincided, only the reference + // count is affected + lane_parking_request_.value().update( + *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path, + path_decision_controller_.get_current_state()); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since // behavior_path_planner::run() updates @@ -524,19 +596,71 @@ void GoalPlannerModule::updateData() // to copy route_handler as value to use lanelet_map_ptr_/routing_graph_ptr_ thread-safely in // onTimer/onFreespaceParkingTimer // TODO(Mamoru Sobue): If the copy of RouteHandler.road_lanelets/shoulder_lanelets is not - // lightweight, we should update gp_planner_data_.route_handler only when + // lightweight, we should update lane_parking_request.planner_data.route_handler only when // `planner_data_.is_route_handler_updated` variable is set true by behavior_path_planner - // (although this flag is not implemented yet). In that case, gp_planner_data members except - // for route_handler should be copied from planner_data_ + // (although this flag is not implemented yet). In that case, lane_parking_request members + // except for route_handler should be copied from planner_data_ + lane_parking_response = lane_parking_response_; + } - // GoalPlannerModule::occupancy_grid_map_ and gp_planner_data.occupancy_grid_map share the - // ownership, and gp_planner_data.occupancy_grid_map maybe also shared by the local - // planner_data on onFreespaceParkingTimer thread local memory space. So following operation - // is thread-safe because gp_planner_data.occupancy_grid_map is only re-pointed here and its - // prior resource is still owned by the onFreespaceParkingTimer thread locally. - occupancy_grid_map_ = gp_planner_data.occupancy_grid_map; + FreespaceParkingResponse freespace_parking_response; + { + std::lock_guard guard(freespace_parking_mutex_); + if (!freespace_parking_request_) { + freespace_parking_request_.emplace( + *parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); + } + constexpr double stuck_time = 5.0; + freespace_parking_request_.value().update( + *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time, + isStopped( + odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time, + parameters_->th_stopped_velocity)); + // TODO(soblin): currently, ogm-based-collision-detector is updated to latest in + // freespace_parking_request_.value().update, and it is shared with goal_planner_module. Next, + // goal_planner_module update it and pass it to freespace_parking_request. + occupancy_grid_map_ = freespace_parking_request_.value().get_occupancy_grid_map(); + freespace_parking_response = freespace_parking_response_; } // end of critical section + return {lane_parking_response, freespace_parking_response}; +} + +void GoalPlannerModule::updateData() +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // extract static and dynamic objects in extraction polygon for path collision check + const auto & p = parameters_; + const auto & rh = *(planner_data_->route_handler); + const auto objects = *(planner_data_->dynamic_object); + const double vehicle_width = planner_data_->parameters.vehicle_width; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); + const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( + pull_over_lanes, left_side_parking_, p->detection_bound_offset, + p->margin_from_boundary + p->max_lateral_offset + vehicle_width); + if (objects_extraction_polygon.has_value()) { + debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); + } + PredictedObjects dynamic_target_objects{}; + for (const auto & object : objects.objects) { + const auto object_polygon = universe_utils::toPolygon2d(object); + if ( + objects_extraction_polygon.has_value() && + boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { + dynamic_target_objects.objects.push_back(object); + } + } + const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_target_objects, p->th_moving_object_velocity); + + // update goal searcher and generate goal candidates + if (goal_candidates_.empty()) { + goal_candidates_ = generateGoalCandidates(); + } + + auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { return; @@ -546,33 +670,79 @@ void GoalPlannerModule::updateData() resetPathReference(); path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - // update goal searcher and generate goal candidates - if (thread_safe_data_.get_goal_candidates().empty()) { - goal_searcher_->setReferenceGoal( - calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose())); - thread_safe_data_.set_goal_candidates(generateGoalCandidates()); + const bool found_pull_over_path = + context_data_ ? context_data_.value().pull_over_path_opt.has_value() : false; + std::optional pull_over_path_recv = + found_pull_over_path + ? std::make_optional(context_data_.value().pull_over_path_opt.value()) + : std::nullopt; + + const auto modified_goal_pose = [&]() -> std::optional { + if (!pull_over_path_recv) { + return std::nullopt; + } + return pull_over_path_recv.value().modified_goal(); + }(); + + // save "old" state + const auto prev_decision_state = path_decision_controller_.get_current_state(); + const auto [is_current_safe, collision_check_map] = isSafePath( + planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_, + ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); + debug_data_.collision_check = collision_check_map; + // update to latest state + path_decision_controller_.transit_state( + found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, + modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, + goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); + + if (context_data_) { + context_data_.value().update( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects, prev_decision_state, + isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity), + lane_parking_response, freespace_parking_response); + } else { + context_data_.emplace( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects, prev_decision_state, + isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity), + lane_parking_response, freespace_parking_response); } + auto & ctx_data = context_data_.value(); if (!isActivated()) { return; } - if (hasFinishedCurrentPath()) { - thread_safe_data_.incrementPathIndex(); + if (hasFinishedCurrentPath(ctx_data)) { + if (ctx_data.pull_over_path_opt) { + auto & pull_over_path = ctx_data.pull_over_path_opt.value(); + if (pull_over_path.incrementPathIndex()) { + ctx_data.last_path_idx_increment_time = clock_->now(); + } + } } if (!last_approval_data_) { last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); - decideVelocity(); + // TODO(soblin): do not "plan" in updateData + if (ctx_data.pull_over_path_opt) decideVelocity(ctx_data.pull_over_path_opt.value()); } } void GoalPlannerModule::initializeSafetyCheckParameters() { - updateEgoPredictedPathParams(ego_predicted_path_params_, parameters_); - updateSafetyCheckParams(safety_check_params_, parameters_); - updateObjectsFilteringParams(objects_filtering_params_, parameters_); + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); } void GoalPlannerModule::processOnExit() @@ -580,7 +750,7 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - thread_safe_data_.reset(); + context_data_ = std::nullopt; last_approval_data_.reset(); } @@ -662,11 +832,19 @@ bool GoalPlannerModule::isExecutionRequested() const lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); - lanelet::ConstLanelet previous_module_terminal_lane{}; + // Get the lanelet of the ego vehicle's side edge at the terminal pose of the previous module's + // path. Check if the lane is the target lane or the neighboring lane. NOTE: This is because in + // the case of avoidance, there is a possibility of base_link entering the neighboring lane, and + // we want to activate the pull over at this time as well. + const Pose previous_terminal_pose = getPreviousModuleOutput().path.points.back().point.pose; + const double vehicle_half_width = planner_data_->parameters.vehicle_width / 2.0; + const Pose previous_terminal_vehicle_edge_pose = calcOffsetPose( + previous_terminal_pose, 0, left_side_parking_ ? vehicle_half_width : -vehicle_half_width, 0); + lanelet::ConstLanelet previous_module_terminal_vehicle_edge_lane{}; route_handler->getClosestLaneletWithinRoute( - getPreviousModuleOutput().path.points.back().point.pose, &previous_module_terminal_lane); + previous_terminal_vehicle_edge_pose, &previous_module_terminal_vehicle_edge_lane); - if (!isCrossingPossible(previous_module_terminal_lane, target_lane)) { + if (!isCrossingPossible(previous_module_terminal_vehicle_edge_lane, target_lane)) { return false; } @@ -675,12 +853,14 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // NOTE(soblin): at least in goal_planner, isExecutionReady is called via super::updateRTCStatus + // from self::postProcess, so returning cached member variable like + // path_decision_controller.get_current_state() is valid if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { - if (!isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_) - .first) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + if (!path_decision_controller_.get_current_state().is_stable_safe) { + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 5000, + "Path is not safe against dynamic objects, so the candidate path is not approved."); return false; } } @@ -707,127 +887,39 @@ double GoalPlannerModule::calcModuleRequestLength() const return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } -Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const -{ - const double vehicle_width = planner_data_->parameters.vehicle_width; - const double base_link2front = planner_data_->parameters.base_link2front; - const double base_link2rear = planner_data_->parameters.base_link2rear; - - const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - - lanelet::Lanelet closest_pull_over_lanelet{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); - - // calc closest center line pose - Pose center_pose{}; - { - // find position - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); - const auto segment = lanelet::utils::getClosestSegment( - lanelet::utils::to2D(lanelet_point), closest_pull_over_lanelet.centerline()); - const auto p1 = segment.front().basicPoint(); - const auto p2 = segment.back().basicPoint(); - const auto direction_vector = (p2 - p1).normalized(); - const auto p1_to_goal = lanelet_point.basicPoint() - p1; - const double s = direction_vector.dot(p1_to_goal); - const auto refined_point = p1 + direction_vector * s; - - center_pose.position.x = refined_point.x(); - center_pose.position.y = refined_point.y(); - center_pose.position.z = refined_point.z(); - - // find orientation - const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, yaw); - center_pose.orientation = tf2::toMsg(tf_quat); - } - - const auto distance_from_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, - left_side_parking_); - if (!distance_from_bound) { - RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); - return goal_pose; - } - - const double sign = left_side_parking_ ? -1.0 : 1.0; - const double offset_from_center_line = - sign * (distance_from_bound.value() + parameters_->margin_from_boundary); - - const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); - - return refined_goal_pose; -} - -bool GoalPlannerModule::planFreespacePath( - std::shared_ptr planner_data, - const std::shared_ptr goal_searcher, - const std::shared_ptr occupancy_grid_map) -{ - GoalCandidates goal_candidates{}; - goal_candidates = thread_safe_data_.get_goal_candidates(); - goal_searcher->update( - goal_candidates, occupancy_grid_map, planner_data, - thread_safe_data_.get_static_target_objects()); - thread_safe_data_.set_goal_candidates(goal_candidates); - debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); - debug_data_.freespace_planner.is_planning = true; - - for (size_t i = 0; i < goal_candidates.size(); i++) { - const auto goal_candidate = goal_candidates.at(i); - { - const std::lock_guard lock(mutex_); - debug_data_.freespace_planner.current_goal_idx = i; - } - - if (!goal_candidate.is_safe) { - continue; - } - freespace_planner_->setPlannerData(planner_data); - auto freespace_path = freespace_planner_->plan(goal_candidate.goal_pose); - freespace_path->goal_id = goal_candidate.id; - if (!freespace_path) { - continue; - } - - { - const std::lock_guard lock(mutex_); - thread_safe_data_.set_pull_over_path(*freespace_path); - thread_safe_data_.set_modified_goal_pose(goal_candidate); - debug_data_.freespace_planner.is_planning = false; - } - - return true; - } - - const std::lock_guard lock(mutex_); - debug_data_.freespace_planner.is_planning = false; - return false; -} - -bool GoalPlannerModule::canReturnToLaneParking() +bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data) { // return only before starting free space parking - if (!isStopped()) { + if (!isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity)) { return false; } - const auto lane_parking_path = thread_safe_data_.get_lane_parking_pull_over_path(); - if (!lane_parking_path) { + if (!context_data.pull_over_path_opt) { return false; } + if (context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { + return false; + } + // TODO(soblin): return from freespace to lane is disabled temporarily, because if + // context_data_with_velocity contained freespace path, since lane_parking_pull_over_path is + // deleted, freespace path is set again + // So context_data need to have old_selected_lane_pull_over_path also, which is only updated + // against lane_pull_over_path in selectPullOverPath() + const auto & lane_parking_path = context_data.pull_over_path_opt.value(); - const PathWithLaneId path = lane_parking_path->getFullPath(); - const std::vector curvatures = lane_parking_path->getFullPathCurvatures(); + const auto & path = lane_parking_path.full_path(); + const auto & curvatures = lane_parking_path.full_path_curvatures(); if ( parameters_->use_object_recognition && - checkObjectsCollision( - path, curvatures, planner_data_, *parameters_, + goal_planner_utils::checkObjectsCollision( + path, curvatures, context_data.static_target_objects, context_data.dynamic_target_objects, + planner_data_->parameters, parameters_->object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false)) { + /*extract_static_objects=*/false, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded)) { return false; } @@ -875,67 +967,30 @@ BehaviorModuleOutput GoalPlannerModule::plan() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOver(); + if (!context_data_) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, " [pull_over] plan() is called without valid context_data"); + } else { + auto & context_data = context_data_.value(); + return planPullOver(context_data); + } } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } -std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins; + const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins; - // ========================================================================================== - // print path priority for debug - const auto debugPrintPathPriority = - [this]( - const std::vector & sorted_pull_over_path_candidates, - const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, - const std::map & path_id_to_margin_map, - const std::function & isSoftMargin, - const std::function & isHighCurvature) { - std::stringstream ss; - - // unsafe goal and it's priority are not visible as debug marker in rviz, - // so exclude unsafe goal from goal_priority - std::map goal_id_and_priority; - { - int priority = 0; - for (const auto & goal_candidate : goal_candidates) { - goal_id_and_priority[goal_candidate.id] = goal_candidate.is_safe ? priority++ : -1; - } - } - - ss << "\n---------------------- path priority ----------------------\n"; - for (size_t i = 0; i < sorted_pull_over_path_candidates.size(); ++i) { - const auto & path = sorted_pull_over_path_candidates[i]; - - // goal_index is same to goal priority including unsafe goal - const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); - const bool is_safe_goal = goal_candidates[goal_index].is_safe; - const int goal_priority = goal_id_and_priority[path.goal_id]; - - ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id << ", goal_id: " << path.goal_id - << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") - << ", margin: " << path_id_to_margin_map.at(path.id) - << (isSoftMargin(path) ? " (soft)" : " (hard)") - << ", curvature: " << path.getParkingPathMaxCurvature() - << (isHighCurvature(path) ? " (high)" : " (low)"); - ss << "\n"; - } - ss << "-----------------------------------------------------------\n"; - RCLCPP_DEBUG_STREAM(getLogger(), ss.str()); - }; - // ========================================================================================== - - const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins; - const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins; - - // (1) Sort pull_over_path_candidates based on the order in goal_candidates + // STEP2: Sort pull over path candidates + // STEP2-1: Sort pull_over_path_candidates based on the order in goal_candidates // Create a map of goal_id to its index in goal_candidates std::map goal_id_to_index; for (size_t i = 0; i < goal_candidates.size(); ++i) { @@ -943,91 +998,102 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri } // Sort pull_over_path_candidates based on the order in goal_candidates - auto sorted_pull_over_path_candidates = pull_over_path_candidates; std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&goal_id_to_index](const auto & a, const auto & b) { - return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; + 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]; + return goal_id_to_index[a.goal_id()] < goal_id_to_index[b.goal_id()]; }); + // Sort the path only when the number of objects to avoid is the same to minimize the + // num_objects_to_avoid. + const auto isSameNumObjectsToAvoid = [&](const PullOverPath & a, const PullOverPath & b) -> bool { + return goal_candidates[goal_id_to_index[a.goal_id()]].num_objects_to_avoid == + goal_candidates[goal_id_to_index[b.goal_id()]].num_objects_to_avoid; + }; + // compare to sort pull_over_path_candidates based on the order in efficient_path_order const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - const auto & order = parameters_->efficient_path_order; - const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); - const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type)); + const auto & order = parameters.efficient_path_order; + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); return a_pos < b_pos; }; // if object recognition is enabled, sort by collision check margin - if (parameters_->use_object_recognition) { - // (2) Sort by collision check margins - const std::vector margins = std::invoke([&]() { - std::vector combined_margins = soft_margins; - combined_margins.insert(combined_margins.end(), hard_margins.begin(), hard_margins.end()); - return combined_margins; - }); + if (parameters.use_object_recognition) { + // STEP2-2: Sort by collision check 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 - auto calcLargestMargin = [&](const PullOverPath & pull_over_path) { - // check has safe goal - const auto goal_candidate_it = std::find_if( - goal_candidates.begin(), goal_candidates.end(), - [pull_over_path](const auto & goal_candidate) { - return goal_candidate.id == pull_over_path.goal_id; - }); - if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { - return 0.0; - } - // check path collision margin - const PathWithLaneId parking_path = pull_over_path.getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); - for (const auto & margin : margins) { - if (!checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data_, *parameters_, margin, - /*extract_static_objects=*/true)) { - return margin; - } + std::map path_id_to_rough_margin_map; + const auto & target_objects = static_target_objects; + for (const size_t i : sorted_path_indices) { + const auto & path = pull_over_path_candidates[i]; + const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( + path.parking_path(), target_objects, planner_data->parameters, false, "max"); + 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; } - return 0.0; - }; - - // Create a map of PullOverPath pointer to largest collision check margin - std::map path_id_to_margin_map; - for (const auto & path : sorted_pull_over_path_candidates) { - path_id_to_margin_map[path.id] = calcLargestMargin(path); } // sorts in descending order so the item with larger margin comes first std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&path_id_to_margin_map](const PullOverPath & a, const PullOverPath & b) { - if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) { + 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 (!isSameNumObjectsToAvoid(a, b)) { return false; } - return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; + 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()]; }); - // (3) Sort by curvature + // 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.getParkingPathMaxCurvature() >= parameters_->high_curvature_threshold; + return path.parking_path_max_curvature() >= parameters.high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_margin_map[path.id]; + 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_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; + 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_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&](const PullOverPath & a, const PullOverPath & b) { + 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 (!isSameNumObjectsToAvoid(a, b)) { + return false; + } + // 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)) { @@ -1037,49 +1103,61 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return false; }); - // (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") { + // 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_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&](const auto & a, const auto & b) { + 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]; + if (!isSameNumObjectsToAvoid(a, b)) { + return false; + } if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { return comparePathTypePriority(a, b); } // otherwise, keep the order. return false; }); - - // debug print path priority sorted by - // - efficient_path_order - // - collision check margin - // - curvature - debugPrintPathPriority( - sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map, - isSoftMargin, isHighCurvature); } + + // debug print path priority sorted by + // - efficient_path_order + // - collision check margin + // - curvature + const std::string path_priority_info_str = goal_planner_utils::makePathPriorityDebugMessage( + sorted_path_indices, pull_over_path_candidates, goal_id_to_index, goal_candidates, + path_id_to_rough_margin_map, isSoftMargin, isHighCurvature); + RCLCPP_DEBUG_STREAM(logger, path_priority_info_str); } else { /** - * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the + * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the * future. sort by curvature is not implemented yet. * Sort pull_over_path_candidates based on the order in efficient_path_order */ - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { std::stable_sort( - sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), - [&](const auto & a, const auto & b) { return comparePathTypePriority(a, b); }); + 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 (!isSameNumObjectsToAvoid(a, b)) { + return false; + } + return comparePathTypePriority(a, b); + }); } } - - return sorted_pull_over_path_candidates; } -std::optional> GoalPlannerModule::selectPullOverPath( +std::optional GoalPlannerModule::selectPullOverPath( + const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const double collision_check_margin) const + const GoalCandidates & goal_candidates) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1087,6 +1165,27 @@ std::optional> GoalPlannerModule::selectP const double backward_length = parameters_->backward_goal_search_length + parameters_->decide_path_distance; const auto & prev_module_output_path = getPreviousModuleOutput().path; + + // STEP1: Filter valid paths before sorting + // NOTE: Since copying pull over path takes time, it is handled by indices + std::vector sorted_path_indices; + sorted_path_indices.reserve(pull_over_path_candidates.size()); + + // STEP1-1: Extract paths which have safe goal + // Create a map of goal_id to GoalCandidate for quick access + std::unordered_map goal_candidate_map; + for (const auto & goal_candidate : goal_candidates) { + goal_candidate_map[goal_candidate.id] = goal_candidate; + } + for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { + const auto & path = pull_over_path_candidates[i]; + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); + if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { + sorted_path_indices.push_back(i); + } + } + + // STEP1-2: Remove paths which do not have enough distance const double prev_path_front_to_goal_dist = calcSignedArcLength( prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, goal_pose.position); @@ -1096,50 +1195,50 @@ std::optional> GoalPlannerModule::selectP } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - prev_module_output_path, planner_data_, backward_length, 0.0, - /*forward_only_in_route*/ false); + prev_module_output_path, planner_data_, backward_length, 0.0, false); const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data_->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), goal_pose_length + parameters_->forward_goal_search_length); }(); - for (const auto & pull_over_path : pull_over_path_candidates) { - // check if goal is safe - const auto goal_candidate_it = std::find_if( - goal_candidates.begin(), goal_candidates.end(), - [pull_over_path](const auto & goal_candidate) { - return goal_candidate.id == pull_over_path.goal_id; - }); - if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { - continue; - } - if (!hasEnoughDistance(pull_over_path, long_tail_reference_path)) { - continue; - } - - // const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), - // 0.5); - const PathWithLaneId parking_path = pull_over_path.getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); + sorted_path_indices.erase( + std::remove_if( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t i) { + return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path); + }), + sorted_path_indices.end()); + + sortPullOverPaths( + planner_data_, *parameters_, pull_over_path_candidates, goal_candidates, + context_data.static_target_objects, getLogger(), sorted_path_indices); + + // STEP3: Select the final pull over path by checking collision to make it as high priority as + // possible + const double collision_check_margin = + parameters_->object_recognition_collision_check_hard_margins.back(); + for (const size_t i : sorted_path_indices) { + const auto & path = pull_over_path_candidates[i]; + const PathWithLaneId & parking_path = path.parking_path(); + const auto & parking_path_curvatures = path.parking_path_curvatures(); if ( parameters_->use_object_recognition && - checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data_, *parameters_, collision_check_margin, - /*extract_static_objects=*/true, - /*update_debug_data=*/true)) { + goal_planner_utils::checkObjectsCollision( + parking_path, parking_path_curvatures, context_data.static_target_objects, + context_data.dynamic_target_objects, planner_data_->parameters, collision_check_margin, + true, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded, true)) { continue; } - if ( parameters_->use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } - - return std::make_pair(pull_over_path, *goal_candidate_it); + return path; } - return {}; } @@ -1157,34 +1256,32 @@ std::vector GoalPlannerModule::generateDrivableLanes() const return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } -void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) +void GoalPlannerModule::setOutput( + const std::optional selected_pull_over_path_with_velocity_opt, + const PullOverContextData & context_data, BehaviorModuleOutput & output) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); output.reference_path = getPreviousModuleOutput().reference_path; - if (!thread_safe_data_.foundPullOverPath()) { + if (!selected_pull_over_path_with_velocity_opt) { // situation : not safe against static objects use stop_path - output.path = generateStopPath(); - RCLCPP_WARN_THROTTLE( + output.path = generateStopPath(context_data); + RCLCPP_INFO_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); - setDrivableAreaInfo(output); + setDrivableAreaInfo(context_data, output); return; } + const auto & pull_over_path = selected_pull_over_path_with_velocity_opt.value(); if ( - parameters_->safety_check_params.enable_safety_check && - !isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_) - .first && + parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints - output.path = - generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath()); - RCLCPP_WARN_THROTTLE( + output.path = generateFeasibleStopPath(pull_over_path.getCurrentPath()); + RCLCPP_INFO_THROTTLE( getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path"); debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); } else { @@ -1192,29 +1289,31 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // before approval) don't stop // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected - auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - keepStoppedWithCurrentPath(current_path); + auto current_path = pull_over_path.getCurrentPath(); + keepStoppedWithCurrentPath(context_data, current_path); output.path = current_path; } - setModifiedGoal(output); - setDrivableAreaInfo(output); + setModifiedGoal(context_data, output); + setDrivableAreaInfo(context_data, output); // set hazard and turn signal if ( - hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_) && + path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::DECIDED && isActivated()) { - setTurnSignalInfo(output); + setTurnSignalInfo(context_data, output); } } -void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +void GoalPlannerModule::setDrivableAreaInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { + if ( + context_data.pull_over_path_opt && + context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1229,13 +1328,14 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const +void GoalPlannerModule::setModifiedGoal( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (thread_safe_data_.foundPullOverPath()) { + if (context_data.pull_over_path_opt) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = thread_safe_data_.get_modified_goal_pose()->goal_pose; + modified_goal.pose = context_data.pull_over_path_opt.value().modified_goal_pose(); modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -1243,10 +1343,11 @@ void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) +void GoalPlannerModule::setTurnSignalInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = calcTurnSignalInfo(); + const auto new_signal = calcTurnSignalInfo(context_data); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -1255,10 +1356,11 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) } void GoalPlannerModule::updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type) + const PullOverContextData & context_data, const std::array & pose, + const std::array distance, const uint16_t type) { - const uint16_t steering_factor_direction = std::invoke([this]() { - const auto turn_signal = calcTurnSignalInfo(); + const uint16_t steering_factor_direction = std::invoke([&]() { + const auto turn_signal = calcTurnSignalInfo(context_data); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { @@ -1271,166 +1373,14 @@ void GoalPlannerModule::updateSteeringFactor( pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, ""); } -bool GoalPlannerModule::hasDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, - const std::shared_ptr goal_searcher) const -{ - return checkDecidingPathStatus( - planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params, goal_searcher) - .first == DecidingPathStatus::DECIDED; -} - -bool GoalPlannerModule::hasNotDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, - const std::shared_ptr goal_searcher) const -{ - return checkDecidingPathStatus( - planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params, goal_searcher) - .first == DecidingPathStatus::NOT_DECIDED; -} - -DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, - const std::shared_ptr goal_searcher) const -{ - const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status; - const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; - - // Once this function returns true, it will continue to return true thereafter - if (prev_status.first == DecidingPathStatus::DECIDED) { - return prev_status; - } - - // if path is not safe, not decided - if (!thread_safe_data_.foundPullOverPath()) { - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // If it is dangerous against dynamic objects before approval, do not determine the path. - // This eliminates a unsafe path to be approved - if ( - enable_safety_check && - !isSafePath( - planner_data, parameters, ego_predicted_path_params, objects_filtering_params, - safety_check_params) - .first && - !isActivated()) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " - "approval"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if object recognition for path collision check is enabled, transition to DECIDED after - // DECIDING for a certain period of time if there is no collision. - const auto pull_over_path = thread_safe_data_.get_pull_over_path(); - const auto current_path = pull_over_path->getCurrentPath(); - if (prev_status.first == DecidingPathStatus::DECIDING) { - const double hysteresis_factor = 0.9; - - // check goal pose collision - const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose(); - if ( - modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( - modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, - planner_data, thread_safe_data_.get_static_target_objects())) { - RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // check current parking path collision - const auto parking_path = pull_over_path->getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path->getParkingPathCurvatures(); - const double margin = - parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; - if (checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data, parameters, margin, - /*extract_static_objects=*/false)) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - if ( - enable_safety_check && !isSafePath( - planner_data, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params) - .first) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if enough time has passed since deciding status starts, transition to DECIDED - constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); - if (elapsed_time_from_deciding > check_collision_duration) { - RCLCPP_DEBUG( - getLogger(), "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); - return {DecidingPathStatus::DECIDED, clock_->now()}; - } - - // if enough time has NOT passed since deciding status starts, keep DECIDING - RCLCPP_DEBUG( - getLogger(), "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", - elapsed_time_from_deciding); - return prev_status; - } - - // if ego is sufficiently close to the start of the nearest candidate path, the path is decided - const auto & current_pose = planner_data->self_odometry->pose.pose; - const size_t ego_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - - const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - current_path.points, pull_over_path->start_pose.position); - const double dist_to_parking_start_pose = calcSignedArcLength( - current_path.points, current_pose.position, ego_segment_idx, - pull_over_path->start_pose.position, start_pose_segment_idx); - if (dist_to_parking_start_pose > parameters.decide_path_distance) { - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if object recognition for path collision check is enabled, transition to DECIDING to check - // collision for a certain period of time. Otherwise, transition to DECIDED directly. - if (parameters.use_object_recognition) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " - "period of time"); - return {DecidingPathStatus::DECIDING, clock_->now()}; - } - RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDED"); - return {DecidingPathStatus::DECIDED, clock_->now()}; -} - -void GoalPlannerModule::decideVelocity() +void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); + // partial_paths + auto & first_path = pull_over_path.partial_paths().front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { @@ -1438,32 +1388,32 @@ void GoalPlannerModule::decideVelocity() } } -BehaviorModuleOutput GoalPlannerModule::planPullOver() +BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_)) { - return planPullOverAsCandidate(); + if ( + path_decision_controller_.get_current_state().state != + PathDecisionState::DecisionKind::DECIDED) { + return planPullOverAsCandidate(context_data); } - return planPullOverAsOutput(); + return planPullOverAsOutput(context_data); } -BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() +BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // if pull over path candidates generation is not finished, use previous module output - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } BehaviorModuleOutput output{}; - const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(); + const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(context_data); output.modified_goal = pull_over_output.modified_goal; - output.path = generateStopPath(); + output.path = generateStopPath(context_data); output.reference_path = getPreviousModuleOutput().reference_path; const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( @@ -1474,94 +1424,122 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { return output; } - setDebugData(); + setDebugData(context_data); return output; } -BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() +BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::chrono::system_clock::time_point start; + start = std::chrono::system_clock::now(); + // if pull over path candidates generation is not finished, use previous module output - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } + /** + NOTE(soblin): this path originates from the previously selected(by main thread) pull_over_path + which was originally generated by either road_parking or freespace thread + */ + auto pull_over_path_with_velocity_opt = context_data.pull_over_path_opt; const bool is_freespace = - thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE; + pull_over_path_with_velocity_opt && + pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE; + const std::optional modified_goal_opt = + pull_over_path_with_velocity_opt + ? std::make_optional(pull_over_path_with_velocity_opt.value().modified_goal()) + : std::nullopt; + const auto & last_path_update_time = context_data.last_path_update_time; if ( - hasNotDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_) && + path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::NOT_DECIDED && + !is_freespace && needPathUpdate( - planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { + planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), + modified_goal_opt, last_path_update_time, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); - thread_safe_data_.clearPullOverPath(); + context_data.pull_over_path_opt = std::nullopt; + context_data.last_path_update_time = std::nullopt; + context_data.last_path_idx_increment_time = std::nullopt; // update goal candidates - auto goal_candidates = thread_safe_data_.get_goal_candidates(); + auto goal_candidates = goal_candidates_; goal_searcher_->update( - goal_candidates, occupancy_grid_map_, planner_data_, - thread_safe_data_.get_static_target_objects()); + goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); - // update pull over path candidates - const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority( - thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); - - // select pull over path which is safe against static objects and get it's goal - const auto path_and_goal_opt = selectPullOverPath( - pull_over_path_candidates, goal_candidates, - parameters_->object_recognition_collision_check_hard_margins.back()); + // Select a path that is as safe as possible and has a high priority. + const auto & pull_over_path_candidates = + context_data.lane_parking_response.pull_over_path_candidates; + const auto lane_pull_over_path_opt = + selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ - if (path_and_goal_opt) { - auto [pull_over_path, modified_goal] = *path_and_goal_opt; - deceleratePath(pull_over_path); - thread_safe_data_.set( - goal_candidates, pull_over_path_candidates, pull_over_path, modified_goal); + const auto & pull_over_path_opt = + lane_pull_over_path_opt ? lane_pull_over_path_opt + : context_data.freespace_parking_response.freespace_pull_over_path; + if (pull_over_path_opt) { + const auto & pull_over_path = pull_over_path_opt.value(); + /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old + * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed + * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of + * member variable + * + * set this selected pull_over_path to ThreadSafeData, but actually RoadParking thread does + * not use pull_over_path, but only FreespaceParking thread use this selected pull_over_path. + * As the next action item, only set this selected pull_over_path to only + * FreespaceThreadSafeData. + */ + context_data.pull_over_path_opt = pull_over_path; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; + + if (pull_over_path_with_velocity_opt) { + auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); + // copy the path for later setOutput() + pull_over_path_with_velocity = pull_over_path; + // modify the velocity for latest setOutput() + deceleratePath(pull_over_path_with_velocity); + } RCLCPP_DEBUG( - getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id, - modified_goal.id); - } else { - thread_safe_data_.set(goal_candidates, pull_over_path_candidates); + getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), + pull_over_path.modified_goal().id); } } // set output and status BehaviorModuleOutput output{}; - setOutput(output); + setOutput(pull_over_path_with_velocity_opt, context_data, output); // return to lane parking if it is possible - if ( - is_freespace && thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && - canReturnToLaneParking()) { - thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); + if (is_freespace && canReturnToLaneParking(context_data)) { + if (pull_over_path_with_velocity_opt) { + context_data.pull_over_path_opt = pull_over_path_with_velocity_opt; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; + } } // For debug - setDebugData(); - if (parameters_->print_debug_info) { - // For evaluations - printParkingPositionError(); - } + setDebugData(context_data); - if (!thread_safe_data_.foundPullOverPath()) { + if (!pull_over_path_with_velocity_opt) { return output; } path_candidate_ = - std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - - updatePreviousData(); + std::make_shared(pull_over_path_with_velocity_opt.value().full_path()); return output; } @@ -1570,66 +1548,37 @@ void GoalPlannerModule::postProcess() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!thread_safe_data_.foundPullOverPath()) { - return; + if (!context_data_) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + " [pull_over] postProcess() is called without valid context_data. use dummy context data."); } + const auto context_data_dummy = PullOverContextData( + true, PredictedObjects{}, PredictedObjects{}, PathDecisionState{}, false /*is _stopped*/, + LaneParkingResponse{}, FreespaceParkingResponse{}); + const auto & context_data = + context_data_.has_value() ? context_data_.value() : context_data_dummy; + + const bool has_decided_path = + path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; - const auto distance_to_path_change = calcDistanceToPathChange(); + if (!context_data.pull_over_path_opt) { + return; + } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); - // TODO(Mamoru Sobue): repetitive call to checkDecidingPathStatus() in the main thread is a - // waste of time because it gives same result throughout the main thread. - const bool has_decided_path = hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_); + const auto distance_to_path_change = calcDistanceToPathChange(context_data); if (has_decided_path) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose, - thread_safe_data_.get_modified_goal_pose()->goal_pose}, + context_data, {pull_over_path.start_pose(), pull_over_path.modified_goal_pose()}, {distance_to_path_change.first, distance_to_path_change.second}, has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); -} - -void GoalPlannerModule::updatePreviousData() -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - // for the next loop setOutput(). - // this is used to determine whether to generate a new stop path or keep the current stop path. - // TODO(Mamoru Sobue): put prev_data_ out of ThreadSafeData - auto prev_data = thread_safe_data_.get_prev_data(); - prev_data.found_path = thread_safe_data_.foundPullOverPath(); - - prev_data.deciding_path_status = checkDecidingPathStatus( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_); - - // This is related to safety check, so if it is disabled, return here. - if (!parameters_->safety_check_params.enable_safety_check) { - prev_data.safety_status.is_safe = true; - } else { - // Even if the current path is safe, it will not be safe unless it stands for a certain period - // of time. Record the time when the current path has become safe - const auto [is_safe, current_is_safe] = isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_); - if (current_is_safe) { - if (!prev_data.safety_status.safe_start_time) { - prev_data.safety_status.safe_start_time = clock_->now(); - } - } else { - prev_data.safety_status.safe_start_time = std::nullopt; - } - prev_data.safety_status.is_safe = is_safe; - } - - // commit the change - thread_safe_data_.set_prev_data(prev_data); + // setVelocityFactor(pull_over_path.full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1637,22 +1586,32 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOverAsCandidate(); + if (!context_data_) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + " [pull_over] planWaitingApproval() is called without valid context_data. use fixed goal " + "planner"); + } else { + auto & context_data = context_data_.value(); + return planPullOverAsCandidate(context_data); + } } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } -std::pair GoalPlannerModule::calcDistanceToPathChange() const +std::pair GoalPlannerModule::calcDistanceToPathChange( + const PullOverContextData & context_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); - const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); + const auto & full_path = pull_over_path.full_path(); const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1662,15 +1621,15 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); + full_path.points, pull_over_path.start_pose().position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + pull_over_path.start_pose().position, start_pose_segment_idx); const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); + full_path.points, pull_over_path.modified_goal_pose().position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); + pull_over_path.modified_goal_pose().position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -1680,7 +1639,7 @@ void GoalPlannerModule::setParameters(const std::shared_ptrgetClosetGoalCandidateAlongLanes( - thread_safe_data_.get_goal_candidates(), planner_data_); + const auto closest_goal_candidate = + goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); @@ -1731,14 +1690,16 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const // difference between the outer and inner sides) // 4. feasible stop const auto stop_pose_with_info = + std::invoke([&]() -> std::optional> { - if (thread_safe_data_.foundPullOverPath()) { + if (context_data.pull_over_path_opt) { return std::make_pair( - thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose"); + context_data.pull_over_path_opt.value().start_pose(), "stop at selected start pose"); } - if (thread_safe_data_.get_closest_start_pose()) { + if (context_data.lane_parking_response.closest_start_pose) { return std::make_pair( - thread_safe_data_.get_closest_start_pose().value(), "stop at closest start pose"); + context_data.lane_parking_response.closest_start_pose.value(), + "stop at closest start pose"); } if (!decel_pose) { return std::nullopt; @@ -1816,70 +1777,38 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId return stop_path; } -bool GoalPlannerModule::isStopped( - std::deque & odometry_buffer, const double time) -{ - const std::lock_guard lock(mutex_); - odometry_buffer.push_back(planner_data_->self_odometry); - // Delete old data in buffer - while (rclcpp::ok()) { - const auto time_diff = rclcpp::Time(odometry_buffer.back()->header.stamp) - - rclcpp::Time(odometry_buffer.front()->header.stamp); - if (time_diff.seconds() < time) { - break; - } - odometry_buffer.pop_front(); - } - bool is_stopped = true; - for (const auto & odometry : odometry_buffer) { - const double ego_vel = utils::l2Norm(odometry->twist.twist.linear); - if (ego_vel > parameters_->th_stopped_velocity) { - is_stopped = false; - break; - } - } - return is_stopped; -} - -bool GoalPlannerModule::isStopped() +bool FreespaceParkingPlanner::isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const FreespaceParkingRequest & req) const { - const std::lock_guard lock(mutex_); - return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); -} - -bool GoalPlannerModule::isStuck( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters) -{ - const std::lock_guard lock(mutex_); - if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, parameters)) { + const auto & parameters = req.parameters_; + const auto & planner_data = req.get_planner_data(); + const std::optional modified_goal_opt = + req.get_pull_over_path() + ? std::make_optional(req.get_pull_over_path().value().modified_goal()) + : std::nullopt; + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } - constexpr double stuck_time = 5.0; - if (!isStopped(odometry_buffer_stuck_, stuck_time)) { + if (!req.is_stopped()) { return false; } - // not found safe path - if (!thread_safe_data_.foundPullOverPath()) { + if (!req.get_pull_over_path()) { return true; } - // any path has never been found - if (!thread_safe_data_.get_pull_over_path()) { - return false; - } - - const auto pull_over_path = thread_safe_data_.get_pull_over_path(); if (parameters.use_object_recognition) { - const auto path = pull_over_path->getCurrentPath(); + const auto & path = req.get_pull_over_path().value().getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); - if (checkObjectsCollision( - path, curvatures, planner_data, parameters, + std::vector ego_polygons_expanded; + if (goal_planner_utils::checkObjectsCollision( + path, curvatures, static_target_objects, dynamic_target_objects, planner_data->parameters, parameters.object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false)) { + /*extract_static_objects=*/false, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + ego_polygons_expanded)) { return true; } } @@ -1887,14 +1816,14 @@ bool GoalPlannerModule::isStuck( if ( parameters.use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), occupancy_grid_map)) { + req.get_pull_over_path().value().getCurrentPath(), req.get_occupancy_grid_map())) { return true; } return false; } -bool GoalPlannerModule::hasFinishedCurrentPath() +bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1902,7 +1831,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath() return false; } - if (!isStopped()) { + if (!ctx_data.is_stopped) { return false; } @@ -1919,43 +1848,42 @@ bool GoalPlannerModule::hasFinishedCurrentPath() // to prevent increment before driving // when the end of the current path is close to the current pose // this value should be `keep_stop_time` in keepStoppedWithCurrentPath + if (!ctx_data.last_path_update_time) { + return false; + } constexpr double keep_current_idx_time = 4.0; const bool has_passed_enough_time_from_increment = - (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > - keep_current_idx_time; + (clock_->now() - ctx_data.last_path_update_time.value()).seconds() > keep_current_idx_time; if (!has_passed_enough_time_from_increment) { return false; } // check if self pose is near the end of current path - const auto current_path_end = - thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); + if (!ctx_data.pull_over_path_opt) { + return false; + } + const auto & current_path_end = + ctx_data.pull_over_path_opt.value().getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; } -bool GoalPlannerModule::isOnModifiedGoal( - const Pose & current_pose, const GoalPlannerParameters & parameters) const -{ - if (!thread_safe_data_.get_modified_goal_pose()) { - return false; - } - - return calcDistance2d(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose) < - parameters.th_arrived_distance; -} - -TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() +TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto path = thread_safe_data_.get_pull_over_path()->getFullPath(); + if (!context_data.pull_over_path_opt) { + return {}; + } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + + const auto & path = pull_over_path.full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; - const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; + const auto & start_pose = pull_over_path.start_pose(); + const auto & end_pose = pull_over_path.modified_goal_pose(); const auto shift_start_idx = autoware::motion_utils::findNearestIndex(path.points, start_pose.position); @@ -1969,7 +1897,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; @@ -1999,12 +1927,11 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_lane_change = false; constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { + if (pull_over_path.type() == PullOverPlannerType::SHIFT) { return false; } constexpr double distance_threshold = 1.0; - const auto stop_point = - thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); + const auto stop_point = pull_over_path.partial_paths().front().points.back(); const double distance_from_ego_to_stop_point = std::abs(autoware::motion_utils::calcSignedArcLength( path.points, stop_point.point.pose.position, current_pose.position)); @@ -2019,88 +1946,6 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() return new_signal; } -bool GoalPlannerModule::checkOccupancyGridCollision( - const PathWithLaneId & path, - const std::shared_ptr occupancy_grid_map) const -{ - if (!occupancy_grid_map) { - return false; - } - const bool check_out_of_range = false; - return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); -} - -bool GoalPlannerModule::checkObjectsCollision( - const PathWithLaneId & path, const std::vector & curvatures, - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, - const double collision_check_margin, const bool extract_static_objects, - const bool update_debug_data) const -{ - const auto target_objects = extract_static_objects - ? thread_safe_data_.get_static_target_objects() - : thread_safe_data_.get_dynamic_target_objects(); - if (target_objects.objects.empty()) { - return false; - } - - // check collision roughly with {min_distance, max_distance} between ego footprint and objects - // footprint - std::pair has_collision_rough = - utils::path_safety_checker::checkObjectsCollisionRough( - path, target_objects, collision_check_margin, planner_data_->parameters, false); - if (!has_collision_rough.first) { - return false; - } - if (has_collision_rough.second) { - return true; - } - - std::vector obj_polygons; - for (const auto & object : target_objects.objects) { - obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); - } - - /* Expand ego collision check polygon - * - `collision_check_margin` is added in all directions. - * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. - * - `extra_lateral_margin` adds the lateral margin on curves. - */ - if (path.points.size() != curvatures.size()) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision"); - return false; - } - std::vector ego_polygons_expanded{}; - for (size_t i = 0; i < path.points.size(); ++i) { - const auto p = path.points.at(i); - const double extra_stopping_margin = std::min( - std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters.maximum_deceleration, - parameters.object_recognition_collision_check_max_extra_stopping_margin); - - // The square is meant to imply centrifugal force, but it is not a very well-founded formula. - // TODO(kosuke55): It is needed to consider better way because there is an inherently - // different conception of the inside and outside margins. - const double extra_lateral_margin = std::min( - extra_stopping_margin, - std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - - const auto ego_polygon = autoware::universe_utils::toFootprint( - p.point.pose, - planner_data->parameters.base_link2front + collision_check_margin + extra_stopping_margin, - planner_data->parameters.base_link2rear + collision_check_margin, - planner_data->parameters.vehicle_width + collision_check_margin * 2.0 + - extra_lateral_margin * 2.0); - ego_polygons_expanded.push_back(ego_polygon); - } - - if (update_debug_data) { - debug_data_.ego_polygons_expanded = ego_polygons_expanded; - } - - return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); -} - bool GoalPlannerModule::hasEnoughDistance( const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const { @@ -2112,9 +1957,9 @@ bool GoalPlannerModule::hasEnoughDistance( // 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 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); + 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; @@ -2139,15 +1984,16 @@ bool GoalPlannerModule::hasEnoughDistance( return true; } -void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) const +void GoalPlannerModule::keepStoppedWithCurrentPath( + const PullOverContextData & ctx_data, PathWithLaneId & path) const { + const auto last_path_idx_increment_time = ctx_data.last_path_idx_increment_time; constexpr double keep_stop_time = 2.0; - if (!thread_safe_data_.get_last_path_idx_increment_time()) { + if (!last_path_idx_increment_time) { return; } - const auto time_diff = - (clock_->now() - *thread_safe_data_.get_last_path_idx_increment_time()).seconds(); + const auto time_diff = (clock_->now() - last_path_idx_increment_time.value()).seconds(); if (time_diff > keep_stop_time) { return; } @@ -2172,13 +2018,15 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // decelerate before the search area start - const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( - thread_safe_data_.get_goal_candidates(), planner_data_); + const auto closest_goal_candidate = + goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( - pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, + pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); - auto & first_path = pull_over_path.partial_paths.front(); + auto & first_path = pull_over_path.partial_paths().front(); if (decel_pose) { decelerateBeforeSearchStart(*decel_pose, first_path); return; @@ -2342,8 +2190,8 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); - const Pose & start_pose = pull_over_path.start_pose; - const Pose & end_pose = pull_over_path.end_pose; + const Pose & start_pose = pull_over_path.start_pose(); + const Pose & end_pose = pull_over_path.modified_goal_pose(); return isCrossingPossible(start_pose, end_pose, lanes); } @@ -2377,7 +2225,7 @@ static std::vector filterOb const auto & target_object_types = params->object_types_to_check; PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( - *objects, ignore_object_velocity_threshold, false); + *objects, ignore_object_velocity_threshold, true); utils::path_safety_checker::filterObjectsByClass(filtered_objects, target_object_types); @@ -2403,16 +2251,24 @@ static std::vector filterOb return refined_filtered_objects; } -std::pair GoalPlannerModule::isSafePath( - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, +std::pair GoalPlannerModule::isSafePath( + const std::shared_ptr planner_data, const bool found_pull_over_path, + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, + const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const { - if (!thread_safe_data_.get_pull_over_path()) { - return {false, false}; + using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; + using autoware::behavior_path_planner::utils::path_safety_checker:: + filterPredictedPathAfterTargetPose; + CollisionCheckDebugMap collision_check{}; + + if (!found_pull_over_path || !pull_over_path_opt) { + return {false, collision_check}; } - const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); + const auto & pull_over_path = pull_over_path_opt.value(); + const auto & current_pull_over_path = pull_over_path.getCurrentPath(); const auto & current_pose = planner_data->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data->self_odometry->twist.twist.linear.x, @@ -2425,11 +2281,8 @@ std::pair GoalPlannerModule::isSafePath( const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters.backward_goal_search_length, parameters.forward_goal_search_length); - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = - utils::parking_departure::getPairsTerminalVelocityAndAccel( - thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, - thread_safe_data_.get_pull_over_path()->path_idx); + pull_over_path.getPairsTerminalVelocityAndAccel(); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); @@ -2438,10 +2291,12 @@ std::pair GoalPlannerModule::isSafePath( // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; const bool limit_to_max_velocity = true; - const auto ego_predicted_path = - autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, pull_over_path.points, current_pose, current_velocity, ego_seg_idx, - is_object_front, limit_to_max_velocity); + const auto ego_seg_idx = planner_data->findEgoIndex(current_pull_over_path.points); + const auto ego_predicted_path_from_current_pose = createPredictedPath( + ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity, + ego_seg_idx, is_object_front, limit_to_max_velocity); + const auto ego_predicted_path = filterPredictedPathAfterTargetPose( + ego_predicted_path_from_current_pose, pull_over_path.start_pose()); // ========================================================================================== // if ego is before the entry of pull_over_lanes, the beginning of the safety check area @@ -2471,8 +2326,8 @@ std::pair GoalPlannerModule::isSafePath( first_road_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); // if current ego pose is before pull over lanes segment, use first road lanelet center pose if ( - calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) < - 0.0) { + calcSignedArcLength( + current_pull_over_path.points, first_road_pose.position, current_pose.position) < 0.0) { return first_road_pose; } // if current ego pose is in pull over lanes segment, use current ego pose @@ -2492,15 +2347,13 @@ std::pair GoalPlannerModule::isSafePath( const auto filtered_objects = filterObjectsByWithinPolicy( dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params); - const auto prev_data = thread_safe_data_.get_prev_data(); const double hysteresis_factor = - prev_data.safety_status.is_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; + prev_data.is_stable_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; - CollisionCheckDebugMap collision_check{}; const bool current_is_safe = std::invoke([&]() { if (parameters.safety_check_params.method == "RSS") { return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( - pull_over_path, ego_predicted_path, filtered_objects, collision_check, + current_pull_over_path, ego_predicted_path, filtered_objects, collision_check, planner_data->parameters, safety_check_params->rss_params, objects_filtering_params->use_all_predicted_path, hysteresis_factor, safety_check_params->collision_check_yaw_diff_threshold); @@ -2516,7 +2369,6 @@ std::pair GoalPlannerModule::isSafePath( parameters.safety_check_params.method.c_str()); throw std::domain_error("[pull_over] invalid safety check method"); }); - thread_safe_data_.set_collision_check(collision_check); /* * ==== is_safe @@ -2531,19 +2383,10 @@ std::pair GoalPlannerModule::isSafePath( * | | | | | | | | * 0 =========================-------==========-- t */ - if (current_is_safe) { - if ( - prev_data.safety_status.safe_start_time && - (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds() > - parameters.safety_check_params.keep_unsafe_time) { - return {true /*is_safe*/, true /*current_is_safe*/}; - } - } - - return {false /*is_safe*/, current_is_safe}; + return {current_is_safe, collision_check}; } -void GoalPlannerModule::setDebugData() +void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2571,19 +2414,16 @@ void GoalPlannerModule::setDebugData() }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = - hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_) - ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const auto color = path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::DECIDED + ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - const auto goal_candidates = thread_safe_data_.get_goal_candidates(); - add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); + add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); // Visualize objects extraction polygon auto marker = autoware::universe_utils::createDefaultMarker( @@ -2608,27 +2448,19 @@ void GoalPlannerModule::setDebugData() add(createPathMarkerArray( getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); - const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); - if (last_previous_module_output.has_value()) { - add(createPathMarkerArray( - last_previous_module_output.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); - } - // Visualize path and related pose - if (thread_safe_data_.foundPullOverPath()) { - add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, - 0.9)); + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + add( + createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); + pull_over_path.modified_goal_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray(pull_over_path.full_path(), "full_path", 0, 0.0, 0.5, 0.9)); + add(createPathMarkerArray(pull_over_path.getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths.size(); ++i) { - const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths.at(i); + for (size_t i = 0; i < pull_over_path.partial_paths().size(); ++i) { + const auto & partial_path = pull_over_path.partial_paths().at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -2661,14 +2493,13 @@ void GoalPlannerModule::setDebugData() } // Visualize debug poses - const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses; + const auto & debug_poses = pull_over_path.debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); } } - auto collision_check = thread_safe_data_.get_collision_check(); // safety check if (parameters_->safety_check_params.enable_safety_check) { if (goal_planner_data_.ego_predicted_path.size() > 0) { @@ -2682,6 +2513,7 @@ void GoalPlannerModule::setDebugData() goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); } + auto collision_check = debug_data_.collision_check; if (parameters_->safety_check_params.method == "RSS") { add(showSafetyCheckInfo(collision_check, "object_debug_info")); } @@ -2699,19 +2531,19 @@ void GoalPlannerModule::setDebugData() // visualize safety status maker { - const auto prev_data = thread_safe_data_.get_prev_data(); + const auto & prev_data = context_data.prev_state_for_debug; visualization_msgs::msg::MarkerArray marker_array{}; - const auto color = prev_data.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "safety_status", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = planner_data_->self_odometry->pose.pose; - marker.text += "is_safe: " + std::to_string(prev_data.safety_status.is_safe) + "\n"; - if (prev_data.safety_status.safe_start_time) { + marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; + if (prev_data.safe_start_time) { const double elapsed_time_from_safe_start = - (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds(); + (clock_->now() - prev_data.safe_start_time.value()).seconds(); marker.text += "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; } @@ -2723,27 +2555,32 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = thread_safe_data_.foundPullOverPath() - ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = context_data.pull_over_path_opt ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = thread_safe_data_.get_modified_goal_pose() - ? thread_safe_data_.get_modified_goal_pose()->goal_pose + marker.pose = context_data.pull_over_path_opt + ? context_data.pull_over_path_opt.value().modified_goal_pose() : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); - if (thread_safe_data_.foundPullOverPath()) { - marker.text += - " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx) + "/" + - std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); - } - - if (isStuck(planner_data_, occupancy_grid_map_, *parameters_)) { + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + marker.text = magic_enum::enum_name(pull_over_path.type()); + marker.text += " " + std::to_string(pull_over_path.path_idx()) + "/" + + std::to_string(pull_over_path.partial_paths().size() - 1); + } + + /* + TODO(soblin): disable until thread safe design is done + if (isStuck( + context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_, + occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; - } else if (isStopped()) { + } + if (isStopped()) { marker.text += " stopped"; } + */ if (debug_data_.freespace_planner.is_planning) { marker.text += @@ -2754,109 +2591,6 @@ void GoalPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } - - // Visualize stop pose info - if (debug_stop_pose_with_info_.pose->has_value()) { - visualization_msgs::msg::MarkerArray stop_pose_marker_array{}; - const auto color = createMarkerColor(1.0, 1.0, 1.0, 0.99); - auto marker = createDefaultMarker( - header.frame_id, header.stamp, "stop_pose_info", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.5), color); - marker.pose = debug_stop_pose_with_info_.pose->value(); - marker.text = debug_stop_pose_with_info_.string; - stop_pose_marker_array.markers.push_back(marker); - add(stop_pose_marker_array); - add(createPoseMarkerArray(marker.pose, "stop_pose", 1.0, 1.0, 1.0, 0.9)); - } -} - -void GoalPlannerModule::printParkingPositionError() const -{ - const auto current_pose = planner_data_->self_odometry->pose.pose; - const double real_shoulder_to_map_shoulder = 0.0; - - const Pose goal_to_ego = - inverseTransformPose(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose); - const double dx = goal_to_ego.position.x; - const double dy = goal_to_ego.position.y; - const double distance_from_real_shoulder = - real_shoulder_to_map_shoulder + parameters_->margin_from_boundary - dy; - RCLCPP_INFO( - getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, - autoware::universe_utils::rad2deg( - tf2::getYaw(current_pose.orientation) - - tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), - distance_from_real_shoulder); -} - -bool GoalPlannerModule::needPathUpdate( - const Pose & current_pose, const double path_update_duration, - const GoalPlannerParameters & parameters) const -{ - return !isOnModifiedGoal(current_pose, parameters) && - hasEnoughTimePassedSincePathUpdate(path_update_duration); -} - -bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const -{ - if (!thread_safe_data_.get_last_path_update_time()) { - return true; - } - - return (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > duration; -} - -void GoalPlannerModule::GoalPlannerData::initializeOccupancyGridMap( - const PlannerData & planner_data, const GoalPlannerParameters & parameters) -{ - OccupancyGridMapParam occupancy_grid_map_param{}; - const double margin = parameters.occupancy_grid_collision_check_margin; - occupancy_grid_map_param.vehicle_shape.length = - planner_data.parameters.vehicle_length + 2 * margin; - occupancy_grid_map_param.vehicle_shape.width = planner_data.parameters.vehicle_width + 2 * margin; - occupancy_grid_map_param.vehicle_shape.base2back = - planner_data.parameters.base_link2rear + margin; - occupancy_grid_map_param.theta_size = parameters.theta_size; - occupancy_grid_map_param.obstacle_threshold = parameters.obstacle_threshold; - occupancy_grid_map = std::make_shared(); - occupancy_grid_map->setParam(occupancy_grid_map_param); -} - -GoalPlannerModule::GoalPlannerData GoalPlannerModule::GoalPlannerData::clone() const -{ - GoalPlannerModule::GoalPlannerData gp_planner_data(planner_data, parameters); - gp_planner_data.update( - parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, - planner_data, current_status, previous_module_output, goal_searcher, vehicle_footprint); - return gp_planner_data; -} - -void GoalPlannerModule::GoalPlannerData::update( - const GoalPlannerParameters & parameters_, - const std::shared_ptr & ego_predicted_path_params_, - const std::shared_ptr & objects_filtering_params_, - const std::shared_ptr & safety_check_params_, - const PlannerData & planner_data_, const ModuleStatus & current_status_, - const BehaviorModuleOutput & previous_module_output_, - const std::shared_ptr goal_searcher_, - const autoware::universe_utils::LinearRing2d & vehicle_footprint_) -{ - parameters = parameters_; - ego_predicted_path_params = ego_predicted_path_params_; - objects_filtering_params = objects_filtering_params_; - safety_check_params = safety_check_params_; - vehicle_footprint = vehicle_footprint_; - - planner_data = planner_data_; - planner_data.route_handler = std::make_shared(*(planner_data_.route_handler)); - current_status = current_status_; - previous_module_output = previous_module_output_; - occupancy_grid_map->setMap(*(planner_data.occupancy_grid)); - // to create a deepcopy of GoalPlanner(not GoalPlannerBase), goal_searcher_ is not enough, so - // recreate it here - goal_searcher = std::make_shared(parameters, vehicle_footprint); - // and then copy the reference goal - goal_searcher->setReferenceGoal(goal_searcher_->getReferenceGoal()); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 4f0a18e01d034..a38289b1c7567 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -15,24 +15,27 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" +#include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" +#include + #include #include +#include #include +#include #include namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::universe_utils::calcOffsetPose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; @@ -101,11 +104,24 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p { GoalCandidates goal_candidates{}; + const auto reference_goal_pose_opt = goal_planner_utils::calcRefinedGoal( + planner_data->route_handler->getOriginalGoalPose(), planner_data->route_handler, + left_side_parking_, planner_data->parameters.vehicle_width, + planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, parameters_); + + if (!reference_goal_pose_opt) { + return goal_candidates; + } + const auto & reference_goal_pose = reference_goal_pose_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 double margin_from_boundary = parameters_.margin_from_boundary; - const double lateral_offset_interval = parameters_.lateral_offset_interval; + const bool use_bus_stop_area = parameters_.bus_stop_area.use_bus_stop_area; + 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 double vehicle_width = planner_data->parameters.vehicle_width; @@ -118,12 +134,18 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( pull_over_lanes, *route_handler, left_side_parking_); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose); 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 = utils::resamplePathWithSpline( - route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), - parameters_.goal_search_interval); + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval); + + const auto no_parking_area_polygons = getNoParkingAreaPolygons(pull_over_lanes); + const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); + const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -147,29 +169,36 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - -distance_from_bound.value() + sign * margin_from_boundary; + use_bus_stop_area ? -distance_from_bound.value() + : -distance_from_bound.value() + sign * margin_from_boundary; // original means non lateral offset poses const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(autoware::motion_utils::calcSignedArcLength( - center_line_path.points, reference_goal_pose_.position, original_search_pose.position)); + center_line_path.points, reference_goal_pose.position, original_search_pose.position)); original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; // search goal_pose in lateral direction - double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { - lateral_offset = dy; search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, autoware::universe_utils::pose2transform(search_pose)); - if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { + if ( + parameters_.bus_stop_area.use_bus_stop_area && + !goal_planner_utils::isWithinAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) { + continue; + } + + if (goal_planner_utils::isIntersectingAreas( + transformed_vehicle_footprint, no_parking_area_polygons)) { // break here to exclude goals located laterally in no_parking_areas break; } - if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { + if (goal_planner_utils::isIntersectingAreas( + transformed_vehicle_footprint, no_stopping_area_polygons)) { // break here to exclude goals located laterally in no_stopping_areas break; } @@ -179,9 +208,26 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p continue; } + // modify the goal_pose orientation so that vehicle footprint front heading is parallel to the + // lane boundary + const auto vehicle_front_midpoint = + (transformed_vehicle_footprint.at(0) + transformed_vehicle_footprint.at(1)) / 2.0; + lanelet::ConstLanelet vehicle_front_closest_lanelet; + lanelet::utils::query::getClosestLanelet( + pull_over_lanes, search_pose, &vehicle_front_closest_lanelet); + const auto vehicle_front_pose_for_bound_opt = goal_planner_utils::calcClosestPose( + left_side_parking_ ? vehicle_front_closest_lanelet.leftBound() + : vehicle_front_closest_lanelet.rightBound(), + autoware::universe_utils::createPoint( + vehicle_front_midpoint.x(), vehicle_front_midpoint.y(), search_pose.position.z)); + if (!vehicle_front_pose_for_bound_opt) { + continue; + } + const auto & vehicle_front_pose_for_bound = vehicle_front_pose_for_bound_opt.value(); GoalCandidate goal_candidate{}; goal_candidate.goal_pose = search_pose; - goal_candidate.lateral_offset = lateral_offset; + goal_candidate.goal_pose.orientation = vehicle_front_pose_for_bound.orientation; + goal_candidate.lateral_offset = dy; goal_candidate.id = goal_id; goal_id++; // use longitudinal_distance as distance_from_original_goal @@ -196,19 +242,19 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p void GoalSearcher::countObjectsToAvoid( GoalCandidates & goal_candidates, const PredictedObjects & objects, - const std::shared_ptr & planner_data) const + const std::shared_ptr & planner_data, const Pose & reference_goal_pose) const { 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; // calculate search start/end pose in pull over lanes - const auto [search_start_pose, search_end_pose] = std::invoke([&]() -> std::pair { + const auto search_start_end_poses = std::invoke([&]() -> std::pair { const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose); 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 auto center_line_path = utils::resamplePathWithSpline( @@ -217,6 +263,8 @@ void GoalSearcher::countObjectsToAvoid( return std::make_pair( center_line_path.points.front().point.pose, center_line_path.points.back().point.pose); }); + const auto search_start_pose = std::get<0>(search_start_end_poses); + const auto search_end_pose = std::get<1>(search_start_end_poses); // generate current lane center line path to check collision with objects const auto current_lanes = utils::getExtendedCurrentLanes( @@ -264,8 +312,18 @@ void GoalSearcher::update( const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, const PredictedObjects & objects) const { + const auto refined_goal_opt = goal_planner_utils::calcRefinedGoal( + planner_data->route_handler->getOriginalGoalPose(), planner_data->route_handler, + left_side_parking_, planner_data->parameters.vehicle_width, + planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, parameters_); + + if (!refined_goal_opt) { + return; + } + + const auto & refined_goal = refined_goal_opt.value(); if (parameters_.prioritize_goals_before_objects) { - countObjectsToAvoid(goal_candidates, objects, planner_data); + countObjectsToAvoid(goal_candidates, objects, planner_data, refined_goal); } if (parameters_.goal_priority == "minimum_weighted_distance") { @@ -488,16 +546,6 @@ BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLane return area_polygons; } -bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const -{ - for (const auto & area : areas) { - if (boost::geometry::intersects(area, footprint)) { - return true; - } - } - return false; -} - GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 2d565607ec610..42c1973c78b87 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -14,7 +14,8 @@ #include "autoware/behavior_path_goal_planner_module/manager.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include @@ -25,14 +26,18 @@ namespace autoware::behavior_path_planner { -void GoalPlannerModuleManager::init(rclcpp::Node * node) + +std::unique_ptr GoalPlannerModuleManager::createNewSceneModuleInstance() { - // init manager interface - initInterface(node, {""}); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); +} +GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( + rclcpp::Node * node, const std::string & base_ns) +{ GoalPlannerParameters p; - - const std::string base_ns = "goal_planner."; // general params { p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); @@ -62,6 +67,12 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "ignore_distance_from_lane_start"); p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); p.high_curvature_threshold = node->declare_parameter(ns + "high_curvature_threshold"); + p.bus_stop_area.use_bus_stop_area = + node->declare_parameter(ns + "bus_stop_area.use_bus_stop_area"); + p.bus_stop_area.goal_search_interval = + node->declare_parameter(ns + "bus_stop_area.goal_search_interval"); + p.bus_stop_area.lateral_offset_interval = + node->declare_parameter(ns + "bus_stop_area.lateral_offset_interval"); const std::string parking_policy_name = node->declare_parameter(ns + "parking_policy"); @@ -71,7 +82,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.parking_policy = ParkingPolicy::RIGHT_SIDE; } else { RCLCPP_ERROR_STREAM( - node->get_logger().get_child(name()), + node->get_logger(), "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); exit(EXIT_FAILURE); } @@ -115,7 +126,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.object_recognition_collision_check_soft_margins.empty() || p.object_recognition_collision_check_hard_margins.empty()) { RCLCPP_FATAL_STREAM( - node->get_logger().get_child(name()), + node->get_logger(), "object_recognition.collision_check_soft_margins and " << "object_recognition.collision_check_hard_margins must not be empty. " << "Terminating the program..."); @@ -155,7 +166,6 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) // parallel parking common { - const std::string ns = base_ns + "pull_over.parallel_parking."; p.parallel_parking_parameters.center_line_path_interval = p.center_line_path_interval; // for geometric parallel parking } @@ -407,22 +417,28 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) // validation of parameters if (p.shift_sampling_num < 1) { RCLCPP_FATAL_STREAM( - node->get_logger().get_child(name()), - "shift_sampling_num must be positive integer. Given parameter: " - << p.shift_sampling_num << std::endl - << "Terminating the program..."); + node->get_logger(), "shift_sampling_num must be positive integer. Given parameter: " + << p.shift_sampling_num << std::endl + << "Terminating the program..."); exit(EXIT_FAILURE); } if (p.maximum_deceleration < 0.0) { RCLCPP_FATAL_STREAM( - node->get_logger().get_child(name()), - "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); + node->get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); exit(EXIT_FAILURE); } + return p; +} - parameters_ = std::make_shared(p); +void GoalPlannerModuleManager::init(rclcpp::Node * node) +{ + // init manager interface + initInterface(node, {""}); + + const std::string base_ns = "goal_planner."; + parameters_ = std::make_shared(initGoalPlannerParameters(node, base_ns)); } void GoalPlannerModuleManager::updateModuleParams( @@ -535,7 +551,6 @@ void GoalPlannerModuleManager::updateModuleParams( // parallel parking common { - const std::string ns = base_ns + "pull_over.parallel_parking."; p->parallel_parking_parameters.center_line_path_interval = p->center_line_path_interval; // for geometric parallel parking } @@ -848,23 +863,8 @@ void GoalPlannerModuleManager::updateModuleParams( }); } -bool GoalPlannerModuleManager::isAlwaysExecutableModule() const -{ - // enable AlwaysExecutable whenever goal modification is not allowed - // because only minor path refinements are made for fixed goals - if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { - return true; - } - - return false; -} - bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { - if (isAlwaysExecutableModule()) { - return true; - } - // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { @@ -876,10 +876,6 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { - if (isAlwaysExecutableModule()) { - return true; - } - // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp new file mode 100644 index 0000000000000..588247e4f1c6e --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -0,0 +1,156 @@ +// Copyright 2023 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/freespace_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/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ + +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; + +FreespacePullOver::FreespacePullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +: PullOverPlannerBase{node, parameters}, + velocity_{parameters.freespace_parking_velocity}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}, + use_back_{ + parameters.freespace_parking_algorithm == "astar" + ? parameters.astar_parameters.use_back + : true // no option for disabling back in rrtstar + } +{ + autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( + vehicle_info, parameters.vehicle_shape_margin); + if (parameters.freespace_parking_algorithm == "astar") { + planner_ = std::make_unique( + parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.freespace_parking_algorithm == "rrtstar") { + planner_ = std::make_unique( + parameters.freespace_parking_common_parameters, vehicle_shape, + parameters.rrt_star_parameters); + } +} + +std::optional FreespacePullOver::plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) +{ + const Pose & current_pose = planner_data->self_odometry->pose.pose; + + // offset goal pose to make straight path near goal for improving parking precision + // todo: support straight path when using back + constexpr double straight_distance = 1.0; + const auto & goal_pose = modified_goal_pose.goal_pose; + const Pose end_pose = + use_back_ ? goal_pose + : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); + + try { + if (!planner_->makePlan(current_pose, end_pose)) { + return {}; + } + } catch (const std::exception & e) { + return {}; + } + + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); + + PathWithLaneId path = + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); + const auto reverse_indices = utils::getReversingIndices(path); + std::vector partial_paths = utils::dividePath(path, reverse_indices); + + // remove points which are near the goal + PathWithLaneId & last_path = partial_paths.back(); + const double th_goal_distance = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_goal_distance) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // add PathPointWithLaneId to last path + auto addPose = [&last_path](const Pose & pose) { + PathPointWithLaneId p = last_path.points.back(); + p.point.pose = pose; + last_path.points.push_back(p); + }; + + if (use_back_) { + addPose(end_pose); + } else { + // add interpolated poses + auto addInterpolatedPoses = [&addPose](const Pose & pose1, const Pose & pose2) { + constexpr double interval = 0.5; + std::vector interpolated_poses = utils::interpolatePose(pose1, pose2, interval); + for (const auto & pose : interpolated_poses) { + addPose(pose); + } + }; + addInterpolatedPoses(last_path.points.back().point.pose, end_pose); + addPose(end_pose); + addInterpolatedPoses(end_pose, goal_pose); + addPose(goal_pose); + } + + std::vector> pairs_terminal_velocity_and_accel{}; + pairs_terminal_velocity_and_accel.resize(partial_paths.size()); + utils::parking_departure::modifyVelocityByDirection( + partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); + + // Check if driving forward for each path, return empty if not + for (auto & partial_path : partial_paths) { + if (!autoware::motion_utils::isDrivingForward(partial_path.points)) { + return {}; + } + } + + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), id, partial_paths, current_pose, modified_goal_pose, + pairs_terminal_velocity_and_accel); + if (!pull_over_path_opt) { + return {}; + } + return pull_over_path_opt.value(); +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp new file mode 100644 index 0000000000000..4a1b776e3a3d0 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -0,0 +1,83 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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/geometric_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 +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +GeometricPullOver::GeometricPullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const LaneDepartureChecker & lane_departure_checker, const bool is_forward) +: PullOverPlannerBase{node, parameters}, + parallel_parking_parameters_{parameters.parallel_parking_parameters}, + lane_departure_checker_{lane_departure_checker}, + is_forward_{is_forward}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} +{ + planner_.setParameters(parallel_parking_parameters_); +} + +std::optional GeometricPullOver::plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) +{ + const auto & route_handler = planner_data->route_handler; + + const auto & goal_pose = modified_goal_pose.goal_pose; + // prepare road nad shoulder lanes + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + + const auto & p = parallel_parking_parameters_; + const double max_steer_angle = + is_forward_ ? p.forward_parking_max_steer_angle : p.backward_parking_max_steer_angle; + planner_.setTurningRadius(planner_data->parameters, max_steer_angle); + planner_.setPlannerData(planner_data); + + const bool found_valid_path = + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_); + if (!found_valid_path) { + return {}; + } + + const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *planner_data->route_handler, left_side_parking_); + const auto arc_path = planner_.getArcPath(); + + // check lane departure with road and shoulder lanes + if (lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, arc_path)) return {}; + + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose, + planner_.getPairsTerminalVelocityAndAccel()); + return pull_over_path_opt; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp new file mode 100644 index 0000000000000..31d8afffbdabe --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -0,0 +1,156 @@ +// 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 + +#include +#include + +namespace autoware::behavior_path_planner +{ + +std::optional PullOverPath::create( + const PullOverPlannerType & type, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, + const std::vector> & pairs_terminal_velocity_and_accel) +{ + if (partial_paths.empty()) { + return std::nullopt; + } + PathWithLaneId path{}; + for (size_t i = 0; i < partial_paths.size(); ++i) { + if (i == 0) { + path.points.insert( + path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(partial_paths.at(i).points.begin()), + partial_paths.at(i).points.end()); + } + } + PathWithLaneId full_path{}; + full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + if (full_path.points.size() < 3) { + return std::nullopt; + } + + const size_t start_idx = + autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); + + PathWithLaneId parking_path{}; + std::copy( + full_path.points.begin() + start_idx, full_path.points.end(), + std::back_inserter(parking_path.points)); + + if (parking_path.points.size() < 3) { + return std::nullopt; + } + + const auto calculateCurvaturesAndMax = + [](const auto & path) -> std::pair, double> { + std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); + double max_curvature = 0.0; + if (!curvatures.empty()) { + max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + } + return std::make_pair(curvatures, max_curvature); + }; + + std::vector full_path_curvatures{}; + std::vector parking_path_curvatures{}; + double full_path_max_curvature{}; + double parking_path_max_curvature{}; + std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); + std::tie(parking_path_curvatures, parking_path_max_curvature) = + calculateCurvaturesAndMax(parking_path); + + return PullOverPath( + type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path, + full_path_curvatures, parking_path_curvatures, full_path_max_curvature, + parking_path_max_curvature, pairs_terminal_velocity_and_accel); +} + +PullOverPath::PullOverPath(const PullOverPath & other) +: type_(other.type_), + modified_goal_pose_(other.modified_goal_pose_), + id_(other.id_), + start_pose_(other.start_pose_), + partial_paths_(other.partial_paths_), + full_path_(other.full_path_), + parking_path_(other.parking_path_), + full_path_curvatures_(other.full_path_curvatures_), + parking_path_curvatures_(other.parking_path_curvatures_), + full_path_max_curvature_(other.full_path_max_curvature_), + parking_path_max_curvature_(other.parking_path_max_curvature_), + path_idx_(other.path_idx_), + pairs_terminal_velocity_and_accel_(other.pairs_terminal_velocity_and_accel_) +{ +} + +PullOverPath::PullOverPath( + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel) +: type_(type), + modified_goal_pose_(modified_goal_pose), + id_(id), + start_pose_(start_pose), + partial_paths_(partial_paths), + full_path_(full_path), + parking_path_(parking_path), + full_path_curvatures_(full_path_curvatures), + parking_path_curvatures_(parking_path_curvatures), + full_path_max_curvature_(full_path_max_curvature), + parking_path_max_curvature_(parking_path_max_curvature), + path_idx_(0), + pairs_terminal_velocity_and_accel_(pairs_terminal_velocity_and_accel) +{ +} + +bool PullOverPath::incrementPathIndex() +{ + { + if (partial_paths_.size() - 1 <= path_idx_) { + return false; + } + path_idx_ += 1; + return true; + } +} + +PathWithLaneId & PullOverPath::getCurrentPath() +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +const PathWithLaneId & PullOverPath::getCurrentPath() const +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp new file mode 100644 index 0000000000000..52461dd2d3bc4 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -0,0 +1,347 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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/shift_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_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +ShiftPullOver::ShiftPullOver( + 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 ShiftPullOver::plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + 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; + 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 = generatePullOverPath( + modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + lateral_jerk); + if (!pull_over_path) continue; + return pull_over_path; + } + + return {}; +} + +PathWithLaneId ShiftPullOver::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; +} + +std::optional ShiftPullOver::cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const +{ + // clip previous module path to shift end pose nearest segment index + const size_t shift_end_idx = autoware::motion_utils::findNearestSegmentIndex( + prev_module_path.points, shift_end_pose.position); + std::vector clipped_points{ + prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected shift end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = autoware::motion_utils::calcSignedArcLength( + prev_module_path.points, shift_end_idx, shift_end_pose.position); + projected_point.point.pose = + autoware::universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_prev_module_path = prev_module_path; + clipped_prev_module_path.points = clipped_points; + + return clipped_prev_module_path; +} + +std::optional ShiftPullOver::generatePullOverPath( + 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 double after_shift_straight_distance = parameters_.after_shift_straight_distance; + + 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 = + autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); + + // 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 = PathShifter::calcLongitudinalDistFromJerk( + 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); + + // 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)) { + return {}; + } + shifted_path.path.points = autoware::motion_utils::removeOverlapPoints(shifted_path.path.points); + autoware::motion_utils::insertOrientation(shifted_path.path.points, true); + + // for debug. result of shift is not equal to the target + const Pose actual_shift_end_pose = shifted_path.path.points.back().point.pose; + + // interpolate between shift end pose to goal pose + std::vector interpolated_poses = + utils::interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); + for (const auto & pose : interpolated_poses) { + PathPointWithLaneId p = shifted_path.path.points.back(); + p.point.pose = pose; + shifted_path.path.points.push_back(p); + } + + // 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) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); + pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); + pull_over_path.debug_poses.push_back(actual_shift_end_pose); + pull_over_path.debug_poses.push_back(goal_pose); + pull_over_path.debug_poses.push_back(shift_end_pose); + pull_over_path.debug_poses.push_back( + road_lane_reference_path_to_shift_end.points.back().point.pose); + pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); + + // 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 auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *planner_data->route_handler, left_side_parking_); + const bool is_in_lanes = !lane_departure_checker_.checkPathWillLeaveLane( + {departure_check_lane}, pull_over_path.parking_path()); + + if (!is_in_parking_lots && !is_in_lanes) { + return {}; + } + + return pull_over_path; +} + +double ShiftPullOver::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 before_arc_length{0.0}; + 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; + before_arc_length += k > 0 ? offset / (1 + k * dr) : offset * (1 - k * dr); + break; + } + before_arc_length += segment_length; + after_arc_length += after_segment_length; + } + + return before_arc_length; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp new file mode 100644 index 0000000000000..f12d510e23030 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -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. + +#include + +#include + +namespace autoware::behavior_path_planner +{ + +void LaneParkingRequest::update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const BehaviorModuleOutput & previous_module_output, + const std::optional & pull_over_path, const PathDecisionState & prev_data) +{ + planner_data_ = std::make_shared(planner_data); + planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); + current_status_ = current_status; + last_previous_module_output_ = previous_module_output_; + previous_module_output_ = previous_module_output; + pull_over_path_ = pull_over_path; + prev_data_ = prev_data; +} + +void FreespaceParkingRequest::initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters) +{ + OccupancyGridMapParam occupancy_grid_map_param{}; + const double margin = parameters.occupancy_grid_collision_check_margin; + occupancy_grid_map_param.vehicle_shape.length = + planner_data.parameters.vehicle_length + 2 * margin; + occupancy_grid_map_param.vehicle_shape.width = planner_data.parameters.vehicle_width + 2 * margin; + occupancy_grid_map_param.vehicle_shape.base2back = + planner_data.parameters.base_link2rear + margin; + occupancy_grid_map_param.theta_size = parameters.theta_size; + occupancy_grid_map_param.obstacle_threshold = parameters.obstacle_threshold; + occupancy_grid_map_ = std::make_shared(); + occupancy_grid_map_->setParam(occupancy_grid_map_param); +} + +void FreespaceParkingRequest::update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped) +{ + planner_data_ = std::make_shared(planner_data); + planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); + current_status_ = current_status; + occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); + pull_over_path_ = pull_over_path; + last_path_update_time_ = last_path_update_time; + is_stopped_ = is_stopped; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 0907118b4bd03..bc20c335fab36 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -14,13 +14,15 @@ #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include #include #include #include +#include #include #include @@ -31,8 +33,11 @@ #include #include +#include #include #include +#include +#include #include namespace autoware::behavior_path_planner::goal_planner_utils @@ -77,17 +82,6 @@ lanelet::ConstLanelets getPullOverLanes( outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } -lanelet::ConstLanelets generateExpandedPullOverLanes( - const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, const double bound_offset) -{ - const auto pull_over_lanes = - getPullOverLanes(route_handler, left_side, backward_distance, forward_distance); - - return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0) - : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, -bound_offset); -} - static double getOffsetToLanesBoundary( const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose target_pose, const bool left_side) @@ -190,7 +184,7 @@ std::optional generateObjectExtractionPolygon( constexpr double INTERSECTION_CHECK_DISTANCE = 10.0; std::vector modified_bound{}; size_t i = 0; - while (i < bound.size() - 2) { + while (i < bound.size() - 1) { BoostPoint p1(bound.at(i).x, bound.at(i).y); BoostPoint p2(bound.at(i + 1).x, bound.at(i + 1).y); LineString p_line{}; @@ -258,32 +252,6 @@ std::optional generateObjectExtractionPolygon( return polygon; } -PredictedObjects extractObjectsInExpandedPullOverLanes( - const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, double bound_offset, const PredictedObjects & objects) -{ - const auto lanes = generateExpandedPullOverLanes( - route_handler, left_side, backward_distance, forward_distance, bound_offset); - - const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( - objects, lanes, [](const auto & obj, const auto & lanelet, const auto yaw_threshold) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet, yaw_threshold); - }); - - return objects_in_lanes; -} - -PredictedObjects extractStaticObjectsInExpandedPullOverLanes( - const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, double bound_offset, const PredictedObjects & objects, - const double velocity_thresh) -{ - const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes( - route_handler, left_side, backward_distance, forward_distance, bound_offset, objects); - - return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh); -} - PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside) @@ -300,6 +268,114 @@ PredictedObjects filterObjectsByLateralDistance( return filtered_objects; } +bool isIntersectingAreas( + const LinearRing2d & footprint, const std::vector & areas) +{ + for (const auto & area : areas) { + if (boost::geometry::intersects(area, footprint)) { + return true; + } + } + return false; +} + +bool isWithinAreas( + const LinearRing2d & footprint, const std::vector & areas) +{ + for (const auto & area : areas) { + if (boost::geometry::within(footprint, area)) { + return true; + } + } + return false; +} + +std::vector getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes) +{ + std::vector area_polygons{}; + for (const auto & bus_stop_area_reg_elem : lanelet::utils::query::busStopAreas(lanes)) { + for (const auto & area : bus_stop_area_reg_elem->busStopAreas()) { + const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); + area_polygons.push_back(area_poly); + } + } + return area_polygons; +} + +bool checkObjectsCollision( + const PathWithLaneId & path, const std::vector & curvatures, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const BehaviorPathPlannerParameters & behavior_path_parameters, + const double collision_check_margin, const bool extract_static_objects, + const double maximum_deceleration, + const double object_recognition_collision_check_max_extra_stopping_margin, + std::vector & debug_ego_polygons_expanded, const bool update_debug_data) +{ + if (path.points.size() != curvatures.size()) { + RCLCPP_WARN( + rclcpp::get_logger("goal_planner_util"), + "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision"); + return false; + } + + const auto & target_objects = + extract_static_objects ? static_target_objects : dynamic_target_objects; + if (target_objects.objects.empty()) { + return false; + } + + // check collision roughly with {min_distance, max_distance} between ego footprint and objects + // footprint + std::pair has_collision_rough = + utils::path_safety_checker::checkObjectsCollisionRough( + path, target_objects, collision_check_margin, behavior_path_parameters, false); + if (!has_collision_rough.first) { + return false; + } + if (has_collision_rough.second) { + return true; + } + + std::vector obj_polygons; + for (const auto & object : target_objects.objects) { + obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); + } + + /* Expand ego collision check polygon + * - `collision_check_margin` is added in all directions. + * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. + * - `extra_lateral_margin` adds the lateral margin on curves. + */ + std::vector ego_polygons_expanded{}; + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, + object_recognition_collision_check_max_extra_stopping_margin); + + // The square is meant to imply centrifugal force, but it is not a very well-founded formula. + // TODO(kosuke55): It is needed to consider better way because there is an inherently + // different conception of the inside and outside margins. + const double extra_lateral_margin = std::min( + extra_stopping_margin, + std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); + + const auto ego_polygon = autoware::universe_utils::toFootprint( + p.point.pose, + behavior_path_parameters.base_link2front + collision_check_margin + extra_stopping_margin, + behavior_path_parameters.base_link2rear + collision_check_margin, + behavior_path_parameters.vehicle_width + collision_check_margin * 2.0 + + extra_lateral_margin * 2.0); + ego_polygons_expanded.push_back(ego_polygon); + } + + if (update_debug_data) { + debug_ego_polygons_expanded = ego_polygons_expanded; + } + + return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); +} + MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) @@ -576,6 +652,43 @@ std::vector createPathFootPrints( return footprints; } +std::string makePathPriorityDebugMessage( + const std::vector & sorted_path_indices, + const std::vector & pull_over_path_candidates, + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_rough_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature) +{ + std::stringstream ss; + + // Unsafe goal and its priority are not visible as debug marker in rviz, + // so exclude unsafe goal from goal_priority + std::map goal_id_and_priority; + for (size_t i = 0; i < goal_candidates.size(); ++i) { + goal_id_and_priority[goal_candidates[i].id] = goal_candidates[i].is_safe ? i : -1; + } + + ss << "\n---------------------- path priority ----------------------\n"; + for (size_t i = 0; i < sorted_path_indices.size(); ++i) { + const auto & path = pull_over_path_candidates[sorted_path_indices[i]]; + // goal_index is same to goal priority including unsafe goal + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id())); + const bool is_safe_goal = goal_candidates[goal_index].is_safe; + const int goal_priority = goal_id_and_priority[path.goal_id()]; + + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type()) + << ", path_id: " << path.id() << ", goal_id: " << path.goal_id() + << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") + << ", margin: " << path_id_to_rough_margin_map.at(path.id()) + << (isSoftMargin(path) ? " (soft)" : " (hard)") + << ", curvature: " << path.parking_path_max_curvature() + << (isHighCurvature(path) ? " (high)" : " (low)") << "\n"; + } + ss << "-----------------------------------------------------------\n"; + return ss.str(); +} + lanelet::Points3d combineLanePoints( const lanelet::Points3d & points, const lanelet::Points3d & points_next) { @@ -610,8 +723,13 @@ lanelet::Lanelet createDepartureCheckLanelet( }; const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet { - return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true) - : route_handler.getMostLeftLanelet(lane, false, true); + const auto getInnerLane = + left_side_parking ? &RouteHandler::getMostRightLanelet : &RouteHandler::getMostLeftLanelet; + const auto getOppositeLane = left_side_parking ? &RouteHandler::getRightOppositeLanelets + : &RouteHandler::getLeftOppositeLanelets; + const auto inner_lane = (route_handler.*getInnerLane)(lane, true, true); + const auto opposite_lanes = (route_handler.*getOppositeLane)(inner_lane); + return opposite_lanes.empty() ? inner_lane : opposite_lanes.front(); }; lanelet::Points3d outer_bound_points{}; @@ -631,4 +749,86 @@ lanelet::Lanelet createDepartureCheckLanelet( left_side_parking ? inner_linestring : outer_linestring); } +std::optional calcRefinedGoal( + const Pose & goal_pose, const std::shared_ptr route_handler, + const bool left_side_parking, const double vehicle_width, const double base_link2front, + const double base_link2rear, const GoalPlannerParameters & parameters) +{ + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + + lanelet::Lanelet closest_pull_over_lanelet{}; + lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); + + // calc closest center line pose + Pose center_pose{}; + { + // find position + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); + const auto segment = lanelet::utils::getClosestSegment( + lanelet::utils::to2D(lanelet_point), closest_pull_over_lanelet.centerline()); + const auto p1 = segment.front().basicPoint(); + const auto p2 = segment.back().basicPoint(); + const auto direction_vector = (p2 - p1).normalized(); + const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const double s = direction_vector.dot(p1_to_goal); + const auto refined_point = p1 + direction_vector * s; + + center_pose.position.x = refined_point.x(); + center_pose.position.y = refined_point.y(); + center_pose.position.z = refined_point.z(); + + // find orientation + const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + center_pose.orientation = tf2::toMsg(tf_quat); + } + + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking); + if (!distance_from_bound) { + return std::nullopt; + } + + const double sign = left_side_parking ? -1.0 : 1.0; + const double offset_from_center_line = + sign * (distance_from_bound.value() + parameters.margin_from_boundary); + + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + + return refined_goal_pose; +} + +std::optional calcClosestPose( + const lanelet::ConstLineString3d line, const Point & query_point) +{ + const auto segment = + lanelet::utils::getClosestSegment(lanelet::BasicPoint2d{query_point.x, query_point.y}, line); + if (segment.empty()) { + return std::nullopt; + } + + const Eigen::Vector2d direction( + (segment.back().basicPoint2d() - segment.front().basicPoint2d()).normalized()); + const Eigen::Vector2d xf(segment.front().basicPoint2d()); + const Eigen::Vector2d x(query_point.x, query_point.y); + const Eigen::Vector2d p = xf + (x - xf).dot(direction) * direction; + + geometry_msgs::msg::Pose closest_pose; + closest_pose.position.x = p.x(); + closest_pose.position.y = p.y(); + closest_pose.position.z = query_point.z; + + const double lane_yaw = + std::atan2(segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); + tf2::Quaternion q; + q.setRPY(0, 0, lane_yaw); + closest_pose.orientation = tf2::toMsg(q); + + return closest_pose; +} + } // namespace autoware::behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 05f7c70fa42c2..1902901852d45 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -95,6 +96,9 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( const ExtendedPredictedObjects & objects, const double time_horizon, const bool check_all_predicted_path); +std::vector filterPredictedPathAfterTargetPose( + const std::vector & path, const Pose & target_pose); + bool checkSafetyWithRSS( const PathWithLaneId & planned_path, const std::vector & ego_predicted_path, @@ -163,6 +167,23 @@ std::pair checkObjectsCollisionRough( const PathWithLaneId & path, const PredictedObjects & objects, const double margin, const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point); +/** + * @brief Calculate the rough distance between the ego vehicle and the objects. + * @param path The path of the ego vehicle. + * @param objects The predicted objects. + * @param parameters The common parameters used in behavior path planner. + * @param use_offset_ego_point If true, the closest point to the object is calculated by + * interpolating the path points. + * @param distance_type The type of distance to calculate. "min" or "max". Calculate the distance + * when the distance is minimized or maximized when the direction of the ego and the object is + * changed. + * @return The rough distance between the ego vehicle and the objects. + */ +double calculateRoughDistanceToObjects( + const PathWithLaneId & path, const PredictedObjects & objects, + const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point, + const std::string & distance_type); + // debug CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 8d2dd2fb86e29..f84b403b1565a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -486,6 +486,22 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( return filtered_objects; } +std::vector filterPredictedPathAfterTargetPose( + const std::vector & path, const Pose & target_pose) +{ + std::vector filtered_path; + + const auto target_idx = + std::min_element(path.begin(), path.end(), [&target_pose](const auto & a, const auto & b) { + return calcDistance2d(a.pose.position, target_pose.position) < + calcDistance2d(b.pose.position, target_pose.position); + }); + + std::copy(target_idx, path.end(), std::back_inserter(filtered_path)); + + return filtered_path; +}; + bool checkSafetyWithRSS( const PathWithLaneId & planned_path, const std::vector & ego_predicted_path, @@ -820,4 +836,54 @@ std::pair checkObjectsCollisionRough( return has_collision; } +double calculateRoughDistanceToObjects( + const PathWithLaneId & path, const PredictedObjects & objects, + const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point, + const std::string & distance_type) +{ + const auto & points = path.points; + + const double ego_length = std::invoke([&]() -> double { + const auto & p = parameters; + if (distance_type == "min") { + return std::max( + std::hypot(p.vehicle_width / 2, p.front_overhang), + std::hypot(p.vehicle_width / 2, p.rear_overhang)); + } else if (distance_type == "max") { + return std::min({p.vehicle_width / 2, p.front_overhang / 2, p.rear_overhang / 2}); + } + throw std::invalid_argument("Invalid distance type"); + }); + + double min_distance = std::numeric_limits::max(); + for (const auto & object : objects.objects) { + const double object_length = std::invoke([&]() -> double { + if (distance_type == "min") { + return calcObstacleMaxLength(object.shape); + } else if (distance_type == "max") { + return calcObstacleMinLength(object.shape); + } + throw std::invalid_argument("Invalid distance type"); + }); + const Point & object_point = object.kinematics.initial_pose_with_covariance.pose.position; + const double distance = std::invoke([&]() -> double { + if (use_offset_ego_point) { + const size_t nearest_segment_idx = findNearestSegmentIndex(points, object_point); + const double offset_length = + calcLongitudinalOffsetToSegment(points, nearest_segment_idx, object_point); + const auto offset_point = + calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length); + const Point ego_point = + offset_point ? offset_point.value() + : points.at(findNearestIndex(points, object_point)).point.pose.position; + return std::max(calcDistance2d(ego_point, object_point) - object_length - ego_length, 0.0); + } + const Point ego_point = points.at(findNearestIndex(points, object_point)).point.pose.position; + return std::max(calcDistance2d(ego_point, object_point) - object_length - ego_length, 0.0); + }); + min_distance = std::min(min_distance, distance); + } + return min_distance; +} + } // namespace autoware::behavior_path_planner::utils::path_safety_checker