From 4a98ca9f35e8d71c1a6e4ee4db827011991e3170 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 25 Nov 2024 14:05:26 +0900 Subject: [PATCH 1/9] feat(goal_planner): disable freespace feature temporarily Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 1 + .../goal_planner_module.hpp | 211 ++--- .../thread_data.hpp | 190 +++-- .../src/goal_planner_module.cpp | 744 +++++++++--------- .../src/thread_data.cpp | 81 ++ 5 files changed, 662 insertions(+), 565 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index 8e945093659c1..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,7 @@ 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 ) 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 5e6afa16d1f9f..1dd2e452af008 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,9 +18,7 @@ #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/interface/scene_module_interface.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" @@ -84,32 +82,62 @@ struct LastApprovalData Pose pose{}; }; +// store stop_pose_ pointer with reason string +struct PoseWithString +{ + std::optional * pose; + std::string string; + + explicit PoseWithString(std::optional * shared_pose) : pose(shared_pose), string("") {} + + void set(const Pose & new_pose, const std::string & new_string) + { + *pose = new_pose; + string = new_string; + } + + void set(const std::string & new_string) { string = new_string; } + + void clear() + { + pose->reset(); + string = ""; + } +}; + +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( @@ -133,6 +161,11 @@ bool checkOccupancyGridCollision( const PathWithLaneId & path, const std::shared_ptr occupancy_grid_map); +// freespace parking +std::optional planFreespacePath( + const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects, + std::shared_ptr freespace_planner); + bool isStopped( std::deque & odometry_buffer, const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, @@ -150,6 +183,61 @@ class ScopedFlag std::atomic & flag_; }; +class LaneParkingPlanner +{ +public: + LaneParkingPlanner( + rclcpp::Node & node, std::mutex & lane_parking_mutex, + const std::optional & request, LaneParkingResponse & response, + std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, + const GoalPlannerParameters & parameters); + rclcpp::Logger getLogger() const { return logger_; } + void onTimer(); + +private: + std::mutex & mutex_; + const std::optional & request_; + LaneParkingResponse & response_; + std::atomic & is_lane_parking_cb_running_; + rclcpp::Logger logger_; + + std::vector> pull_over_planners_; +}; + +class FreespaceParkingPlanner +{ +public: + FreespaceParkingPlanner( + std::mutex & freespace_parking_mutex, const std::optional & request, + FreespaceParkingResponse & response, std::atomic & is_freespace_parking_cb_running, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr freespace_planner) + : mutex_(freespace_parking_mutex), + request_(request), + response_(response), + is_freespace_parking_cb_running_(is_freespace_parking_cb_running), + logger_(logger), + clock_(clock), + freespace_planner_(freespace_planner) + { + } + void onTimer(); + +private: + std::mutex & mutex_; + const std::optional & request_; + FreespaceParkingResponse & response_; + std::atomic & is_freespace_parking_cb_running_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + std::shared_ptr freespace_planner_; + + bool isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const FreespaceParkingRequest & req) const; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -217,48 +305,15 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - /** - * @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this) - */ - struct GoalPlannerData - { - GoalPlannerData( - const PlannerData & planner_data, const GoalPlannerParameters & parameters, - 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); - }; - std::optional gp_planner_data_{std::nullopt}; - std::mutex gp_planner_data_mutex_; + std::pair syncWithThreads(); + + std::mutex lane_parking_mutex_; + std::optional lane_parking_request_; + LaneParkingResponse lane_parking_response_; + + std::mutex freespace_parking_mutex_; + std::optional freespace_parking_request_; + FreespaceParkingResponse freespace_parking_response_; /* * state transitions and plan function used in each state @@ -291,15 +346,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 @@ -317,11 +369,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()}; @@ -367,14 +414,9 @@ class GoalPlannerModule : public SceneModuleInterface double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status - bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); + bool hasFinishedCurrentPath(const PullOverContextData & ctx_data) const; double calcModuleRequestLength() 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 @@ -385,21 +427,14 @@ 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 planPullOver(PullOverContextData & context_data); + BehaviorModuleOutput planPullOverAsOutput(PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsCandidate( - const PullOverContextData & context_data, const std::string & detail); + PullOverContextData & context_data, const std::string & detail); std::optional selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, @@ -411,7 +446,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; @@ -421,10 +458,6 @@ class GoalPlannerModule : public SceneModuleInterface TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data); std::optional ignore_signal_{std::nullopt}; - // 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 549319b42ee4c..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 @@ -16,143 +16,129 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ #include "autoware/behavior_path_goal_planner_module/decision_state.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include #include -#include #include +#include #include namespace autoware::behavior_path_planner { -#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) - -class ThreadSafeData +class LaneParkingRequest { public: - ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) - : mutex_(mutex), clock_(clock) + 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) { } - bool incrementPathIndex() - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - return pull_over_path_->incrementPathIndex(); - } + 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 set_pull_over_path(const PullOverPath & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } + const GoalPlannerParameters parameters_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; - void set_pull_over_path(const std::shared_ptr & path) + 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_); - set_pull_over_path_no_lock(path); + return previous_module_output_; } - - template - void set(Args... args) + const BehaviorModuleOutput & get_last_previous_module_output() const { - std::lock_guard lock(mutex_); - (..., set_no_lock(args)); + return last_previous_module_output_; } - - void clearPullOverPath() + const std::optional> & get_selected_pull_over_path() const { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; + return selected_pull_over_path_; } + const PathDecisionState & get_prev_data() const { return prev_data_; } + LaneParkingRequest clone() const; - bool foundPullOverPath() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } +private: + std::shared_ptr planner_data_; + ModuleStatus current_status_; + BehaviorModuleOutput previous_module_output_; + BehaviorModuleOutput last_previous_module_output_; + std::optional> + selected_pull_over_path_; // pull_over_path_candidates; + std::optional closest_start_pose; +}; - void reset() +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_); - pull_over_path_ = nullptr; - pull_over_path_candidates_.clear(); - last_path_update_time_ = std::nullopt; - closest_start_pose_ = std::nullopt; - prev_data_ = PathDecisionState{}; - } - - // main --> lane - DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) - - // lane --> main - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) - - // main <--> lane/freespace - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - -private: - void set_pull_over_path_no_lock(const PullOverPath & path) + initializeOccupancyGridMap(planner_data, parameters_); + }; + + 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); + + const GoalPlannerParameters parameters_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; + + const std::shared_ptr & get_planner_data() const { return planner_data_; } + const ModuleStatus & get_current_status() const { return current_status_; } + std::shared_ptr get_occupancy_grid_map() const { - pull_over_path_ = std::make_shared(path); - - last_path_update_time_ = clock_->now(); + return occupancy_grid_map_; } - - void set_pull_over_path_no_lock(const std::shared_ptr & path) + const std::optional> & get_selected_pull_over_path() const { - pull_over_path_ = path; - last_path_update_time_ = clock_->now(); + return selected_pull_over_path_; } + bool is_stopped() const { return is_stopped_; } - 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); } + FreespaceParkingRequest clone() const; - 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_{}; - - std::recursive_mutex & mutex_; - rclcpp::Clock::SharedPtr clock_; +private: + std::shared_ptr planner_data_; + ModuleStatus current_status_; + std::shared_ptr occupancy_grid_map_; + std::optional> selected_pull_over_path_; + bool is_stopped_; + + void initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters); }; -#undef DEFINE_SETTER_WITH_MUTEX -#undef DEFINE_GETTER_WITH_MUTEX -#undef DEFINE_SETTER_GETTER_WITH_MUTEX +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/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index e1f667d91b692..3dc79b790b199 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" @@ -70,17 +71,9 @@ 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} { - 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; @@ -88,26 +81,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_WARN( - getLogger(), - "No enabled planner found. The vehicle will stop in the road lane without pull over. Please " - "check the parameters if this is the intended behavior."); - } - // set selected goal searcher // currently there is only one goal_searcher_type const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); @@ -118,19 +91,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_); } @@ -241,6 +225,35 @@ bool checkOccupancyGridCollision( 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, @@ -267,50 +280,72 @@ bool isStopped( 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_INFO_THROTTLE( - getLogger(), *clock_, 5000, - "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 & local_planner_data = local_request.get_planner_data(); + const auto & current_status = local_request.get_current_status(); + const auto & previous_module_output = local_request.get_previous_module_output(); + const auto & last_previous_module_output = local_request.get_last_previous_module_output(); + const auto & 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; @@ -326,16 +361,17 @@ void GoalPlannerModule::onTimer() } // 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)) { @@ -345,9 +381,6 @@ void GoalPlannerModule::onTimer() RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { - return true; - } if (hasPreviousModulePathShapeChanged(previous_module_output, last_previous_module_output)) { RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; @@ -431,51 +464,40 @@ void GoalPlannerModule::onTimer() throw std::domain_error("[pull_over] invalid path_priority"); } - // set member variables - thread_safe_data_.set_pull_over_path_candidates(path_candidates); - thread_safe_data_.set_closest_start_pose(closest_start_pose); - RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); + // 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(getLogger(), "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(request); } } // end of critical section - if ( - !local_planner_data || !current_status_opt || !occupancy_grid_map || !parameters_opt || - !vehicle_footprint_opt || !goal_candidates_opt) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "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 & local_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; @@ -489,20 +511,16 @@ void GoalPlannerModule::onFreespaceParkingTimer() 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()) + 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)) { return; } - const double vehicle_width = planner_data_->parameters.vehicle_width; + const double vehicle_width = local_planner_data->parameters.vehicle_width; const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(local_planner_data->route_handler), left_side_parking, parameters.backward_goal_search_length, @@ -526,82 +544,65 @@ void GoalPlannerModule::onFreespaceParkingTimer() const bool is_new_costmap = (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; + const auto selected_time_opt = + selected_pull_over_path + ? std::make_optional(selected_pull_over_path.value().second) + : std::nullopt; + 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, clock_->now(), - modified_goal_opt, thread_safe_data_.get_last_path_update_time(), parameters)) { - auto goal_searcher = std::make_shared(parameters, vehicle_footprint); - - planFreespacePath( - local_planner_data, goal_searcher, goal_candidates, occupancy_grid_map, - static_target_objects); + modified_goal_opt, selected_time_opt, 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::updateData() +std::pair GoalPlannerModule::syncWithThreads() { - 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(); - } - // 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 @@ -613,15 +614,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; @@ -631,46 +685,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(); + } } } @@ -678,7 +735,8 @@ 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); } } @@ -696,7 +754,6 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - thread_safe_data_.reset(); last_approval_data_.reset(); } @@ -833,48 +890,6 @@ 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 @@ -884,13 +899,20 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte 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(); @@ -954,7 +976,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, " [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); } } @@ -1228,13 +1250,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, (goal_candidates_.empty() ? "no goal candidate" : "no static safe path")); @@ -1244,7 +1267,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()) { @@ -1283,8 +1306,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; @@ -1303,10 +1327,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 { @@ -1343,15 +1367,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) { @@ -1359,7 +1382,7 @@ void GoalPlannerModule::decideVelocity() } } -BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & context_data) +BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1381,12 +1404,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & } BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( - const PullOverContextData & context_data, const std::string & detail) + PullOverContextData & context_data, const std::string & detail) { 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(); } @@ -1404,7 +1427,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; } @@ -1413,8 +1436,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_); @@ -1422,22 +1444,30 @@ 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()) + const auto selected_modified_goal_opt = + context_data.selected_pull_over_path_opt + ? std::make_optional( + context_data.selected_pull_over_path_opt.value().path.modified_goal()) + : std::nullopt; + const auto selected_time_opt = + context_data.selected_pull_over_path_opt + ? std::make_optional( + context_data.selected_pull_over_path_opt.value().selected_time) : std::nullopt; if ( path_decision_controller_.get_current_state().state == @@ -1445,13 +1475,13 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), - modified_goal_opt, thread_safe_data_.get_last_path_update_time(), *parameters_)) { + selected_modified_goal_opt, selected_time_opt, *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_; @@ -1459,13 +1489,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 @@ -1476,7 +1511,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); + 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() @@ -1492,20 +1528,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; @@ -1526,19 +1560,20 @@ void GoalPlannerModule::postProcess() getLogger(), *clock_, 5000, " [pull_over] postProcess() is called without valid context_data. use dummy context data."); } - const auto context_data_dummy = - PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, 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); @@ -1567,7 +1602,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() " [pull_over] planWaitingApproval() is called without valid context_data. use fixed goal " "planner"); } else { - const auto & context_data = context_data_.value(); + auto & context_data = context_data_.value(); return planPullOverAsCandidate(context_data, "waiting approval"); } } @@ -1581,12 +1616,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(), @@ -1665,17 +1700,27 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto stop_pose_opt = std::invoke([&]() -> std::optional { - if (context_data.pull_over_path_opt) - return context_data.pull_over_path_opt.value().start_pose(); - if (thread_safe_data_.get_closest_start_pose()) - return thread_safe_data_.get_closest_start_pose().value(); - if (!decel_pose) return std::nullopt; - return decel_pose.value(); - }); - if (!stop_pose_opt.has_value()) { - const auto feasible_stop_path = - generateFeasibleStopPath(getPreviousModuleOutput().path, detail); + const auto stop_pose_with_info = + std::invoke([&]() -> std::optional> { + if (context_data.selected_pull_over_path_opt) { + return std::make_pair( + context_data.selected_pull_over_path_opt.value().path.start_pose(), + "stop at selected start pose"); + } + if (context_data.lane_parking_response.closest_start_pose) { + return std::make_pair( + context_data.lane_parking_response.closest_start_pose.value(), + "stop at closest start pose"); + } + if (!decel_pose) { + return std::nullopt; + } + return std::make_pair(decel_pose.value(), "stop at search start pose"); + }); + if (!stop_pose_with_info) { + const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); + // override stop pose info debug string + debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose")); return feasible_stop_path; } const Pose stop_pose = stop_pose_opt.value(); @@ -1743,61 +1788,51 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath( return stop_path; } -bool GoalPlannerModule::isStuck( +bool FreespaceParkingPlanner::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 FreespaceParkingRequest & req) const { - 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)) { + if (!req.get_selected_pull_over_path()) { return false; } - constexpr double stuck_time = 5.0; - if (!isStopped( - odometry_buffer_stuck_, planner_data->self_odometry, stuck_time, - parameters_->th_stopped_velocity)) { + 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; } - if (!found_pull_over_path) { - return true; + if (req.is_stopped()) { + return false; } - 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); + 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, - debug_data_.ego_polygons_expanded)) { + 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)) { + checkOccupancyGridCollision(pull_over_path.getCurrentPath(), req.get_occupancy_grid_map())) { return true; } return false; } -bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) +bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1805,9 +1840,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d return false; } - if (!isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity)) { + if (!ctx_data.is_stopped) { return false; } @@ -1820,24 +1853,26 @@ 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; @@ -1847,10 +1882,10 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & { 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; @@ -1961,7 +1996,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; @@ -2423,8 +2462,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( @@ -2433,8 +2472,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)); } @@ -2529,16 +2568,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); @@ -2567,48 +2607,4 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } -void GoalPlannerModule::GoalPlannerData::initializeOccupancyGridMap( - const PlannerData & planner_data, const GoalPlannerParameters & parameters) -{ - OccupancyGridMapParam occupancy_grid_map_param{}; - const double margin = parameters.occupancy_grid_collision_check_margin; - occupancy_grid_map_param.vehicle_shape.length = - planner_data.parameters.vehicle_length + 2 * margin; - occupancy_grid_map_param.vehicle_shape.width = planner_data.parameters.vehicle_width + 2 * margin; - occupancy_grid_map_param.vehicle_shape.base2back = - planner_data.parameters.base_link2rear + margin; - occupancy_grid_map_param.theta_size = parameters.theta_size; - occupancy_grid_map_param.obstacle_threshold = parameters.obstacle_threshold; - occupancy_grid_map = std::make_shared(); - occupancy_grid_map->setParam(occupancy_grid_map_param); -} - -GoalPlannerModule::GoalPlannerData GoalPlannerModule::GoalPlannerData::clone() const -{ - GoalPlannerModule::GoalPlannerData gp_planner_data( - planner_data, parameters, 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 GoalPlannerModule::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_; -} - } // 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 From cdac350155ebf0a4368b2c3fad40fa5359048162 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Dec 2024 11:47:43 +0900 Subject: [PATCH 2/9] revert SelectedPullOverPath Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 47 +--- .../thread_data.hpp | 23 +- .../src/goal_planner_module.cpp | 260 ++++++++---------- .../src/thread_data.cpp | 17 +- 4 files changed, 145 insertions(+), 202 deletions(-) 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 1dd2e452af008..742bdb26b2561 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 @@ -82,36 +82,6 @@ struct LastApprovalData Pose pose{}; }; -// store stop_pose_ pointer with reason string -struct PoseWithString -{ - std::optional * pose; - std::string string; - - explicit PoseWithString(std::optional * shared_pose) : pose(shared_pose), string("") {} - - void set(const Pose & new_pose, const std::string & new_string) - { - *pose = new_pose; - string = new_string; - } - - void set(const std::string & new_string) { string = new_string; } - - void clear() - { - pose->reset(); - string = ""; - } -}; - -struct SelectedPullOverPath -{ - PullOverPath path; - rclcpp::Time selected_time; - std::optional last_path_idx_increment_time; -}; - struct PullOverContextData { PullOverContextData() = delete; @@ -136,8 +106,11 @@ struct PullOverContextData 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; + // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it), + // and bind following three member, rename as "SelectedPullOverPath" + std::optional pull_over_path_opt; + std::optional last_path_update_time; + std::optional last_path_idx_increment_time; }; bool isOnModifiedGoal( @@ -307,6 +280,7 @@ class GoalPlannerModule : public SceneModuleInterface private: std::pair syncWithThreads(); + // NOTE: never access to following variables except in updateData()!!! std::mutex lane_parking_mutex_; std::optional lane_parking_request_; LaneParkingResponse lane_parking_response_; @@ -346,9 +320,9 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr parameters_; - std::shared_ptr ego_predicted_path_params_; - std::shared_ptr objects_filtering_params_; - std::shared_ptr safety_check_params_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; @@ -414,7 +388,7 @@ class GoalPlannerModule : public SceneModuleInterface double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status - bool hasFinishedCurrentPath(const PullOverContextData & ctx_data) const; + bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); double calcModuleRequestLength() const; void decideVelocity(PullOverPath & pull_over_path); void updateStatus(const BehaviorModuleOutput & output); @@ -427,7 +401,6 @@ 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 canReturnToLaneParking(const PullOverContextData & context_data); // plan pull over path 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 ed2766bcc7cac..244c0be624f23 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 @@ -46,8 +46,7 @@ class LaneParkingRequest 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); + const std::optional & pull_over_path, const PathDecisionState & prev_data); const GoalPlannerParameters parameters_; const autoware::universe_utils::LinearRing2d vehicle_footprint_; @@ -63,10 +62,7 @@ class LaneParkingRequest { return last_previous_module_output_; } - const std::optional> & get_selected_pull_over_path() const - { - return selected_pull_over_path_; - } + const std::optional & get_pull_over_path() const { return pull_over_path_; } const PathDecisionState & get_prev_data() const { return prev_data_; } LaneParkingRequest clone() const; @@ -75,8 +71,7 @@ class LaneParkingRequest ModuleStatus current_status_; BehaviorModuleOutput previous_module_output_; BehaviorModuleOutput last_previous_module_output_; - std::optional> - selected_pull_over_path_; // pull_over_path_; //> & selected_pull_over_path, - const bool is_stopped); + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped); const GoalPlannerParameters parameters_; const autoware::universe_utils::LinearRing2d vehicle_footprint_; @@ -116,9 +111,10 @@ class FreespaceParkingRequest { return occupancy_grid_map_; } - const std::optional> & get_selected_pull_over_path() const + const std::optional & get_pull_over_path() const { return pull_over_path_; } + const std::optional & get_last_path_update_time() const { - return selected_pull_over_path_; + return last_path_update_time_; } bool is_stopped() const { return is_stopped_; } @@ -128,7 +124,8 @@ class FreespaceParkingRequest std::shared_ptr planner_data_; ModuleStatus current_status_; std::shared_ptr occupancy_grid_map_; - std::optional> selected_pull_over_path_; + std::optional pull_over_path_; + std::optional last_path_update_time_; bool is_stopped_; void initializeOccupancyGridMap( 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 3dc79b790b199..c6ea313f81dd5 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 @@ -344,7 +344,7 @@ void LaneParkingPlanner::onTimer() 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 & pull_over_path_opt = local_request.get_pull_over_path(); const auto & prev_data = local_request.get_prev_data(); if (current_status == ModuleStatus::IDLE) { @@ -370,8 +370,8 @@ void LaneParkingPlanner::onTimer() } } const std::optional modified_goal_opt = - selected_pull_over_path - ? std::make_optional(selected_pull_over_path.value().first.modified_goal()) + 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)) { @@ -496,7 +496,8 @@ void FreespaceParkingPlanner::onTimer() const auto & parameters = local_request.parameters_; const auto & local_planner_data = local_request.get_planner_data(); const auto & current_status = local_request.get_current_status(); - const auto & selected_pull_over_path = local_request.get_selected_pull_over_path(); + const auto & pull_over_path_opt = local_request.get_pull_over_path(); + const auto & last_path_update_time = local_request.get_last_path_update_time(); const auto & occupancy_grid_map = local_request.get_occupancy_grid_map(); if (current_status == ModuleStatus::IDLE) { @@ -512,8 +513,8 @@ void FreespaceParkingPlanner::onTimer() } const std::optional modified_goal_opt = - selected_pull_over_path - ? std::make_optional(selected_pull_over_path.value().first.modified_goal()) + 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)) { @@ -544,16 +545,11 @@ void FreespaceParkingPlanner::onTimer() const bool is_new_costmap = (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; - const auto selected_time_opt = - selected_pull_over_path - ? std::make_optional(selected_pull_over_path.value().second) - : std::nullopt; - if ( isStuck(static_target_objects, dynamic_target_objects, local_request) && is_new_costmap && needPathUpdate( local_planner_data->self_odometry->pose.pose, path_update_duration, clock_->now(), - modified_goal_opt, selected_time_opt, parameters)) { + modified_goal_opt, last_path_update_time, parameters)) { const auto freespace_path_opt = planFreespacePath(local_request, static_target_objects, freespace_planner_); if (freespace_path_opt) { @@ -571,15 +567,10 @@ std::pair GoalPlannerModule::sync // 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); - } + std::optional pull_over_path = + context_data_ ? context_data_.value().pull_over_path_opt : std::nullopt; + std::optional last_path_update_time = + context_data_ ? context_data_.value().last_path_update_time : std::nullopt; // NOTE: 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 @@ -601,7 +592,7 @@ std::pair GoalPlannerModule::sync // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) // and if these two coincided, only the reference count is affected lane_parking_request.update( - *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), selected_pull_over_path, + *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path, path_decision_controller_.get_current_state()); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since @@ -627,8 +618,10 @@ std::pair GoalPlannerModule::sync 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)); + *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time, + isStopped( + odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time, + parameters_->th_stopped_velocity)); // 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 @@ -686,35 +679,37 @@ void GoalPlannerModule::updateData() path_reference_ = std::make_shared(getPreviousModuleOutput().reference_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; + context_data_ ? context_data_.value().pull_over_path_opt.has_value() : false; + std::optional pull_over_path_recv = + found_pull_over_path + ? std::make_optional(context_data_.value().pull_over_path_opt.value()) + : std::nullopt; const auto modified_goal_pose = [&]() -> std::optional { - if (!pull_over_path) { + if (!pull_over_path_recv) { return std::nullopt; } - return pull_over_path.value().modified_goal(); + return pull_over_path_recv.value().modified_goal(); }(); // save "old" state const auto prev_decision_state = path_decision_controller_.get_current_state(); const auto [is_current_safe, collision_check_map] = isSafePath( - planner_data_, found_pull_over_path, pull_over_path, prev_decision_state, *parameters_, + planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); debug_data_.collision_check = collision_check_map; // update to latest state path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, - goal_searcher_, isActivated(), pull_over_path, debug_data_.ego_polygons_expanded); + goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, dynamic_target_objects, prev_decision_state, - isStopped(odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time), + isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); auto & ctx_data = context_data_.value(); @@ -723,10 +718,10 @@ void GoalPlannerModule::updateData() } if (hasFinishedCurrentPath(ctx_data)) { - if (ctx_data.selected_pull_over_path_opt) { - auto & pull_over_path = ctx_data.selected_pull_over_path_opt.value().path; + if (ctx_data.pull_over_path_opt) { + auto & pull_over_path = ctx_data.pull_over_path_opt.value(); if (pull_over_path.incrementPathIndex()) { - ctx_data.selected_pull_over_path_opt.value().last_path_idx_increment_time = clock_->now(); + ctx_data.last_path_idx_increment_time = clock_->now(); } } } @@ -735,8 +730,7 @@ void GoalPlannerModule::updateData() last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); // TODO(soblin): do not "plan" in updateData - if (ctx_data.selected_pull_over_path_opt) - decideVelocity(ctx_data.selected_pull_over_path_opt.value().path); + if (ctx_data.pull_over_path_opt) decideVelocity(ctx_data.pull_over_path_opt.value()); } } @@ -899,12 +893,10 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return false; } - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return false; } - if ( - context_data.selected_pull_over_path_opt.value().path.type() == - PullOverPlannerType::FREESPACE) { + if (context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { return false; } // TODO(soblin): return from freespace to lane is disabled temporarily, because if @@ -912,7 +904,7 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte // 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 & lane_parking_path = context_data.pull_over_path_opt.value(); const auto & path = lane_parking_path.full_path(); const auto & curvatures = lane_parking_path.full_path_curvatures(); @@ -1267,7 +1259,7 @@ void GoalPlannerModule::setOutput( return; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); if ( parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { @@ -1306,9 +1298,8 @@ void GoalPlannerModule::setDrivableAreaInfo( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if ( - context_data.selected_pull_over_path_opt && - context_data.selected_pull_over_path_opt.value().path.type() == - PullOverPlannerType::FREESPACE) { + context_data.pull_over_path_opt && + context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1327,10 +1318,10 @@ void GoalPlannerModule::setModifiedGoal( const PullOverContextData & context_data, BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (context_data.selected_pull_over_path_opt) { + if (context_data.pull_over_path_opt) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = context_data.selected_pull_over_path_opt.value().path.modified_goal_pose(); + modified_goal.pose = context_data.pull_over_path_opt.value().modified_goal_pose(); modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -1389,14 +1380,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & conte const auto current_state = path_decision_controller_.get_current_state(); if (current_state.state != PathDecisionState::DecisionKind::DECIDED) { const bool is_stable_safe = current_state.is_stable_safe; - // clang-format off const std::string detail = - goal_candidates_.empty() ? "no goal candidate" : - context_data.pull_over_path_candidates.empty() ? "no path candidate" : - !thread_safe_data_.foundPullOverPath() ? "no static safe path" : - !is_stable_safe ? "unsafe against dynamic objects" : - "too far goal"; - // clang-format on + goal_candidates_.empty() ? "no goal candidate" + : context_data.lane_parking_response.pull_over_path_candidates.empty() ? "no path candidate" + : !context_data.pull_over_path_opt ? "no static safe path" + : !is_stable_safe ? "unsafe against dynamic objects" + : "too far goal"; return planPullOverAsCandidate(context_data, detail); } @@ -1427,7 +1416,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return output; } @@ -1452,36 +1441,30 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData 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.selected_pull_over_path_opt - ? std::make_optional(context_data.selected_pull_over_path_opt.value().path) - : std::nullopt; + auto pull_over_path_with_velocity_opt = context_data.pull_over_path_opt; const bool is_freespace = pull_over_path_with_velocity_opt && pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE; - const auto selected_modified_goal_opt = - context_data.selected_pull_over_path_opt - ? std::make_optional( - context_data.selected_pull_over_path_opt.value().path.modified_goal()) - : std::nullopt; - const auto selected_time_opt = - context_data.selected_pull_over_path_opt - ? std::make_optional( - context_data.selected_pull_over_path_opt.value().selected_time) + const std::optional modified_goal_opt = + pull_over_path_with_velocity_opt + ? std::make_optional(pull_over_path_with_velocity_opt.value().modified_goal()) : std::nullopt; + const auto & last_path_update_time = context_data.last_path_update_time; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), - selected_modified_goal_opt, selected_time_opt, *parameters_)) { + modified_goal_opt, last_path_update_time, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); - context_data.selected_pull_over_path_opt = std::nullopt; + context_data.pull_over_path_opt = std::nullopt; + context_data.last_path_update_time = std::nullopt; + context_data.last_path_idx_increment_time = std::nullopt; // update goal candidates auto goal_candidates = goal_candidates_; @@ -1495,10 +1478,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ - 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; + const auto & pull_over_path_opt = + lane_pull_over_path_opt ? lane_pull_over_path_opt + : context_data.freespace_parking_response.freespace_pull_over_path; if (pull_over_path_opt) { const auto & pull_over_path = pull_over_path_opt.value(); /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old @@ -1511,8 +1493,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData * As the next action item, only set this selected pull_over_path to only * FreespaceThreadSafeData. */ - context_data.selected_pull_over_path_opt = - SelectedPullOverPath{pull_over_path, clock_->now(), std::nullopt}; + context_data.pull_over_path_opt = pull_over_path; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; if (pull_over_path_with_velocity_opt) { auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); // copy the path for later setOutput() @@ -1533,8 +1516,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData // return to lane parking if it is possible 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}; + context_data.pull_over_path_opt = pull_over_path_with_velocity_opt; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; } } @@ -1569,11 +1553,11 @@ void GoalPlannerModule::postProcess() const bool has_decided_path = path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { context_data_ = std::nullopt; return; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); const auto distance_to_path_change = calcDistanceToPathChange(context_data); @@ -1616,10 +1600,10 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); const auto & full_path = pull_over_path.full_path(); @@ -1700,27 +1684,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto stop_pose_with_info = - std::invoke([&]() -> std::optional> { - if (context_data.selected_pull_over_path_opt) { - return std::make_pair( - context_data.selected_pull_over_path_opt.value().path.start_pose(), - "stop at selected start pose"); - } - if (context_data.lane_parking_response.closest_start_pose) { - return std::make_pair( - context_data.lane_parking_response.closest_start_pose.value(), - "stop at closest start pose"); - } - if (!decel_pose) { - return std::nullopt; - } - return std::make_pair(decel_pose.value(), "stop at search start pose"); - }); - if (!stop_pose_with_info) { - const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); - // override stop pose info debug string - debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose")); + const auto stop_pose_opt = std::invoke([&]() -> std::optional { + if (context_data.pull_over_path_opt) + return context_data.pull_over_path_opt.value().start_pose(); + if (context_data.lane_parking_response.closest_start_pose) + return context_data.lane_parking_response.closest_start_pose.value(); + if (!decel_pose) return std::nullopt; + return decel_pose.value(); + }); + if (!stop_pose_opt.has_value()) { + const auto feasible_stop_path = + generateFeasibleStopPath(getPreviousModuleOutput().path, detail); return feasible_stop_path; } const Pose stop_pose = stop_pose_opt.value(); @@ -1792,25 +1766,26 @@ bool FreespaceParkingPlanner::isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const FreespaceParkingRequest & req) const { - if (!req.get_selected_pull_over_path()) { - return false; - } - 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)) { + const std::optional modified_goal_opt = + req.get_pull_over_path() + ? std::make_optional(req.get_pull_over_path().value().modified_goal()) + : std::nullopt; + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } - if (req.is_stopped()) { + if (!req.is_stopped()) { return false; } + if (!req.get_pull_over_path()) { + return true; + } + if (parameters.use_object_recognition) { - const auto & path = pull_over_path.getCurrentPath(); + const auto & path = req.get_pull_over_path().value().getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); std::vector ego_polygons_expanded; if (goal_planner_utils::checkObjectsCollision( @@ -1825,14 +1800,15 @@ bool FreespaceParkingPlanner::isStuck( if ( parameters.use_occupancy_grid_for_path_collision_check && - checkOccupancyGridCollision(pull_over_path.getCurrentPath(), req.get_occupancy_grid_map())) { + checkOccupancyGridCollision( + req.get_pull_over_path().value().getCurrentPath(), req.get_occupancy_grid_map())) { return true; } return false; } -bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) const +bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1853,26 +1829,27 @@ 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 + if (!ctx_data.last_path_idx_increment_time) { + return false; + } constexpr double keep_current_idx_time = 4.0; const bool has_passed_enough_time_from_increment = - (clock_->now() - selected_time).seconds() > keep_current_idx_time; + (clock_->now() - ctx_data.last_path_idx_increment_time.value()).seconds() > + keep_current_idx_time; if (!has_passed_enough_time_from_increment) { return false; } - const auto & current_path_end = pull_over_path.getCurrentPath().points.back(); + // 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 & self_pose = planner_data_->self_odometry->pose.pose; return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1882,10 +1859,10 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data.selected_pull_over_path_opt) { + if (!context_data.pull_over_path_opt) { return {}; } - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + const auto & pull_over_path = context_data.pull_over_path_opt.value(); const auto & path = pull_over_path.full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; @@ -1996,11 +1973,7 @@ bool GoalPlannerModule::hasEnoughDistance( void GoalPlannerModule::keepStoppedWithCurrentPath( const PullOverContextData & ctx_data, PathWithLaneId & path) const { - 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; + const auto last_path_idx_increment_time = ctx_data.last_path_idx_increment_time; constexpr double keep_stop_time = 2.0; if (!last_path_idx_increment_time) { return; @@ -2462,8 +2435,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.selected_pull_over_path_opt) { - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); add( createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -2568,17 +2541,16 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - 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); + const auto color = context_data.pull_over_path_opt ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = context_data.selected_pull_over_path_opt - ? context_data.selected_pull_over_path_opt.value().path.modified_goal_pose() + marker.pose = context_data.pull_over_path_opt + ? context_data.pull_over_path_opt.value().modified_goal_pose() : planner_data_->self_odometry->pose.pose; - if (context_data.selected_pull_over_path_opt) { - const auto & pull_over_path = context_data.selected_pull_over_path_opt.value().path; + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); marker.text = magic_enum::enum_name(pull_over_path.type()); marker.text += " " + std::to_string(pull_over_path.path_idx()) + "/" + std::to_string(pull_over_path.partial_paths().size() - 1); 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 index a6deacb8b43e0..6abfd714f4f89 100644 --- 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 @@ -20,15 +20,14 @@ 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) + const std::optional & pull_over_path, const PathDecisionState & prev_data) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); current_status_ = current_status; last_previous_module_output_ = previous_module_output_; previous_module_output_ = previous_module_output; - selected_pull_over_path_ = selected_pull_over_path; + pull_over_path_ = pull_over_path; prev_data_ = prev_data; } @@ -37,7 +36,7 @@ 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_); + *planner_data_, current_status_, previous_module_output_, pull_over_path_, prev_data_); return request; } @@ -59,14 +58,15 @@ void FreespaceParkingRequest::initializeOccupancyGridMap( void FreespaceParkingRequest::update( const PlannerData & planner_data, const ModuleStatus & current_status, - const std::optional> & selected_pull_over_path, - const bool is_stopped) + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); current_status_ = current_status; occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); - selected_pull_over_path_ = selected_pull_over_path; + pull_over_path_ = pull_over_path; + last_path_update_time_ = last_path_update_time; is_stopped_ = is_stopped; } @@ -74,7 +74,8 @@ 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_); + request.update( + *planner_data_, current_status_, pull_over_path_, last_path_update_time_, is_stopped_); return request; } From 362d70a842a55a826a1763a8072107fd1501a19d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 12 Dec 2024 10:50:16 +0900 Subject: [PATCH 3/9] does not do pullover Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 36 +++++++++----- .../src/goal_planner_module.cpp | 47 +++++++++++++------ 2 files changed, 58 insertions(+), 25 deletions(-) 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 742bdb26b2561..9dad3681f7570 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 @@ -88,8 +88,8 @@ struct PullOverContextData explicit PullOverContextData( const bool is_stable_safe_path, const PredictedObjects & static_objects, const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state, - const bool is_stopped, LaneParkingResponse && lane_parking_response, - FreespaceParkingResponse && freespace_parking_response) + const bool is_stopped, const LaneParkingResponse & lane_parking_response, + const FreespaceParkingResponse & freespace_parking_response) : is_stable_safe_path(is_stable_safe_path), static_target_objects(static_objects), dynamic_target_objects(dynamic_objects), @@ -99,18 +99,32 @@ struct PullOverContextData freespace_parking_response(freespace_parking_response) { } - const bool is_stable_safe_path; - const PredictedObjects static_target_objects; - const PredictedObjects dynamic_target_objects; - const PathDecisionState prev_state_for_debug; - const bool is_stopped; - const LaneParkingResponse lane_parking_response; - const FreespaceParkingResponse freespace_parking_response; - // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it), - // and bind following three member, rename as "SelectedPullOverPath" + // TODO(soblin): make following variables private + bool is_stable_safe_path; + PredictedObjects static_target_objects; + PredictedObjects dynamic_target_objects; + PathDecisionState prev_state_for_debug; + bool is_stopped; + LaneParkingResponse lane_parking_response; + FreespaceParkingResponse freespace_parking_response; std::optional pull_over_path_opt; std::optional last_path_update_time; std::optional last_path_idx_increment_time; + + void update( + const bool is_stable_safe_path_, const PredictedObjects static_target_objects_, + const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_, + const bool is_stopped_, const LaneParkingResponse & lane_parking_response_, + const FreespaceParkingResponse & freespace_parking_response_) + { + is_stable_safe_path = is_stable_safe_path_; + static_target_objects = static_target_objects_; + dynamic_target_objects = dynamic_target_objects_; + prev_state_for_debug = prev_state_for_debug_; + is_stopped = is_stopped_; + lane_parking_response = lane_parking_response_; + freespace_parking_response = freespace_parking_response_; + } }; bool isOnModifiedGoal( 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 c6ea313f81dd5..12ef1ed84f656 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 @@ -329,7 +329,7 @@ void LaneParkingPlanner::onTimer() std::lock_guard guard(mutex_); if (request_) { auto & request = request_.value(); - local_request_opt.emplace(std::move(request)); + local_request_opt.emplace(request); } } // end of critical section @@ -467,9 +467,11 @@ void LaneParkingPlanner::onTimer() // set response { std::lock_guard guard(mutex_); - response_.pull_over_path_candidates = std::move(path_candidates); + response_.pull_over_path_candidates = path_candidates; response_.closest_start_pose = closest_start_pose; - RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); + RCLCPP_INFO( + getLogger(), "generated %lu pull over path candidates", + response_.pull_over_path_candidates.size()); } } @@ -605,7 +607,7 @@ std::pair GoalPlannerModule::sync // `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_); + lane_parking_response = lane_parking_response_; } FreespaceParkingResponse freespace_parking_response; @@ -628,7 +630,7 @@ std::pair GoalPlannerModule::sync // 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_ = freespace_parking_request.get_occupancy_grid_map(); - freespace_parking_response = std::move(freespace_parking_response_); + freespace_parking_response = freespace_parking_response_; } // end of critical section return {lane_parking_response, freespace_parking_response}; @@ -704,13 +706,23 @@ void GoalPlannerModule::updateData() modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); - context_data_.emplace( - path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, - dynamic_target_objects, prev_decision_state, - isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity), - std::move(lane_parking_response), std::move(freespace_parking_response)); + if (context_data_) { + context_data_.value().update( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects, prev_decision_state, + isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity), + lane_parking_response, freespace_parking_response); + } else { + context_data_.emplace( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects, prev_decision_state, + isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity), + lane_parking_response, freespace_parking_response); + } auto & ctx_data = context_data_.value(); if (!isActivated()) { @@ -1259,7 +1271,7 @@ void GoalPlannerModule::setOutput( return; } - const auto & pull_over_path = context_data.pull_over_path_opt.value(); + const auto & pull_over_path = selected_pull_over_path_with_velocity_opt.value(); if ( parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { @@ -1460,7 +1472,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData // 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"); + RCLCPP_INFO( + getLogger(), "Update %lu pull over path candidates", + context_data.lane_parking_response.pull_over_path_candidates.size()); context_data.pull_over_path_opt = std::nullopt; context_data.last_path_update_time = std::nullopt; @@ -1496,6 +1510,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData context_data.pull_over_path_opt = pull_over_path; context_data.last_path_update_time = clock_->now(); context_data.last_path_idx_increment_time = std::nullopt; + if (pull_over_path_with_velocity_opt) { auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); // copy the path for later setOutput() @@ -1554,6 +1569,8 @@ void GoalPlannerModule::postProcess() path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; if (!context_data.pull_over_path_opt) { + // TODO(soblin): now context_data has part of thread_safe_data, so must not be nullfied because + // this function is called in every cycle context_data_ = std::nullopt; return; } @@ -1572,6 +1589,8 @@ void GoalPlannerModule::postProcess() setVelocityFactor(pull_over_path.full_path()); + // TODO(soblin): now context_data has part of thread_safe_data, so must not be nullfied because + // this function is called in every cycle context_data_ = std::nullopt; } From 4edb7a13859d5715cd1d6d710e4fd5cb2b8805b1 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 12 Dec 2024 17:12:12 +0900 Subject: [PATCH 4/9] seems working Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) 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 12ef1ed84f656..36cd5db939b45 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 @@ -760,6 +760,7 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); + context_data_ = std::nullopt; last_approval_data_.reset(); } @@ -1472,9 +1473,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData // 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_INFO( - getLogger(), "Update %lu pull over path candidates", - context_data.lane_parking_response.pull_over_path_candidates.size()); + RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); context_data.pull_over_path_opt = std::nullopt; context_data.last_path_update_time = std::nullopt; @@ -1569,9 +1568,6 @@ void GoalPlannerModule::postProcess() path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; if (!context_data.pull_over_path_opt) { - // TODO(soblin): now context_data has part of thread_safe_data, so must not be nullfied because - // this function is called in every cycle - context_data_ = std::nullopt; return; } const auto & pull_over_path = context_data.pull_over_path_opt.value(); @@ -1588,10 +1584,6 @@ void GoalPlannerModule::postProcess() has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); setVelocityFactor(pull_over_path.full_path()); - - // TODO(soblin): now context_data has part of thread_safe_data, so must not be nullfied because - // this function is called in every cycle - context_data_ = std::nullopt; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1852,13 +1844,12 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d // to prevent increment before driving // when the end of the current path is close to the current pose // this value should be `keep_stop_time` in keepStoppedWithCurrentPath - if (!ctx_data.last_path_idx_increment_time) { + if (!ctx_data.last_path_update_time) { return false; } constexpr double keep_current_idx_time = 4.0; const bool has_passed_enough_time_from_increment = - (clock_->now() - ctx_data.last_path_idx_increment_time.value()).seconds() > - keep_current_idx_time; + (clock_->now() - ctx_data.last_path_update_time.value()).seconds() > keep_current_idx_time; if (!has_passed_enough_time_from_increment) { return false; } From b4c7d795664503632bc56ef90106d50b5ff95e42 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 13 Dec 2024 15:47:14 +0900 Subject: [PATCH 5/9] fix Signed-off-by: Mamoru Sobue --- .../src/thread_data.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 index 6abfd714f4f89..9425435e35e65 100644 --- 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 @@ -14,6 +14,8 @@ #include +#include + namespace autoware::behavior_path_planner { From d42fc0d5ec451e2789e76d8ae5c2cbf52e52becb Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 13 Dec 2024 16:36:19 +0900 Subject: [PATCH 6/9] reflect comment Signed-off-by: Mamoru Sobue --- .../thread_data.hpp | 3 -- .../src/goal_planner_module.cpp | 52 ++++++++----------- .../src/thread_data.cpp | 18 ------- 3 files changed, 21 insertions(+), 52 deletions(-) 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 244c0be624f23..bfef4226b2661 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 @@ -64,7 +64,6 @@ class LaneParkingRequest } const std::optional & get_pull_over_path() const { return pull_over_path_; } const PathDecisionState & get_prev_data() const { return prev_data_; } - LaneParkingRequest clone() const; private: std::shared_ptr planner_data_; @@ -118,8 +117,6 @@ class FreespaceParkingRequest } bool is_stopped() const { return is_stopped_; } - FreespaceParkingRequest clone() const; - private: std::shared_ptr planner_data_; ModuleStatus current_status_; 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 36cd5db939b45..f01dffc8dc75c 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 @@ -91,13 +91,12 @@ 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, - [lane_parking_planner_bind = std::move(lane_parking_planner)]() { - lane_parking_planner_bind->onTimer(); + [lane_parking_executor = std::make_unique( + node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_, + is_lane_parking_cb_running_, getLogger(), *parameters_)]() { + lane_parking_executor->onTimer(); }, lane_parking_timer_cb_group_); @@ -107,13 +106,12 @@ GoalPlannerModule::GoalPlannerModule( 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, - [freespace_parking_planner_bind = std::move(freespace_parking_planner)]() { - freespace_parking_planner_bind->onTimer(); + [freespace_parking_executor = std::make_unique( + freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_, + is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner)]() { + freespace_parking_executor->onTimer(); }, freespace_parking_timer_cb_group_); } @@ -567,17 +565,15 @@ std::pair GoalPlannerModule::sync // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to - // gp_planner_data_ here + // lane_parking_request/freespace_parking_request std::optional pull_over_path = context_data_ ? context_data_.value().pull_over_path_opt : std::nullopt; std::optional last_path_update_time = context_data_ ? context_data_.value().last_path_update_time : std::nullopt; - // NOTE: 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 + // NOTE: Following clone process is rather lightweight because most of the member variables of + // PlannerData/RouteHandler is shared_ptrs // begin of critical section LaneParkingResponse lane_parking_response; { @@ -586,14 +582,14 @@ std::pair GoalPlannerModule::sync lane_parking_request_.emplace( *parameters_, vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); } - 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 - lane_parking_request.update( + // (reference count is incremented), so `lane_parking_request_local.planner_data_.foo` is now + // thread-safe from the **re-pointing** by `planner_data_->foo = msg` in + // behavior_path_planner::onCallbackFor(msg) and if these two coincided, only the reference + // count is affected + lane_parking_request_.value().update( *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path, path_decision_controller_.get_current_state()); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as @@ -603,10 +599,10 @@ std::pair GoalPlannerModule::sync // to copy route_handler as value to use lanelet_map_ptr_/routing_graph_ptr_ thread-safely in // onTimer/onFreespaceParkingTimer // TODO(Mamoru Sobue): If the copy of RouteHandler.road_lanelets/shoulder_lanelets is not - // lightweight, we should update gp_planner_data_.route_handler only when + // lightweight, we should update lane_parking_request.planner_data.route_handler only when // `planner_data_.is_route_handler_updated` variable is set true by behavior_path_planner - // (although this flag is not implemented yet). In that case, gp_planner_data members except - // for route_handler should be copied from planner_data_ + // (although this flag is not implemented yet). In that case, lane_parking_request members + // except for route_handler should be copied from planner_data_ lane_parking_response = lane_parking_response_; } @@ -617,19 +613,12 @@ std::pair GoalPlannerModule::sync 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( + freespace_parking_request_.value().update( *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time, isStopped( odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time, parameters_->th_stopped_velocity)); - // 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_ = freespace_parking_request.get_occupancy_grid_map(); freespace_parking_response = freespace_parking_response_; } // end of critical section @@ -670,6 +659,7 @@ void GoalPlannerModule::updateData() goal_candidates_ = generateGoalCandidates(); } + occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { 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 index 9425435e35e65..f12d510e23030 100644 --- 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 @@ -33,15 +33,6 @@ void LaneParkingRequest::update( 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_, pull_over_path_, prev_data_); - return request; -} - void FreespaceParkingRequest::initializeOccupancyGridMap( const PlannerData & planner_data, const GoalPlannerParameters & parameters) { @@ -72,13 +63,4 @@ void FreespaceParkingRequest::update( is_stopped_ = is_stopped; } -FreespaceParkingRequest FreespaceParkingRequest::clone() const -{ - FreespaceParkingRequest request( - parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); - request.update( - *planner_data_, current_status_, pull_over_path_, last_path_update_time_, is_stopped_); - return request; -} - } // namespace autoware::behavior_path_planner From bc1eb5fab366a7543e69acd1c2b953c87ad7db08 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 13 Dec 2024 17:16:14 +0900 Subject: [PATCH 7/9] remove ogm-based-collision-detecor from Freespace in the future Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 f01dffc8dc75c..5d7b4811b370f 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 @@ -619,6 +619,10 @@ std::pair GoalPlannerModule::sync isStopped( odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time, parameters_->th_stopped_velocity)); + // TODO(soblin): currently, ogm-based-collision-detector is updated to latest in + // freespace_parking_request_.value().update, and it is shared with goal_planner_module. Next, + // goal_planner_module update it and pass it to freespace_parking_request. + occupancy_grid_map_ = freespace_parking_request_.value().get_occupancy_grid_map(); freespace_parking_response = freespace_parking_response_; } // end of critical section @@ -659,7 +663,6 @@ void GoalPlannerModule::updateData() goal_candidates_ = generateGoalCandidates(); } - occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { From 181415e65cd71233df5065fec2defb4d7410118b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 13 Dec 2024 18:09:43 +0900 Subject: [PATCH 8/9] reflect comment2 Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 5d7b4811b370f..28a2f8cee48b0 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 @@ -1480,7 +1480,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData // Select a path that is as safe as possible and has a high priority. const auto & pull_over_path_candidates = context_data.lane_parking_response.pull_over_path_candidates; - auto lane_pull_over_path_opt = + const auto lane_pull_over_path_opt = selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ From 83e3f8f3bf3e833c08bd6b946f48ee9c03adc680 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sun, 15 Dec 2024 15:05:00 +0900 Subject: [PATCH 9/9] fix ci Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 8 +++----- .../src/pull_over_planner/geometric_pull_over.cpp | 5 +---- .../src/pull_over_planner/shift_pull_over.cpp | 2 +- 3 files changed, 5 insertions(+), 10 deletions(-) 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 28a2f8cee48b0..8e935c73c6ce9 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 @@ -498,7 +498,6 @@ void FreespaceParkingPlanner::onTimer() const auto & current_status = local_request.get_current_status(); const auto & pull_over_path_opt = local_request.get_pull_over_path(); const auto & last_path_update_time = local_request.get_last_path_update_time(); - const auto & occupancy_grid_map = local_request.get_occupancy_grid_map(); if (current_status == ModuleStatus::IDLE) { return; @@ -1690,11 +1689,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // 4. feasible stop const auto stop_pose_opt = std::invoke([&]() -> std::optional { if (context_data.pull_over_path_opt) - return context_data.pull_over_path_opt.value().start_pose(); + return std::make_optional(context_data.pull_over_path_opt.value().start_pose()); if (context_data.lane_parking_response.closest_start_pose) - return context_data.lane_parking_response.closest_start_pose.value(); - if (!decel_pose) return std::nullopt; - return decel_pose.value(); + return context_data.lane_parking_response.closest_start_pose; + return decel_pose; }); if (!stop_pose_opt.has_value()) { const auto feasible_stop_path = diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index f75358601d877..c0bac5c5ce2a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -78,9 +78,6 @@ std::optional GeometricPullOver::plan( auto pull_over_path_opt = PullOverPath::create( getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose, planner_.getPairsTerminalVelocityAndAccel()); - if (!pull_over_path_opt) { - return {}; - } - return pull_over_path_opt.value(); + return pull_over_path_opt; } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 501502125d7ee..edecfd733d3be 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -67,7 +67,7 @@ std::optional ShiftPullOver::plan( modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; - return *pull_over_path; + return pull_over_path; } return {};