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 d3f7f7a4015f0..d680fd2ecd695 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 @@ -19,6 +19,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp src/goal_planner_module.cpp src/manager.cpp + src/thread_data.cpp + src/decision_state.cpp ) if(BUILD_EXAMPLES) 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 d5dd968f6e46e..6a5d07b1e3172 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 @@ -18,7 +18,6 @@ #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/thread_data.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -106,32 +105,136 @@ struct PoseWithString } }; +struct SelectedPullOverPath +{ + PullOverPath path; + rclcpp::Time selected_time; + std::optional last_path_idx_increment_time; +}; + struct PullOverContextData { PullOverContextData() = delete; explicit PullOverContextData( const bool is_stable_safe_path, const PredictedObjects & static_objects, - const PredictedObjects & dynamic_objects, std::optional && pull_over_path_opt, - const std::vector & pull_over_path_candidates, - const PathDecisionState & prev_state) + const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state, + const bool is_stopped, LaneParkingResponse && lane_parking_response, + FreespaceParkingResponse && freespace_parking_response) : is_stable_safe_path(is_stable_safe_path), static_target_objects(static_objects), dynamic_target_objects(dynamic_objects), - pull_over_path_opt(pull_over_path_opt), - pull_over_path_candidates(pull_over_path_candidates), prev_state_for_debug(prev_state), - last_path_idx_increment_time(std::nullopt) + is_stopped(is_stopped), + lane_parking_response(lane_parking_response), + freespace_parking_response(freespace_parking_response) { } const bool is_stable_safe_path; const PredictedObjects static_target_objects; const PredictedObjects dynamic_target_objects; - // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it) - std::optional pull_over_path_opt; - const std::vector pull_over_path_candidates; - // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose const PathDecisionState prev_state_for_debug; - std::optional last_path_idx_increment_time; + const bool is_stopped; + const LaneParkingResponse lane_parking_response; + const FreespaceParkingResponse freespace_parking_response; + // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it) + std::optional selected_pull_over_path_opt; +}; + +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 PullOverPath & pull_over_path, const rclcpp::Time & 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_odomemtry, const double duration_lower); + +// 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); + + 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 @@ -144,7 +247,7 @@ class GoalPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); - ~GoalPlannerModule() + ~GoalPlannerModule() override { if (lane_parking_timer_) { lane_parking_timer_->cancel(); @@ -192,18 +295,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( @@ -213,20 +304,15 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - 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); } + 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 @@ -259,15 +345,12 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr parameters_; - mutable std::shared_ptr ego_predicted_path_params_; - mutable std::shared_ptr objects_filtering_params_; - mutable std::shared_ptr safety_check_params_; + std::shared_ptr ego_predicted_path_params_; + std::shared_ptr objects_filtering_params_; + std::shared_ptr safety_check_params_; 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 @@ -285,11 +368,6 @@ class GoalPlannerModule : public SceneModuleInterface autoware::universe_utils::LinearRing2d vehicle_footprint_; - std::recursive_mutex mutex_; - mutable ThreadSafeData thread_safe_data_; - - // TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData - // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() PathDecisionStateController path_decision_controller_{getLogger()}; @@ -318,11 +396,6 @@ 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; - // goal seach GoalCandidates generateGoalCandidates() const; @@ -339,24 +412,9 @@ class GoalPlannerModule : public SceneModuleInterface double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status - bool isStopped(); - bool isStopped( - std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); - bool isOnModifiedGoal( - const Pose & current_pose, const std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const; double calcModuleRequestLength() const; - bool needPathUpdate( - const Pose & current_pose, const double path_update_duration, - const std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const; - bool isStuck( - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters); - void decideVelocity(); + void decideVelocity(PullOverPath & pull_over_path); void updateStatus(const BehaviorModuleOutput & output); // validation @@ -367,20 +425,13 @@ 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 GoalCandidates & goal_candidates, - const std::shared_ptr occupancy_grid_map, - const PredictedObjects & static_target_objects); + bool canReturnToLaneParking(const PullOverContextData & context_data); // plan pull over path - BehaviorModuleOutput planPullOver(const PullOverContextData & context_data); - BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data); - BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data); + 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, @@ -392,7 +443,9 @@ class GoalPlannerModule : public SceneModuleInterface const PullOverContextData & context_data, BehaviorModuleOutput & output) const; // output setter - void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output); + void setOutput( + const std::optional selected_pull_over_path_with_velocity_opt, + const PullOverContextData & context_data, BehaviorModuleOutput & output); void setModifiedGoal( const PullOverContextData & context_data, BehaviorModuleOutput & output) const; @@ -402,20 +455,6 @@ class GoalPlannerModule : public SceneModuleInterface TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data); std::optional ignore_signal_{std::nullopt}; - bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output) const; - bool hasDeviatedFromLastPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & last_previous_module_output) 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 PullOverContextData & context_data, const std::array & pose, 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 index dcd2933a0fc41..ed2766bcc7cac 100644 --- 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 @@ -17,204 +17,127 @@ #include "autoware/behavior_path_goal_planner_module/decision_state.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include -#include #include +#include #include namespace autoware::behavior_path_planner { -/** - * @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this) - */ -struct GoalPlannerData -{ - GoalPlannerData( - const PlannerData & planner_data, const GoalPlannerParameters & parameters, - const BehaviorModuleOutput & previous_module_output_) - { - initializeOccupancyGridMap(planner_data, parameters); - previous_module_output = previous_module_output_; - last_previous_module_output = previous_module_output_; - }; - GoalPlannerParameters parameters; - autoware::universe_utils::LinearRing2d vehicle_footprint; - - PlannerData planner_data; - ModuleStatus current_status; - BehaviorModuleOutput previous_module_output; - BehaviorModuleOutput last_previous_module_output; // occupancy_grid_map; - - 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 PlannerData & planner_data, - const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, - const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates); - -private: - void initializeOccupancyGridMap( - const PlannerData & planner_data, const GoalPlannerParameters & parameters); -}; - -class ThreadSafeData +class LaneParkingRequest { 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; - } - - return pull_over_path_->incrementPathIndex(); - } - - void set_pull_over_path(const PullOverPath & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } - - void set_pull_over_path(const std::shared_ptr & path) + 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) { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); } - std::shared_ptr get_pull_over_path() - { - const std::lock_guard lock(mutex_); - return pull_over_path_; - } + void update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const BehaviorModuleOutput & previous_module_output, + const std::optional> & selected_pull_over_path, + const PathDecisionState & prev_data); - void clearPullOverPath() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - } + const GoalPlannerParameters parameters_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; - bool foundPullOverPath() const + 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 { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - return true; + return previous_module_output_; } - - void reset() + const BehaviorModuleOutput & get_last_previous_module_output() const { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - pull_over_path_candidates_.clear(); - last_path_update_time_ = std::nullopt; - closest_start_pose_ = std::nullopt; - prev_data_ = PathDecisionState{}; + return last_previous_module_output_; } - - void set_prev_data(const PathDecisionState & prev_data) + const std::optional> & get_selected_pull_over_path() const { - const std::lock_guard lock(mutex_); - prev_data_ = prev_data; + return selected_pull_over_path_; } + const PathDecisionState & get_prev_data() const { return prev_data_; } + LaneParkingRequest clone() const; - PathDecisionState get_prev_data() - { - const std::lock_guard lock(mutex_); - return prev_data_; - } +private: + std::shared_ptr planner_data_; + ModuleStatus current_status_; + BehaviorModuleOutput previous_module_output_; + BehaviorModuleOutput last_previous_module_output_; + std::optional> + selected_pull_over_path_; // get_closest_start_pose() - { - const std::lock_guard lock(mutex_); - return closest_start_pose_; - } +struct LaneParkingResponse +{ + std::vector pull_over_path_candidates; + std::optional closest_start_pose; +}; - void set_closest_start_pose(const std::optional & closest_start_pose) +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) { - const std::lock_guard lock(mutex_); - closest_start_pose_ = closest_start_pose; - } + initializeOccupancyGridMap(planner_data, parameters_); + }; - std::vector get_pull_over_path_candidates() - { - const std::lock_guard lock(mutex_); - return pull_over_path_candidates_; - } + const ModuleStatus & getCurrentStatus() const { return current_status_; } + void update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const std::optional> & selected_pull_over_path, + const bool is_stopped); - void set_pull_over_path_candidates(const std::vector & pull_over_path_candidates) - { - const std::lock_guard lock(mutex_); - pull_over_path_candidates_ = pull_over_path_candidates; - } + const GoalPlannerParameters parameters_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; - std::optional get_last_path_update_time() + 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 { - const std::lock_guard lock(mutex_); - return last_path_update_time_; + return occupancy_grid_map_; } - - void set_last_path_update_time(const std::optional & last_path_update_time) + const std::optional> & get_selected_pull_over_path() const { - const std::lock_guard lock(mutex_); - last_path_update_time_ = last_path_update_time; + return selected_pull_over_path_; } + bool is_stopped() const { return is_stopped_; } - // main --> lane - // DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) - // DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - // DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - - // lane --> main - // DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - // DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + FreespaceParkingRequest clone() const; private: - void set_pull_over_path_no_lock(const PullOverPath & path) - { - pull_over_path_ = std::make_shared(path); - - last_path_update_time_ = clock_->now(); - } + std::shared_ptr planner_data_; + ModuleStatus current_status_; + std::shared_ptr occupancy_grid_map_; + std::optional> selected_pull_over_path_; + bool is_stopped_; - void set_pull_over_path_no_lock(const std::shared_ptr & path) - { - pull_over_path_ = path; - last_path_update_time_ = clock_->now(); - } - - void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } - void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } - - std::shared_ptr pull_over_path_{nullptr}; - std::vector pull_over_path_candidates_; - std::optional last_path_update_time_; - std::optional closest_start_pose_{}; - PathDecisionState prev_data_{}; + void initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters); +}; - std::recursive_mutex & mutex_; - rclcpp::Clock::SharedPtr clock_; +struct FreespaceParkingResponse +{ + std::optional freespace_pull_over_path; }; } // namespace autoware::behavior_path_planner 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..827e1269cfa80 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -0,0 +1,187 @@ +// 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 + +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/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 05f8f3ffbd5fc..b2eb8e49f56f4 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 @@ -15,6 +15,7 @@ #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/freespace_pull_over.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" @@ -53,7 +54,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 @@ -67,18 +67,10 @@ GoalPlannerModule::GoalPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // 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; @@ -86,23 +78,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(); @@ -113,19 +88,30 @@ GoalPlannerModule::GoalPlannerModule( const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); lane_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto lane_parking_planner = std::make_unique( + node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_, + is_lane_parking_cb_running_, getLogger(), *parameters_); 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_planner_bind = std::move(lane_parking_planner)]() { + lane_parking_planner_bind->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); + auto freespace_parking_planner = std::make_unique( + freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_, + is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner); freespace_parking_timer_ = rclcpp::create_timer( &node, clock_, freespace_parking_period_ns, - std::bind(&GoalPlannerModule::onFreespaceParkingTimer, this), + [freespace_parking_planner_bind = std::move(freespace_parking_planner)]() { + freespace_parking_planner_bind->onTimer(); + }, freespace_parking_timer_cb_group_); } @@ -145,9 +131,27 @@ GoalPlannerModule::GoalPlannerModule( // time_keeper_->add_reporter(&std::cerr); } -bool GoalPlannerModule::hasPreviousModulePathShapeChanged( +bool isOnModifiedGoal( + const Pose & current_pose, const GoalCandidate & modified_goal, + const GoalPlannerParameters & parameters) +{ + 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) const + 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 @@ -180,69 +184,164 @@ bool GoalPlannerModule::hasPreviousModulePathShapeChanged( return false; } -bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath( - const std::shared_ptr planner_data, - const BehaviorModuleOutput & last_previous_module_output) const +bool hasDeviatedFromLastPreviousModulePath( + const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output) { return std::abs(autoware::motion_utils::calcLateralOffset( last_previous_module_output.path.points, - planner_data->self_odometry->pose.pose.position)) > 0.3; + 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 PullOverPath & pull_over_path, const rclcpp::Time & selected_time, + const GoalPlannerParameters & parameters) +{ + const bool has_enough_time_passed = (now - selected_time).seconds() > path_update_duration; + return !isOnModifiedGoal(current_pose, pull_over_path.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; + } + // TODO(soblin): this calls setMap() in freespace_planner in the for-loop, which is very + // inefficient + 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) +{ + 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 > duration_lower) { + 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::optional last_previous_module_output_opt{std::nullopt}; - std::shared_ptr occupancy_grid_map{nullptr}; - std::optional parameters_opt{std::nullopt}; - std::optional goal_candidates_opt{std::nullopt}; + 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; - last_previous_module_output_opt = gp_planner_data.last_previous_module_output; - occupancy_grid_map = gp_planner_data.occupancy_grid_map; - parameters_opt = gp_planner_data.parameters; - goal_candidates_opt = gp_planner_data.goal_candidates; + std::lock_guard guard(mutex_); + if (request_) { + auto & request = request_.value(); + local_request_opt.emplace(std::move(request)); } } // end of critical section - if ( - !local_planner_data || !current_status_opt || !previous_module_output_opt || - !last_previous_module_output_opt || !occupancy_grid_map || !parameters_opt || - !goal_candidates_opt) { - RCLCPP_ERROR( - getLogger(), - "failed to get valid " - "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt " - "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 & last_previous_module_output = last_previous_module_output_opt.value(); - const auto & parameters = parameters_opt.value(); - const auto & goal_candidates = goal_candidates_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 & 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 & selected_pull_over_path = local_request.get_selected_pull_over_path(); + const auto & prev_data = local_request.get_prev_data(); if (current_status == ModuleStatus::IDLE) { return; @@ -253,41 +352,38 @@ void GoalPlannerModule::onTimer() return; } - if (!utils::isAllowedGoalModification(local_planner_data->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data->route_handler)) { return; } // check if new pull over path candidates are needed to be generated - const auto current_state = thread_safe_data_.get_prev_data().state; + const auto current_state = prev_data.state; const bool need_update = std::invoke([&]() { - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path_opt = - found_pull_over_path - ? std::make_optional(*thread_safe_data_.get_pull_over_path()) - : std::nullopt; + { + 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()) + selected_pull_over_path + ? std::make_optional(selected_pull_over_path.value().first.modified_goal()) : std::nullopt; - if (isOnModifiedGoal( - local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(local_planner_data, previous_module_output)) { - RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); + if (hasDeviatedFromCurrentPreviousModulePath(*planner_data, previous_module_output)) { + RCLCPP_DEBUG(logger_, "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, last_previous_module_output)) { - RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); + RCLCPP_DEBUG(logger_, "has previous module path shape changed"); return true; } if ( - hasDeviatedFromLastPreviousModulePath(local_planner_data, last_previous_module_output) && + hasDeviatedFromLastPreviousModulePath(*planner_data, last_previous_module_output) && current_state != PathDecisionState::DecisionKind::DECIDED) { - RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); + RCLCPP_DEBUG(logger_, "has deviated from last previous module path"); return true; } // TODO(someone): The generated path inherits the velocity of the path of the previous module. @@ -302,8 +398,7 @@ void GoalPlannerModule::onTimer() // 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, - parameters.forward_goal_search_length, + planner_data, parameters.backward_goal_search_length, parameters.forward_goal_search_length, /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; @@ -311,8 +406,8 @@ void GoalPlannerModule::onTimer() const auto planCandidatePaths = [&]( const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { - const auto pull_over_path = planner->plan( - goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); + const auto pull_over_path = + planner->plan(goal_candidate, path_candidates.size(), planner_data, previous_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority @@ -332,8 +427,7 @@ void GoalPlannerModule::onTimer() const bool is_center_line_input_path = goal_planner_utils::isReferencePath( previous_module_output.reference_path, previous_module_output.path, 0.1); RCLCPP_DEBUG( - getLogger(), "the input path of pull over planner is center line: %d", - is_center_line_input_path); + logger_, "the input path of pull over planner is center line: %d", is_center_line_input_path); // plan candidate paths and set them to the member variable if (parameters.path_priority == "efficient_path") { @@ -358,93 +452,78 @@ void GoalPlannerModule::onTimer() } } else { RCLCPP_ERROR( - getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", + logger_, "path_priority should be efficient_path or close_goal, but %s is given.", parameters.path_priority.c_str()); throw std::domain_error("[pull_over] invalid path_priority"); } - // set member variables - 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()); + // set response + { + std::lock_guard guard(mutex_); + response_.pull_over_path_candidates = std::move(path_candidates); + response_.closest_start_pose = closest_start_pose; + RCLCPP_INFO(logger_, "generated %lu pull over path candidates", 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::optional vehicle_footprint_opt{std::nullopt}; - std::optional goal_candidates_opt{std::nullopt}; + 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; - vehicle_footprint_opt = gp_planner_data.vehicle_footprint; - goal_candidates_opt = gp_planner_data.goal_candidates; + std::lock_guard guard(mutex_); + if (request_) { + auto & request = request_.value(); + local_request_opt.emplace(std::move(request)); } } // end of critical section - if ( - !local_planner_data || !current_status_opt || !occupancy_grid_map || !parameters_opt || - !vehicle_footprint_opt || !goal_candidates_opt) { - RCLCPP_ERROR( - getLogger(), - "failed to get valid planner_data/current_status/parameters 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 & vehicle_footprint = vehicle_footprint_opt.value(); - const auto & goal_candidates = goal_candidates_opt.value(); + const auto & local_request = local_request_opt.value(); + const auto & parameters = local_request.parameters_; + const auto & planner_data = local_request.get_planner_data(); + const auto & current_status = local_request.get_current_status(); + const auto & selected_pull_over_path = local_request.get_selected_pull_over_path(); + const auto & occupancy_grid_map = local_request.get_occupancy_grid_map(); if (current_status == ModuleStatus::IDLE) { return; } - if (!local_planner_data->costmap) { + if (!planner_data->costmap) { return; } // fixed goal planner do not use freespace planner - if (!utils::isAllowedGoalModification(local_planner_data->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data->route_handler)) { return; } - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path_opt = - found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) - : std::nullopt; - 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)) { + if (!selected_pull_over_path) { + return; + } + const auto & [pull_over_path, selected_time] = selected_pull_over_path.value(); + const GoalCandidate modified_goal = pull_over_path.modified_goal(); + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal, parameters)) { return; } - const double vehicle_width = planner_data_->parameters.vehicle_width; + const double vehicle_width = 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, + *(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) { + for (const auto & object : planner_data->dynamic_object->objects) { const auto object_polygon = universe_utils::toPolygon2d(object); if ( objects_extraction_polygon.has_value() && @@ -455,109 +534,106 @@ void GoalPlannerModule::onFreespaceParkingTimer() 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; + const bool is_new_costmap = (clock_->now() - planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; if ( - isStuck( - static_target_objects, dynamic_target_objects, 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, modified_goal_opt, - parameters)) { - auto goal_searcher = std::make_shared(parameters, vehicle_footprint); - - planFreespacePath( - local_planner_data, goal_searcher, goal_candidates, occupancy_grid_map, - static_target_objects); + planner_data->self_odometry->pose.pose, path_update_duration, clock_->now(), pull_over_path, + selected_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) +bool FreespaceParkingPlanner::isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const FreespaceParkingRequest & req) const { - objects_filtering_params = - std::make_shared(goal_planner_params->objects_filtering_params); -} + if (!req.get_selected_pull_over_path()) { + return false; + } -void GoalPlannerModule::updateData() -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & parameters = req.parameters_; + const auto & planner_data = req.get_planner_data(); + const auto & pull_over_path = req.get_selected_pull_over_path().value().first; + const std::optional modified_goal = + std::make_optional(pull_over_path.modified_goal()); + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal, parameters)) { + return false; + } - // 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(); + if (req.is_stopped()) { + return false; } - 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); + + if (parameters.use_object_recognition) { + const auto & path = pull_over_path.getCurrentPath(); + const auto curvatures = autoware::motion_utils::calcCurvature(path.points); + 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, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + ego_polygons_expanded)) { + return true; } } - 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(); + if ( + parameters.use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(pull_over_path.getCurrentPath(), req.get_occupancy_grid_map())) { + return true; } + return false; +} + +std::pair GoalPlannerModule::syncWithThreads() +{ // 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 + const bool found_pull_over_path = + context_data_ ? context_data_.value().selected_pull_over_path_opt.has_value() : false; + std::optional> selected_pull_over_path; + if (found_pull_over_path) { + const auto & ctx_data_pull_over_path = + context_data_.value().selected_pull_over_path_opt.value(); + selected_pull_over_path.emplace( + ctx_data_pull_over_path.path, ctx_data_pull_over_path.selected_time); + } + // 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 // 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_, getPreviousModuleOutput()); + 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(); + auto & lane_parking_request = lane_parking_request_.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_, *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), - vehicle_footprint_, goal_candidates_); + lane_parking_request.update( + *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), selected_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 @@ -569,15 +645,68 @@ void GoalPlannerModule::updateData() // `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_ + lane_parking_response = std::move(lane_parking_response_); + } + 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_); + } + auto & freespace_parking_request = freespace_parking_request_.value(); + constexpr double stuck_time = 5.0; + freespace_parking_request.update( + *planner_data_, getCurrentStatus(), selected_pull_over_path, + isStopped(odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time)); // 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; + occupancy_grid_map_ = freespace_parking_request.get_occupancy_grid_map(); + freespace_parking_response = std::move(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; @@ -587,46 +716,49 @@ void GoalPlannerModule::updateData() resetPathReference(); path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - std::optional pull_over_path_recv = - found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + const bool found_pull_over_path = + context_data_ ? context_data_.value().selected_pull_over_path_opt.has_value() : false; + std::optional pull_over_path = + found_pull_over_path ? std::make_optional( + context_data_.value().selected_pull_over_path_opt.value().path) : std::nullopt; const auto modified_goal_pose = [&]() -> std::optional { - if (!pull_over_path_recv) { + if (!pull_over_path) { return std::nullopt; } - const auto & pull_over_path = pull_over_path_recv.value(); - return pull_over_path.modified_goal(); + return pull_over_path.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_, + planner_data_, found_pull_over_path, pull_over_path, 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); + goal_searcher_, isActivated(), pull_over_path, debug_data_.ego_polygons_expanded); context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, - dynamic_target_objects, std::move(pull_over_path_recv), - thread_safe_data_.get_pull_over_path_candidates(), prev_decision_state); + dynamic_target_objects, prev_decision_state, + isStopped(odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time), + std::move(lane_parking_response), std::move(freespace_parking_response)); auto & ctx_data = context_data_.value(); - thread_safe_data_.set_prev_data(path_decision_controller_.get_current_state()); - if (!isActivated()) { return; } if (hasFinishedCurrentPath(ctx_data)) { - if (thread_safe_data_.incrementPathIndex()) { - ctx_data.last_path_idx_increment_time = clock_->now(); + if (ctx_data.selected_pull_over_path_opt) { + auto & pull_over_path = ctx_data.selected_pull_over_path_opt.value().path; + if (pull_over_path.incrementPathIndex()) { + ctx_data.selected_pull_over_path_opt.value().last_path_idx_increment_time = clock_->now(); + } } } @@ -634,15 +766,18 @@ void GoalPlannerModule::updateData() last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); // TODO(soblin): do not "plan" in updateData - decideVelocity(); + if (ctx_data.selected_pull_over_path_opt) + decideVelocity(ctx_data.selected_pull_over_path_opt.value().path); } } 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() @@ -650,7 +785,6 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - thread_safe_data_.reset(); last_approval_data_.reset(); } @@ -785,62 +919,28 @@ double GoalPlannerModule::calcModuleRequestLength() const return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } -bool GoalPlannerModule::planFreespacePath( - std::shared_ptr planner_data, - const std::shared_ptr goal_searcher, const GoalCandidates & goal_candidates_arg, - const std::shared_ptr occupancy_grid_map, - const PredictedObjects & static_target_objects) -{ - auto goal_candidates = goal_candidates_arg; - goal_searcher->update(goal_candidates, occupancy_grid_map, planner_data, static_target_objects); - 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; - } - const auto freespace_path = freespace_planner_->plan( - goal_candidate, 0, planner_data, BehaviorModuleOutput{} // NOTE: not used so passing {} is OK - ); - if (!freespace_path) { - continue; - } - - { - const std::lock_guard lock(mutex_); - thread_safe_data_.set_pull_over_path(*freespace_path); - 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(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)) { return false; } - if (!context_data.pull_over_path_opt) { + if (!context_data.selected_pull_over_path_opt) { return false; } - if (context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { + if ( + context_data.selected_pull_over_path_opt.value().path.type() == + PullOverPlannerType::FREESPACE) { return false; } - const auto & lane_parking_path = context_data.pull_over_path_opt.value(); + // 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.selected_pull_over_path_opt.value().path; const auto & path = lane_parking_path.full_path(); const auto & curvatures = lane_parking_path.full_path_curvatures(); @@ -903,7 +1003,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() if (!context_data_) { RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); } else { - const auto & context_data = context_data_.value(); + auto & context_data = context_data_.value(); return planPullOver(context_data); } } @@ -1177,13 +1277,14 @@ std::vector GoalPlannerModule::generateDrivableLanes() const } 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 (!context_data.pull_over_path_opt) { + if (!selected_pull_over_path_with_velocity_opt) { // situation : not safe against static objects use stop_path output.path = generateStopPath(context_data); RCLCPP_WARN_THROTTLE( @@ -1192,7 +1293,7 @@ void GoalPlannerModule::setOutput( return; } - const auto & pull_over_path = context_data.pull_over_path_opt.value(); + const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; if ( parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { @@ -1231,8 +1332,9 @@ void GoalPlannerModule::setDrivableAreaInfo( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if ( - context_data.pull_over_path_opt && - context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { + context_data.selected_pull_over_path_opt && + context_data.selected_pull_over_path_opt.value().path.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; @@ -1251,10 +1353,10 @@ void GoalPlannerModule::setModifiedGoal( const PullOverContextData & context_data, BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (context_data.pull_over_path_opt) { + if (context_data.selected_pull_over_path_opt) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = context_data.pull_over_path_opt.value().modified_goal_pose(); + modified_goal.pose = context_data.selected_pull_over_path_opt.value().path.modified_goal_pose(); modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -1291,15 +1393,14 @@ void GoalPlannerModule::updateSteeringFactor( steering_factor_interface_.set(pose, distance, steering_factor_direction, type, ""); } -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; // partial_paths - // TODO(soblin): only update velocity on main thread side, use that on main thread side - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths().front(); + 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) { @@ -1307,7 +1408,7 @@ void GoalPlannerModule::decideVelocity() } } -BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & context_data) +BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1320,13 +1421,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & return planPullOverAsOutput(context_data); } -BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( - const PullOverContextData & context_data) +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 (context_data.pull_over_path_candidates.empty()) { + if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } @@ -1344,7 +1444,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - if (!context_data.pull_over_path_opt) { + if (!context_data.selected_pull_over_path_opt) { return output; } @@ -1353,8 +1453,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( return output; } -BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( - const PullOverContextData & context_data) +BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1362,36 +1461,35 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( start = std::chrono::system_clock::now(); // if pull over path candidates generation is not finished, use previous module output - if (context_data.pull_over_path_candidates.empty()) { + if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } - auto context_data_with_velocity = context_data; /** 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_with_velocity.pull_over_path_opt; + auto pull_over_path_with_velocity_opt = + context_data.selected_pull_over_path_opt + ? std::make_optional(context_data.selected_pull_over_path_opt.value().path) + : std::nullopt; const bool is_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; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && - !is_freespace && + !is_freespace && context_data.selected_pull_over_path_opt.has_value() && needPathUpdate( - planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, modified_goal_opt, - *parameters_)) { + planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), + context_data.selected_pull_over_path_opt.value().path, + context_data.selected_pull_over_path_opt.value().selected_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.selected_pull_over_path_opt = std::nullopt; // update goal candidates auto goal_candidates = goal_candidates_; @@ -1399,13 +1497,18 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); // Select a path that is as safe as possible and has a high priority. - const auto & pull_over_path_candidates = context_data.pull_over_path_candidates; - auto path_and_goal_opt = + const auto & pull_over_path_candidates = + context_data.lane_parking_response.pull_over_path_candidates; + auto lane_pull_over_path_opt = selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ - if (path_and_goal_opt) { - const auto & pull_over_path = path_and_goal_opt.value(); + auto pull_over_path_opt = lane_pull_over_path_opt ? lane_pull_over_path_opt + : (context_data.freespace_parking_response.freespace_pull_over_path) + ? context_data.freespace_parking_response.freespace_pull_over_path + : std::nullopt; + 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 @@ -1416,7 +1519,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( * As the next action item, only set this selected pull_over_path to only * FreespaceThreadSafeData. */ - thread_safe_data_.set_pull_over_path(pull_over_path); + context_data.selected_pull_over_path_opt = + SelectedPullOverPath{pull_over_path, clock_->now(), 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() @@ -1432,20 +1536,18 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // set output and status BehaviorModuleOutput output{}; - setOutput(context_data_with_velocity, output); + setOutput(pull_over_path_with_velocity_opt, context_data, output); // return to lane parking if it is possible - if (is_freespace && canReturnToLaneParking(context_data_with_velocity)) { - // 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 - if (context_data_with_velocity.pull_over_path_opt) { - thread_safe_data_.set_pull_over_path(context_data_with_velocity.pull_over_path_opt.value()); + if (is_freespace && canReturnToLaneParking(context_data)) { + if (pull_over_path_with_velocity_opt) { + context_data.selected_pull_over_path_opt = + SelectedPullOverPath{pull_over_path_with_velocity_opt.value(), clock_->now(), std::nullopt}; } } // For debug - setDebugData(context_data_with_velocity); + setDebugData(context_data); if (!pull_over_path_with_velocity_opt) { return output; @@ -1464,19 +1566,20 @@ void GoalPlannerModule::postProcess() if (!context_data_) { RCLCPP_ERROR(getLogger(), " [pull_over] postProcess() is called without valid context_data"); } - const auto context_data_dummy = - PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, std::nullopt, {}, {}); + 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; - if (!context_data.pull_over_path_opt) { + if (!context_data.selected_pull_over_path_opt) { context_data_ = std::nullopt; return; } - const auto & pull_over_path = context_data.pull_over_path_opt.value(); + const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; const auto distance_to_path_change = calcDistanceToPathChange(context_data); @@ -1503,7 +1606,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() RCLCPP_ERROR( getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); } else { - const auto & context_data = context_data_.value(); + auto & context_data = context_data_.value(); return planPullOverAsCandidate(context_data); } } @@ -1517,12 +1620,12 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data.pull_over_path_opt) { + if (!context_data.selected_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 & pull_over_path = context_data.selected_pull_over_path_opt.value().path; - const auto & full_path = context_data.pull_over_path_opt.value().full_path(); + 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(), @@ -1602,13 +1705,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath(const PullOverContextData & c // 4. feasible stop const auto stop_pose_with_info = std::invoke([&]() -> std::optional> { - if (context_data.pull_over_path_opt) { + if (context_data.selected_pull_over_path_opt) { return std::make_pair( - context_data.pull_over_path_opt.value().start_pose(), "stop at selected start pose"); + context_data.selected_pull_over_path_opt.value().path.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; @@ -1686,89 +1791,6 @@ 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() -{ - const std::lock_guard lock(mutex_); - return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); -} - -bool GoalPlannerModule::isStuck( - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters) -{ - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path_opt = - found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) - : std::nullopt; - const std::optional modified_goal_opt = - pull_over_path_opt - ? std::make_optional(pull_over_path_opt.value().modified_goal()) - : std::nullopt; - const std::lock_guard lock(mutex_); - 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)) { - return false; - } - - if (!found_pull_over_path) { - return true; - } - - const auto & pull_over_path = pull_over_path_opt.value(); - if (parameters.use_object_recognition) { - const auto & path = pull_over_path.getCurrentPath(); - const auto curvatures = autoware::motion_utils::calcCurvature(path.points); - 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, parameters.maximum_deceleration, - parameters.object_recognition_collision_check_max_extra_stopping_margin, - debug_data_.ego_polygons_expanded)) { - return true; - } - } - - if ( - parameters.use_occupancy_grid_for_path_collision_check && - checkOccupancyGridCollision( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), occupancy_grid_map)) { - return true; - } - - return false; -} - bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1777,7 +1799,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d return false; } - if (!isStopped()) { + if (!ctx_data.is_stopped) { return false; } @@ -1790,49 +1812,39 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d return false; } + // check if self pose is near the end of current path + if (!ctx_data.selected_pull_over_path_opt) { + return false; + } + + const auto & [pull_over_path, selected_time, last_path_index_increment_time] = + ctx_data.selected_pull_over_path_opt.value(); + // require increment only when the time passed is enough // 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 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() - selected_time).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 - 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 & current_path_end = pull_over_path.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 std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const -{ - if (!modified_goal_opt) { - return false; - } - - return calcDistance2d(current_pose, modified_goal_opt.value().goal_pose) < - parameters.th_arrived_distance; -} - TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data.pull_over_path_opt) { + if (!context_data.selected_pull_over_path_opt) { return {}; } - const auto & pull_over_path = context_data.pull_over_path_opt.value(); + const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; const auto & path = pull_over_path.full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; @@ -1902,17 +1914,6 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & 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::hasEnoughDistance( const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const { @@ -1954,7 +1955,11 @@ bool GoalPlannerModule::hasEnoughDistance( void GoalPlannerModule::keepStoppedWithCurrentPath( const PullOverContextData & ctx_data, PathWithLaneId & path) const { - const auto last_path_idx_increment_time = ctx_data.last_path_idx_increment_time; + if (!ctx_data.selected_pull_over_path_opt) { + return; + } + const auto last_path_idx_increment_time = + ctx_data.selected_pull_over_path_opt.value().last_path_idx_increment_time; constexpr double keep_stop_time = 2.0; if (!last_path_idx_increment_time) { return; @@ -2416,8 +2421,8 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); // Visualize path and related pose - if (context_data.pull_over_path_opt) { - const auto & pull_over_path = context_data.pull_over_path_opt.value(); + if (context_data.selected_pull_over_path_opt) { + const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; add( createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -2426,8 +2431,8 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) 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 < context_data.pull_over_path_opt.value().partial_paths().size(); ++i) { - const auto & partial_path = context_data.pull_over_path_opt.value().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)); } @@ -2522,16 +2527,17 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - 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); + const auto color = context_data.selected_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 = context_data.pull_over_path_opt - ? context_data.pull_over_path_opt.value().modified_goal_pose() + marker.pose = context_data.selected_pull_over_path_opt + ? context_data.selected_pull_over_path_opt.value().path.modified_goal_pose() : planner_data_->self_odometry->pose.pose; - if (context_data.pull_over_path_opt) { - const auto & pull_over_path = context_data.pull_over_path_opt.value(); + if (context_data.selected_pull_over_path_opt) { + const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; 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); @@ -2544,10 +2550,10 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; } - */ if (isStopped()) { marker.text += " stopped"; } + */ if (debug_data_.freespace_planner.is_planning) { marker.text += @@ -2574,227 +2580,4 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } -bool GoalPlannerModule::needPathUpdate( - const Pose & current_pose, const double path_update_duration, - const std::optional & modified_goal_opt, - const GoalPlannerParameters & parameters) const -{ - return !isOnModifiedGoal(current_pose, modified_goal_opt, 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 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); -} - -GoalPlannerData GoalPlannerData::clone() const -{ - GoalPlannerData gp_planner_data(planner_data, parameters, last_previous_module_output); - gp_planner_data.update( - parameters, planner_data, current_status, previous_module_output, vehicle_footprint, - goal_candidates); - return gp_planner_data; -} - -void GoalPlannerData::update( - const GoalPlannerParameters & parameters_, const PlannerData & planner_data_, - const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, - const autoware::universe_utils::LinearRing2d & vehicle_footprint_, - const GoalCandidates & goal_candidates_) -{ - parameters = parameters_; - vehicle_footprint = vehicle_footprint_; - - planner_data = 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_; - occupancy_grid_map->setMap(*(planner_data.occupancy_grid)); - goal_candidates = goal_candidates_; -} - -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/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp new file mode 100644 index 0000000000000..a6deacb8b43e0 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -0,0 +1,81 @@ +// 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 + +namespace autoware::behavior_path_planner +{ + +void LaneParkingRequest::update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const BehaviorModuleOutput & previous_module_output, + const std::optional> & selected_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; + selected_pull_over_path_ = selected_pull_over_path; + prev_data_ = prev_data; +} + +LaneParkingRequest LaneParkingRequest::clone() const +{ + LaneParkingRequest request( + parameters_, vehicle_footprint_, goal_candidates_, previous_module_output_); + request.update( + *planner_data_, current_status_, previous_module_output_, selected_pull_over_path_, prev_data_); + return request; +} + +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> & selected_pull_over_path, + 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)); + selected_pull_over_path_ = selected_pull_over_path; + is_stopped_ = is_stopped; +} + +FreespaceParkingRequest FreespaceParkingRequest::clone() const +{ + FreespaceParkingRequest request( + parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); + request.update(*planner_data_, current_status_, selected_pull_over_path_, is_stopped_); + return request; +} + +} // namespace autoware::behavior_path_planner