From 705c496a12c132efb1dbb6b7bf23f765355ed767 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 12 Apr 2024 19:20:47 +0900 Subject: [PATCH 1/3] feat(start_planner): fix non-thread-safe access (#6779) Signed-off-by: Mamoru Sobue --- .../pull_out_planner_base.hpp | 2 +- .../start_planner_module.hpp | 23 ++- .../src/start_planner_module.cpp | 162 ++++++++++++++---- 3 files changed, 148 insertions(+), 39 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 0126ab47d1ae6..3089134a16500 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -51,7 +51,7 @@ class PullOutPlannerBase } virtual ~PullOutPlannerBase() = default; - void setPlannerData(std::shared_ptr & planner_data) + void setPlannerData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 927428a77c2b0..591d8adb60819 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -157,6 +157,24 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + struct StartPlannerData + { + StartPlannerParameters parameters; + PlannerData planner_data; + ModuleStatus current_status; + PullOutStatus main_thread_pull_out_status; + bool is_stopped; + + StartPlannerData clone() const; + void update( + const StartPlannerParameters & parameters, const PlannerData & planner_data, + const ModuleStatus & current_status, const PullOutStatus & pull_out_status, + const bool is_stopped); + }; + std::optional freespace_thread_status_{std::nullopt}; + std::optional start_planner_data_{std::nullopt}; + std::mutex start_planner_data_mutex_; + // Flag class for managing whether a certain callback is running in multi-threading class ScopedFlag { @@ -292,7 +310,6 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; bool isStopped(); - bool isStuck(); bool hasFinishedCurrentPath(); void updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, @@ -309,7 +326,9 @@ class StartPlannerModule : public SceneModuleInterface SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); - bool planFreespacePath(); + std::optional planFreespacePath( + const StartPlannerParameters & parameters, + const std::shared_ptr & planner_data, const PullOutStatus & pull_out_status); void setDebugData(); void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 11a72aef36ca4..ba93aa6c8bd4e 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -93,26 +93,56 @@ void StartPlannerModule::onFreespacePlannerTimer() { const ScopedFlag flag(is_freespace_planner_cb_running_); - if (getCurrentStatus() == ModuleStatus::IDLE) { + std::shared_ptr local_planner_data{nullptr}; + std::optional current_status_opt{std::nullopt}; + std::optional parameters_opt{std::nullopt}; + std::optional pull_out_status_opt{std::nullopt}; + bool is_stopped; + + // making a local copy of thread sensitive data + { + std::lock_guard guard(start_planner_data_mutex_); + if (start_planner_data_) { + const auto & start_planner_data = start_planner_data_.value(); + local_planner_data = std::make_shared(start_planner_data.planner_data); + current_status_opt = start_planner_data.current_status; + parameters_opt = start_planner_data.parameters; + pull_out_status_opt = start_planner_data.main_thread_pull_out_status; + is_stopped = start_planner_data.is_stopped; + } + } + // finish copying thread sensitive data + if (!local_planner_data || !current_status_opt || !parameters_opt || !pull_out_status_opt) { return; } - if (!planner_data_) { + const auto & current_status = current_status_opt.value(); + const auto & parameters = parameters_opt.value(); + const auto & pull_out_status = pull_out_status_opt.value(); + + if (current_status == ModuleStatus::IDLE) { return; } - if (!planner_data_->costmap) { + if (!local_planner_data->costmap) { return; } const bool is_new_costmap = - (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; if (!is_new_costmap) { return; } - if (isStuck()) { - planFreespacePath(); + const bool is_stuck = is_stopped && pull_out_status.planner_type == PlannerType::STOP && + !pull_out_status.found_pull_out_path; + if (is_stuck) { + const auto free_space_status = + planFreespacePath(parameters, local_planner_data, pull_out_status); + if (free_space_status) { + std::lock_guard guard(start_planner_data_mutex_); + freespace_thread_status_ = free_space_status; + } } } @@ -172,6 +202,38 @@ void StartPlannerModule::updateObjectsFilteringParams( void StartPlannerModule::updateData() { + // The method PlannerManager::run() calls SceneModuleInterface::setData and + // SceneModuleInterface::setPreviousModuleOutput() before this module's run() method is called + // with module_ptr->run(). Then module_ptr->run() invokes StartPlannerModule::updateData and, + // finally, the planWaitingApproval()/plan() methods are called by run(). So we can copy the + // latest current_status to start_planner_data_ here for later usage. + + // NOTE: onFreespacePlannerTimer copies start_planner_data to its thread local variable, so we + // need to lock start_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 + // making a local copy of thread sensitive data + { + std::lock_guard guard(start_planner_data_mutex_); + if (!start_planner_data_) { + start_planner_data_ = StartPlannerData(); + } + start_planner_data_.value().update( + *parameters_, *planner_data_, getCurrentStatus(), status_, isStopped()); + if (freespace_thread_status_) { + // if freespace solution is available, copy it to status_ on main thread + const auto & freespace_status = freespace_thread_status_.value(); + status_.pull_out_path = freespace_status.pull_out_path; + status_.pull_out_start_pose = freespace_status.pull_out_start_pose; + status_.planner_type = freespace_status.planner_type; + status_.found_pull_out_path = freespace_status.found_pull_out_path; + status_.driving_forward = freespace_status.driving_forward; + // and then reset it + freespace_thread_status_ = std::nullopt; + } + } + // finish copying thread sensitive data + if (receivedNewRoute()) { resetStatus(); DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); @@ -1156,19 +1218,6 @@ bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const return elapsed < parameters_->prepare_time_before_start; } -bool StartPlannerModule::isStuck() -{ - if (!isStopped()) { - return false; - } - - if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) { - return true; - } - - return false; -} - bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); @@ -1369,19 +1418,21 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() return output; } -bool StartPlannerModule::planFreespacePath() +std::optional StartPlannerModule::planFreespacePath( + const StartPlannerParameters & parameters, + const std::shared_ptr & planner_data, const PullOutStatus & pull_out_status) { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto & route_handler = planner_data_->route_handler; + const Pose & current_pose = planner_data->self_odometry->pose.pose; + const auto & route_handler = planner_data->route_handler; - const double end_pose_search_start_distance = parameters_->end_pose_search_start_distance; - const double end_pose_search_end_distance = parameters_->end_pose_search_end_distance; - const double end_pose_search_interval = parameters_->end_pose_search_interval; + const double end_pose_search_start_distance = parameters.end_pose_search_start_distance; + const double end_pose_search_end_distance = parameters.end_pose_search_end_distance; + const double end_pose_search_interval = parameters.end_pose_search_interval; const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + planner_data->parameters.backward_path_length + parameters.max_back_distance; const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); @@ -1394,23 +1445,23 @@ bool StartPlannerModule::planFreespacePath() for (const auto & p : center_line_path.points) { const Pose end_pose = p.point.pose; - freespace_planner_->setPlannerData(planner_data_); + freespace_planner_->setPlannerData(planner_data); auto freespace_path = freespace_planner_->plan(current_pose, end_pose); if (!freespace_path) { continue; } - const std::lock_guard lock(mutex_); - status_.pull_out_path = *freespace_path; - status_.pull_out_start_pose = current_pose; - status_.planner_type = freespace_planner_->getPlannerType(); - status_.found_pull_out_path = true; - status_.driving_forward = true; - return true; + auto status = pull_out_status; + status.pull_out_path = *freespace_path; + status.pull_out_start_pose = current_pose; + status.planner_type = freespace_planner_->getPlannerType(); + status.found_pull_out_path = true; + status.driving_forward = true; + return std::make_optional(status); } - return false; + return std::nullopt; } void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -1698,4 +1749,43 @@ void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const logFunc("======================================="); } + +StartPlannerModule::StartPlannerData StartPlannerModule::StartPlannerData::clone() const +{ + StartPlannerData start_planner_data; + start_planner_data.update( + parameters, planner_data, current_status, main_thread_pull_out_status, is_stopped); + return start_planner_data; +} + +void StartPlannerModule::StartPlannerData::update( + const StartPlannerParameters & parameters_, const PlannerData & planner_data_, + const ModuleStatus & current_status_, const PullOutStatus & pull_out_status_, + const bool is_stopped_) +{ + parameters = parameters_; + planner_data = planner_data_; + // TODO(Mamoru Sobue): in the future we will add planner_data->is_route_handler_updated flag to + // avoid the copy overhead of lanelet objects inside the RouteHandler. behavior_path_planner can + // tell us the flag if map/route changed, so we can skip route_handler update if it + // is false in the following way + /* + auto route_handler_self = planner_data.route_handler; + planner_data = planner_data_; // sync planer_data to planner_data_, planner_data.route_handler + is once re-pointed + + if (!planner_data_->is_route_handler_updated && route_handler_self != nullptr) { + // we do not need to sync planner_data.route_handler with that of planner_data_ + // re-point to the original again + planner_data.route_handler = route_handler_self; + } else { + // this is actually heavy if the lanelet_map is HUGE + planner_data.route_handler = std::make_shared(*(planner_data_.route_handler)); + } + */ + planner_data.route_handler = std::make_shared(*(planner_data_.route_handler)); + current_status = current_status_; + main_thread_pull_out_status = pull_out_status_; + is_stopped = is_stopped_; +} } // namespace behavior_path_planner From ad9ace802a311badf9a3b7d1c59ef51faa62bf65 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 8 Apr 2024 18:17:25 +0900 Subject: [PATCH 2/3] fix(behavior_path_planner): fix behavior_path_planner node to check occupancy_grid before running (#6752) Signed-off-by: Mamoru Sobue From e527bca43e68f926d0085c6d9457424582fdb612 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 10 May 2024 13:17:34 +0900 Subject: [PATCH 3/3] feat(goal_planner): fix non-thread-safe access in goal_planner (revert of #6809 and minor fix) (#6965) Signed-off-by: Mamoru Sobue --- .../geometric_pull_over.hpp | 6 +- .../goal_planner_module.hpp | 190 +++-- .../goal_searcher.hpp | 36 +- .../goal_searcher_base.hpp | 25 +- .../shift_pull_over.hpp | 6 +- .../src/geometric_pull_over.cpp | 5 +- .../src/goal_planner_module.cpp | 699 ++++++++++++------ .../src/goal_searcher.cpp | 108 +-- .../src/shift_pull_over.cpp | 4 +- 9 files changed, 715 insertions(+), 364 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index 6de1a726d4d4e..e67d458874d17 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include @@ -34,9 +33,7 @@ class GeometricPullOver : public PullOverPlannerBase public: GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, - const std::shared_ptr occupancy_grid_map, - const bool is_forward); + const LaneDepartureChecker & lane_departure_checker, const bool is_forward); PullOverPlannerType getPlannerType() const override { @@ -61,7 +58,6 @@ class GeometricPullOver : public PullOverPlannerBase protected: ParallelParkingParameters parallel_parking_parameters_; LaneDepartureChecker lane_departure_checker_{}; - std::shared_ptr occupancy_grid_map_; bool is_forward_{true}; bool left_side_parking_{true}; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 32a2e6ce7659c..8f456a57e4c78 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -91,6 +91,33 @@ public: \ DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) +enum class DecidingPathStatus { + NOT_DECIDED, + DECIDING, + DECIDED, +}; +using DecidingPathStatusWithStamp = std::pair; + +struct PreviousPullOverData +{ + struct SafetyStatus + { + std::optional safe_start_time{}; + bool is_safe{false}; + }; + + void reset() + { + found_path = false; + safety_status = SafetyStatus{}; + deciding_path_status = DecidingPathStatusWithStamp{}; + } + + bool found_path{false}; + SafetyStatus safety_status{}; + DecidingPathStatusWithStamp deciding_path_status{}; +}; + class ThreadSafeData { public: @@ -145,6 +172,9 @@ class ThreadSafeData void set(const std::shared_ptr & arg) { set_pull_over_path(arg); } void set(const PullOverPath & arg) { set_pull_over_path(arg); } void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); } + void set(const BehaviorModuleOutput & arg) { set_last_previous_module_output(arg); } + void set(const PreviousPullOverData & arg) { set_prev_data(arg); } + void set(const CollisionCheckDebugMap & arg) { set_collision_check(arg); } void clearPullOverPath() { @@ -182,6 +212,8 @@ class ThreadSafeData last_path_update_time_ = std::nullopt; last_path_idx_increment_time_ = std::nullopt; closest_start_pose_ = std::nullopt; + last_previous_module_output_ = std::nullopt; + prev_data_.reset(); } DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) @@ -193,6 +225,9 @@ class ThreadSafeData DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) + DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data) + DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check) private: std::shared_ptr pull_over_path_{nullptr}; @@ -203,6 +238,9 @@ class ThreadSafeData std::optional last_path_update_time_; std::optional last_path_idx_increment_time_; std::optional closest_start_pose_{}; + std::optional last_previous_module_output_{}; + PreviousPullOverData prev_data_{}; + CollisionCheckDebugMap collision_check_{}; std::recursive_mutex & mutex_; rclcpp::Clock::SharedPtr clock_; @@ -234,33 +272,6 @@ struct LastApprovalData Pose pose{}; }; -enum class DecidingPathStatus { - NOT_DECIDED, - DECIDING, - DECIDED, -}; -using DecidingPathStatusWithStamp = std::pair; - -struct PreviousPullOverData -{ - struct SafetyStatus - { - std::optional safe_start_time{}; - bool is_safe{false}; - }; - - void reset() - { - found_path = false; - safety_status = SafetyStatus{}; - deciding_path_status = DecidingPathStatusWithStamp{}; - } - - bool found_path{false}; - SafetyStatus safety_status{}; - DecidingPathStatusWithStamp deciding_path_status{}; -}; - // store stop_pose_ pointer with reason string struct PoseWithString { @@ -363,6 +374,50 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: + /** + * @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this) + */ + struct GoalPlannerData + { + GoalPlannerData(const PlannerData & planner_data, const GoalPlannerParameters & parameters) + { + initializeOccupancyGridMap(planner_data, parameters); + }; + GoalPlannerParameters parameters; + std::shared_ptr ego_predicted_path_params; + std::shared_ptr objects_filtering_params; + std::shared_ptr safety_check_params; + tier4_autoware_utils::LinearRing2d vehicle_footprint; + + PlannerData planner_data; + ModuleStatus current_status; + BehaviorModuleOutput previous_module_output; + // collision detector + // need to be shared_ptr to be used in planner and goal searcher + std::shared_ptr occupancy_grid_map; + std::shared_ptr goal_searcher; + + const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; } + const ModuleStatus & getCurrentStatus() const { return current_status; } + void updateOccupancyGrid(); + GoalPlannerData clone() const; + void update( + const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params_, + const std::shared_ptr & objects_filtering_params_, + const std::shared_ptr & safety_check_params_, + const PlannerData & planner_data, const ModuleStatus & current_status, + const BehaviorModuleOutput & previous_module_output, + const std::shared_ptr goal_searcher_, + const tier4_autoware_utils::LinearRing2d & vehicle_footprint); + + private: + void initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters); + }; + std::optional gp_planner_data_{std::nullopt}; + std::mutex gp_planner_data_mutex_; + // Flag class for managing whether a certain callback is running in multi-threading class ScopedFlag { @@ -420,9 +475,10 @@ class GoalPlannerModule : public SceneModuleInterface // goal searcher std::shared_ptr goal_searcher_; - // collision detector - // need to be shared_ptr to be used in planner and goal searcher - std::shared_ptr occupancy_grid_map_; + // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on + // onFreespaceParkingTimer thread storage may point to while calculation. + // onTimer/onFreespaceParkingTimer and their callees MUST NOT use this + std::shared_ptr occupancy_grid_map_{nullptr}; // check stopped and stuck state std::deque odometry_buffer_stopped_; @@ -431,10 +487,10 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::recursive_mutex mutex_; - ThreadSafeData thread_safe_data_; + // TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable + mutable ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; - PreviousPullOverData prev_data_{}; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -460,11 +516,12 @@ class GoalPlannerModule : public SceneModuleInterface mutable PoseWithString debug_stop_pose_with_info_; // collision check - void initializeOccupancyGridMap(); - void updateOccupancyGrid(); - bool checkOccupancyGridCollision(const PathWithLaneId & path) const; + bool checkOccupancyGridCollision( + const PathWithLaneId & path, + const std::shared_ptr occupancy_grid_map) const; bool checkObjectsCollision( - const PathWithLaneId & path, const double collision_check_margin, + const PathWithLaneId & path, const std::shared_ptr planner_data, + const GoalPlannerParameters & parameters, const double collision_check_margin, const bool extract_static_objects, const bool update_debug_data = false) const; // goal seach @@ -487,13 +544,39 @@ class GoalPlannerModule : public SceneModuleInterface bool isStopped( std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); - bool isOnModifiedGoal() const; + bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const; double calcModuleRequestLength() const; - bool needPathUpdate(const double path_update_duration) const; - bool isStuck(); - bool hasDecidedPath() const; - bool hasNotDecidedPath() const; - DecidingPathStatusWithStamp checkDecidingPathStatus() const; + bool needPathUpdate( + const Pose & current_pose, const double path_update_duration, + const GoalPlannerParameters & parameters) const; + bool isStuck( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters); + bool hasDecidedPath( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params, + const std::shared_ptr goal_searcher) const; + bool hasNotDecidedPath( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params, + const std::shared_ptr goal_searcher) const; + DecidingPathStatusWithStamp checkDecidingPathStatus( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params, + const std::shared_ptr goal_searcher) const; void decideVelocity(); bool foundPullOverPath() const; void updateStatus(const BehaviorModuleOutput & output); @@ -509,7 +592,10 @@ class GoalPlannerModule : public SceneModuleInterface bool hasEnoughTimePassedSincePathUpdate(const double duration) const; // freespace parking - bool planFreespacePath(); + bool planFreespacePath( + std::shared_ptr planner_data, + const std::shared_ptr goal_searcher, + const std::shared_ptr occupancy_grid_map); bool canReturnToLaneParking(); // plan pull over path @@ -538,10 +624,12 @@ class GoalPlannerModule : public SceneModuleInterface TurnSignalInfo calcTurnSignalInfo(); std::optional ignore_signal_{std::nullopt}; - std::optional last_previous_module_output_{}; - bool hasPreviousModulePathShapeChanged() const; - bool hasDeviatedFromLastPreviousModulePath() const; - bool hasDeviatedFromCurrentPreviousModulePath() const; + bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const; + bool hasDeviatedFromLastPreviousModulePath( + const std::shared_ptr planner_data) const; + bool hasDeviatedFromCurrentPreviousModulePath( + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) const; // timer for generating pull over path candidates in a separate thread void onTimer(); @@ -557,16 +645,22 @@ class GoalPlannerModule : public SceneModuleInterface // safety check void initializeSafetyCheckParameters(); SafetyCheckParams createSafetyCheckParams() const; + /* void updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, const std::vector & ego_predicted_path) const; + */ /** * @brief Checks if the current path is safe. * @return std::pair * first: If the path is safe for a certain period of time, true. * second: If the path is safe in the current state, true. */ - std::pair isSafePath() const; + std::pair isSafePath( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params) const; // debug void setDebugData(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index f228ecc378c28..89f4086874183 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -29,33 +29,45 @@ using BasicPolygons2d = std::vector; class GoalSearcher : public GoalSearcherBase { public: - GoalSearcher( - const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, - const std::shared_ptr & occupancy_grid_map); + GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint); - GoalCandidates search() override; - void update(GoalCandidates & goal_candidates) const override; + GoalCandidates search( + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) override; + void update( + GoalCandidates & goal_candidates, + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) const override; // todo(kosuke55): Functions for this specific use should not be in the interface, // so it is better to consider interface design when we implement other goal searchers. GoalCandidate getClosetGoalCandidateAlongLanes( - const GoalCandidates & goal_candidates) const override; + const GoalCandidates & goal_candidates, + const std::shared_ptr & planner_data) const override; bool isSafeGoalWithMarginScaleFactor( - const GoalCandidate & goal_candidate, const double margin_scale_factor) const override; + const GoalCandidate & goal_candidate, const double margin_scale_factor, + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) const override; private: void countObjectsToAvoid( - GoalCandidates & goal_candidates, const PredictedObjects & objects) const; - void createAreaPolygons(std::vector original_search_poses); - bool checkCollision(const Pose & pose, const PredictedObjects & objects) const; + GoalCandidates & goal_candidates, const PredictedObjects & objects, + const std::shared_ptr & planner_data) const; + void createAreaPolygons( + std::vector original_search_poses, + const std::shared_ptr & planner_data); + bool checkCollision( + const Pose & pose, const PredictedObjects & objects, + const std::shared_ptr occupancy_grid_map) const; bool checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & objects) const; + const Pose & ego_pose, const PredictedObjects & objects, + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) const; BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; LinearRing2d vehicle_footprint_{}; - std::shared_ptr occupancy_grid_map_{}; bool left_side_parking_{true}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp index 2ddacc0aac46d..f5d879358cd38 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -17,6 +17,7 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include @@ -49,23 +50,29 @@ class GoalSearcherBase { reference_goal_pose_ = reference_goal_pose; } + const Pose & getReferenceGoal() const { return reference_goal_pose_; } - void setPlannerData(const std::shared_ptr & planner_data) + MultiPolygon2d getAreaPolygons() const { return area_polygons_; } + virtual GoalCandidates search( + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) = 0; + virtual void update( + [[maybe_unused]] GoalCandidates & goal_candidates, + [[maybe_unused]] const std::shared_ptr occupancy_grid_map, + [[maybe_unused]] const std::shared_ptr & planner_data) const { - planner_data_ = planner_data; + return; } - - MultiPolygon2d getAreaPolygons() { return area_polygons_; } - virtual GoalCandidates search() = 0; - virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } virtual GoalCandidate getClosetGoalCandidateAlongLanes( - const GoalCandidates & goal_candidates) const = 0; + const GoalCandidates & goal_candidates, + const std::shared_ptr & planner_data) const = 0; virtual bool isSafeGoalWithMarginScaleFactor( - const GoalCandidate & goal_candidate, const double margin_scale_factor) const = 0; + const GoalCandidate & goal_candidate, const double margin_scale_factor, + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) const = 0; protected: GoalPlannerParameters parameters_{}; - std::shared_ptr planner_data_{nullptr}; Pose reference_goal_pose_{}; MultiPolygon2d area_polygons_{}; }; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index 892ef7d5dd303..d0b0aee83e20c 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include @@ -34,9 +33,7 @@ class ShiftPullOver : public PullOverPlannerBase public: ShiftPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, - const std::shared_ptr & occupancy_grid_map); - + const LaneDepartureChecker & lane_departure_checker); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan(const Pose & goal_pose) override; @@ -57,7 +54,6 @@ class ShiftPullOver : public PullOverPlannerBase const Pose & start_pose, const Pose & end_pose, const double resample_interval); LaneDepartureChecker lane_departure_checker_{}; - std::shared_ptr occupancy_grid_map_{}; bool left_side_parking_{true}; }; diff --git a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp index b7d1c240d032a..0779002690f8f 100644 --- a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -27,13 +27,10 @@ namespace behavior_path_planner { GeometricPullOver::GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, - const std::shared_ptr occupancy_grid_map, - const bool is_forward) + const LaneDepartureChecker & lane_departure_checker, const bool is_forward) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{lane_departure_checker}, - occupancy_grid_map_{occupancy_grid_map}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 538aa3609322c..99f8fcd1cc9f4 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -78,14 +78,14 @@ GoalPlannerModule::GoalPlannerModule( 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, occupancy_grid_map_)); + 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, occupancy_grid_map_, /*is_forward*/ true)); + 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, occupancy_grid_map_, /*is_forward*/ false)); + node, *parameters, lane_departure_checker, /*is_forward*/ false)); } } @@ -97,8 +97,7 @@ GoalPlannerModule::GoalPlannerModule( // currently there is only one goal_searcher_type const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info.createFootprint(); - goal_searcher_ = - std::make_shared(*parameters, vehicle_footprint_, occupancy_grid_map_); + goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_); // timer callback for generating lane parking candidate paths const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); @@ -123,51 +122,44 @@ GoalPlannerModule::GoalPlannerModule( // Initialize safety checker if (parameters_->safety_check_params.enable_safety_check) { initializeSafetyCheckParameters(); - utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); } - - prev_data_.reset(); -} - -// This function is needed for waiting for planner_data_ -void GoalPlannerModule::updateOccupancyGrid() -{ - if (!planner_data_->occupancy_grid) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "occupancy_grid is not ready"); - return; - } - occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } -bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const +bool GoalPlannerModule::hasPreviousModulePathShapeChanged( + const BehaviorModuleOutput & previous_module_output) const { - if (!last_previous_module_output_) { + const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); + if (!last_previous_module_output) { return true; } - const auto current_path = getPreviousModuleOutput().path; + const auto current_path = previous_module_output.path; // the terminal distance is far return calcDistance2d( - last_previous_module_output_->path.points.back().point, + last_previous_module_output->path.points.back().point, current_path.points.back().point) > 0.3; } -bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const +bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath( + const std::shared_ptr planner_data) const { - if (!last_previous_module_output_) { + const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); + if (!last_previous_module_output) { return true; } return std::abs(motion_utils::calcLateralOffset( - last_previous_module_output_->path.points, - planner_data_->self_odometry->pose.pose.position)) > 0.3; + last_previous_module_output->path.points, + planner_data->self_odometry->pose.pose.position)) > 0.3; } -bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const +bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath( + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) const { return std::abs(motion_utils::calcLateralOffset( - getPreviousModuleOutput().path.points, - planner_data_->self_odometry->pose.pose.position)) > 0.3; + previous_module_output.path.points, planner_data->self_odometry->pose.pose.position)) > + 0.3; } // generate pull over candidate paths @@ -175,7 +167,50 @@ void GoalPlannerModule::onTimer() { const ScopedFlag flag(is_lane_parking_cb_running_); - if (getCurrentStatus() == ModuleStatus::IDLE) { + std::shared_ptr local_planner_data{nullptr}; + std::optional current_status_opt{std::nullopt}; + std::optional previous_module_output_opt{std::nullopt}; + std::shared_ptr occupancy_grid_map{nullptr}; + std::optional parameters_opt{std::nullopt}; + std::shared_ptr ego_predicted_path_params{nullptr}; + std::shared_ptr objects_filtering_params{nullptr}; + std::shared_ptr safety_check_params{nullptr}; + std::shared_ptr goal_searcher{nullptr}; + + // begin of critical section + { + std::lock_guard guard(gp_planner_data_mutex_); + if (gp_planner_data_) { + const auto & gp_planner_data = gp_planner_data_.value(); + local_planner_data = std::make_shared(gp_planner_data.planner_data); + current_status_opt = gp_planner_data.current_status; + previous_module_output_opt = gp_planner_data.previous_module_output; + occupancy_grid_map = gp_planner_data.occupancy_grid_map; + parameters_opt = gp_planner_data.parameters; + ego_predicted_path_params = gp_planner_data.ego_predicted_path_params; + objects_filtering_params = gp_planner_data.objects_filtering_params; + safety_check_params = gp_planner_data.safety_check_params; + goal_searcher = gp_planner_data.goal_searcher; + } + } + // end of critical section + if ( + !local_planner_data || !current_status_opt || !previous_module_output_opt || + !occupancy_grid_map || !parameters_opt || !ego_predicted_path_params || + !objects_filtering_params || !safety_check_params || !goal_searcher) { + RCLCPP_ERROR( + getLogger(), + "failed to get valid " + "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt/" + "ego_predicted_path_params/objects_filtering_params/safety_check_params/goal_searcher in " + "onTimer"); + return; + } + const auto & current_status = current_status_opt.value(); + const auto & previous_module_output = previous_module_output_opt.value(); + const auto & parameters = parameters_opt.value(); + + if (current_status == ModuleStatus::IDLE) { return; } @@ -184,27 +219,31 @@ void GoalPlannerModule::onTimer() return; } - if (!planner_data_ || !utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(local_planner_data->route_handler)) { return; } // check if new pull over path candidates are needed to be generated const bool need_update = std::invoke([&]() { - if (isOnModifiedGoal()) { + if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath()) { + if (hasDeviatedFromCurrentPreviousModulePath(local_planner_data, previous_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return true; } - if (hasPreviousModulePathShapeChanged()) { + if (hasPreviousModulePathShapeChanged(previous_module_output)) { RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } - if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { + if ( + hasDeviatedFromLastPreviousModulePath(local_planner_data) && + !hasDecidedPath( + local_planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, + objects_filtering_params, safety_check_params, goal_searcher)) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } @@ -214,13 +253,12 @@ void GoalPlannerModule::onTimer() return; } - const auto previous_module_output = getPreviousModuleOutput(); const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + local_planner_data, parameters.backward_goal_search_length, + parameters.forward_goal_search_length, /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; @@ -228,7 +266,7 @@ void GoalPlannerModule::onTimer() const auto planCandidatePaths = [&]( const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { - planner->setPlannerData(planner_data_); + planner->setPlannerData(local_planner_data); planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); if (pull_over_path) { @@ -254,7 +292,7 @@ void GoalPlannerModule::onTimer() is_center_line_input_path); // plan candidate paths and set them to the member variable - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { @@ -264,7 +302,7 @@ void GoalPlannerModule::onTimer() planCandidatePaths(planner, goal_candidate); } } - } else if (parameters_->path_priority == "close_goal") { + } else if (parameters.path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line @@ -277,49 +315,76 @@ void GoalPlannerModule::onTimer() } else { RCLCPP_ERROR( getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", - parameters_->path_priority.c_str()); + parameters.path_priority.c_str()); throw std::domain_error("[pull_over] invalid path_priority"); } // set member variables - { - const std::lock_guard lock(mutex_); - thread_safe_data_.set_pull_over_path_candidates(path_candidates); - thread_safe_data_.set_closest_start_pose(closest_start_pose); - RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); - } + thread_safe_data_.set_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()); - last_previous_module_output_ = previous_module_output; + thread_safe_data_.set_last_previous_module_output(previous_module_output); } void GoalPlannerModule::onFreespaceParkingTimer() { const ScopedFlag flag(is_freespace_parking_cb_running_); - if (getCurrentStatus() == ModuleStatus::IDLE) { + std::shared_ptr local_planner_data{nullptr}; + std::optional current_status_opt{std::nullopt}; + std::shared_ptr occupancy_grid_map{nullptr}; + std::optional parameters_opt{std::nullopt}; + std::shared_ptr goal_searcher{nullptr}; + + // begin of critical section + { + std::lock_guard guard(gp_planner_data_mutex_); + if (gp_planner_data_) { + const auto & gp_planner_data = gp_planner_data_.value(); + local_planner_data = std::make_shared(gp_planner_data.planner_data); + current_status_opt = gp_planner_data.current_status; + occupancy_grid_map = gp_planner_data.occupancy_grid_map; + parameters_opt = gp_planner_data.parameters; + goal_searcher = gp_planner_data.goal_searcher; + } + } + // end of critical section + if (!local_planner_data || !current_status_opt || !parameters_opt || !goal_searcher) { + RCLCPP_ERROR( + getLogger(), + "failed to get valid planner_data/current_status/parameters/goal_searcher in " + "onFreespaceParkingTimer"); return; } - if (!planner_data_) { + const auto & current_status = current_status_opt.value(); + const auto & parameters = parameters_opt.value(); + + if (current_status == ModuleStatus::IDLE) { return; } - if (!planner_data_->costmap) { + + if (!local_planner_data->costmap) { return; } // fixed goal planner do not use freespace planner - if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(local_planner_data->route_handler)) { return; } - if (isOnModifiedGoal()) { + if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { return; } const bool is_new_costmap = - (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; - if (isStuck() && is_new_costmap && needPathUpdate(path_update_duration)) { - planFreespacePath(); + if ( + isStuck(local_planner_data, occupancy_grid_map, parameters) && is_new_costmap && + needPathUpdate( + local_planner_data->self_odometry->pose.pose, path_update_duration, parameters)) { + planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); } } @@ -349,28 +414,64 @@ void GoalPlannerModule::updateObjectsFilteringParams( void GoalPlannerModule::updateData() { + // 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 + + // 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 + { + std::lock_guard guard(gp_planner_data_mutex_); + if (!gp_planner_data_) { + gp_planner_data_ = GoalPlannerData(*planner_data_, *parameters_); + } + auto & gp_planner_data = gp_planner_data_.value(); + // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that + // planner_data_ is not nullptr, so it is OK to copy as value + // By copying PlannerData as value, the internal shared member variables are also copied + // (reference count is incremented), so `gp_planner_data_.foo` is now thread-safe from the + // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) + // and if these two coincided, only the reference count is affected + gp_planner_data.update( + *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_, + *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), goal_searcher_, + vehicle_footprint_); + // 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 + // planner_data_->route_handler->lanelet_map_ptr_/routing_graph_ptr_ especially, we also have + // 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 + // `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_ + + // 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; + } + // end of critical section + if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { return; } - // Initialize Occupancy Grid Map - // This operation requires waiting for `planner_data_`, hence it is executed here instead of in - // the constructor. Ideally, this operation should only need to be performed once. - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - initializeOccupancyGridMap(); - } - resetPathCandidate(); resetPathReference(); path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - updateOccupancyGrid(); - // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { - goal_searcher_->setPlannerData(planner_data_); goal_searcher_->setReferenceGoal( calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose())); thread_safe_data_.set_goal_candidates(generateGoalCandidates()); @@ -391,21 +492,6 @@ void GoalPlannerModule::updateData() } } -void GoalPlannerModule::initializeOccupancyGridMap() -{ - 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_->setParam(occupancy_grid_map_param); -} - void GoalPlannerModule::initializeSafetyCheckParameters() { updateEgoPredictedPathParams(ego_predicted_path_params_, parameters_); @@ -418,7 +504,7 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - prev_data_.reset(); + thread_safe_data_.reset(); last_approval_data_.reset(); } @@ -501,7 +587,10 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { - if (!isSafePath().first) { + if (!isSafePath( + planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, + safety_check_params_) + .first) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; } @@ -584,18 +673,17 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -bool GoalPlannerModule::planFreespacePath() +bool GoalPlannerModule::planFreespacePath( + std::shared_ptr planner_data, + const std::shared_ptr goal_searcher, + const std::shared_ptr occupancy_grid_map) { GoalCandidates goal_candidates{}; - { - const std::lock_guard lock(mutex_); - goal_searcher_->setPlannerData(planner_data_); - goal_candidates = thread_safe_data_.get_goal_candidates(); - goal_searcher_->update(goal_candidates); - thread_safe_data_.set_goal_candidates(goal_candidates); - debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); - debug_data_.freespace_planner.is_planning = true; - } + goal_candidates = thread_safe_data_.get_goal_candidates(); + goal_searcher->update(goal_candidates, occupancy_grid_map, planner_data); + thread_safe_data_.set_goal_candidates(goal_candidates); + debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); + debug_data_.freespace_planner.is_planning = true; for (size_t i = 0; i < goal_candidates.size(); i++) { const auto goal_candidate = goal_candidates.at(i); @@ -607,7 +695,7 @@ bool GoalPlannerModule::planFreespacePath() if (!goal_candidate.is_safe) { continue; } - freespace_planner_->setPlannerData(planner_data_); + freespace_planner_->setPlannerData(planner_data); auto freespace_path = freespace_planner_->plan(goal_candidate.goal_pose); freespace_path->goal_id = goal_candidate.id; if (!freespace_path) { @@ -645,13 +733,15 @@ bool GoalPlannerModule::canReturnToLaneParking() if ( parameters_->use_object_recognition && checkObjectsCollision( - path, parameters_->object_recognition_collision_check_hard_margins.back(), + path, planner_data_, *parameters_, + parameters_->object_recognition_collision_check_hard_margins.back(), /*extract_static_objects=*/false)) { return false; } if ( - parameters_->use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(path)) { + parameters_->use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(path, occupancy_grid_map_)) { return false; } @@ -671,7 +761,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const // calculate goal candidates const auto & route_handler = planner_data_->route_handler; if (utils::isAllowedGoalModification(route_handler)) { - return goal_searcher_->search(); + return goal_searcher_->search(occupancy_grid_map_, planner_data_); } // NOTE: @@ -778,7 +868,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); for (const auto & margin : margins) { if (!checkObjectsCollision( - resampled_path, margin, + resampled_path, planner_data_, *parameters_, margin, /*extract_static_objects=*/true)) { return margin; } @@ -885,16 +975,17 @@ std::optional> GoalPlannerModule::selectP const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); if ( - parameters_->use_object_recognition && checkObjectsCollision( - resampled_path, collision_check_margin, - /*extract_static_objects=*/true, - /*update_debug_data=*/true)) { + parameters_->use_object_recognition && + checkObjectsCollision( + resampled_path, planner_data_, *parameters_, collision_check_margin, + /*extract_static_objects=*/true, + /*update_debug_data=*/true)) { continue; } if ( parameters_->use_occupancy_grid_for_path_collision_check && - checkOccupancyGridCollision(resampled_path)) { + checkOccupancyGridCollision(resampled_path, occupancy_grid_map_)) { continue; } @@ -930,7 +1021,12 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) } if ( - parameters_->safety_check_params.enable_safety_check && !isSafePath().first && isActivated()) { + parameters_->safety_check_params.enable_safety_check && + !isSafePath( + planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, + safety_check_params_) + .first && + isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -953,7 +1049,11 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) setDrivableAreaInfo(output); // set hazard and turn signal - if (hasDecidedPath() && isActivated()) { + if ( + hasDecidedPath( + planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_, goal_searcher_) && + isActivated()) { setTurnSignalInfo(output); } } @@ -1017,20 +1117,47 @@ void GoalPlannerModule::updateSteeringFactor( pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, ""); } -bool GoalPlannerModule::hasDecidedPath() const -{ - return checkDecidingPathStatus().first == DecidingPathStatus::DECIDED; -} - -bool GoalPlannerModule::hasNotDecidedPath() const -{ - return checkDecidingPathStatus().first == DecidingPathStatus::NOT_DECIDED; -} - -DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const -{ - const auto & prev_status = prev_data_.deciding_path_status; - const bool enable_safety_check = parameters_->safety_check_params.enable_safety_check; +bool GoalPlannerModule::hasDecidedPath( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params, + const std::shared_ptr goal_searcher) const +{ + return checkDecidingPathStatus( + planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, + objects_filtering_params, safety_check_params, goal_searcher) + .first == DecidingPathStatus::DECIDED; +} + +bool GoalPlannerModule::hasNotDecidedPath( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params, + const std::shared_ptr goal_searcher) const +{ + return checkDecidingPathStatus( + planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, + objects_filtering_params, safety_check_params, goal_searcher) + .first == DecidingPathStatus::NOT_DECIDED; +} + +DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params, + const std::shared_ptr goal_searcher) const +{ + const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status; + const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; // Once this function returns true, it will continue to return true thereafter if (prev_status.first == DecidingPathStatus::DECIDED) { @@ -1044,7 +1171,13 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if (enable_safety_check && !isSafePath().first && !isActivated()) { + if ( + enable_safety_check && + !isSafePath( + planner_data, parameters, ego_predicted_path_params, objects_filtering_params, + safety_check_params) + .first && + !isActivated()) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -1060,11 +1193,11 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const const double hysteresis_factor = 0.9; // check goal pose collision - goal_searcher_->setPlannerData(planner_data_); const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose(); if ( - modified_goal_opt && !goal_searcher_->isSafeGoalWithMarginScaleFactor( - modified_goal_opt.value(), hysteresis_factor)) { + modified_goal_opt && + !goal_searcher->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data)) { RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } @@ -1072,15 +1205,20 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const // check current parking path collision const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5); const double margin = - parameters_->object_recognition_collision_check_hard_margins.back() * hysteresis_factor; - if (checkObjectsCollision(parking_path, margin, /*extract_static_objects=*/false)) { + parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; + if (checkObjectsCollision( + parking_path, planner_data, parameters, margin, /*extract_static_objects=*/false)) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } - if (enable_safety_check && !isSafePath().first) { + if ( + enable_safety_check && !isSafePath( + planner_data, parameters, ego_predicted_path_params, + objects_filtering_params, safety_check_params) + .first) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); @@ -1104,7 +1242,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const } // if ego is sufficiently close to the start of the nearest candidate path, the path is decided - const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_pose = planner_data->self_odometry->pose.pose; const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); @@ -1113,13 +1251,13 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const const double dist_to_parking_start_pose = calcSignedArcLength( current_path.points, current_pose.position, ego_segment_idx, pull_over_path->start_pose.position, start_pose_segment_idx); - if (dist_to_parking_start_pose > parameters_->decide_path_distance) { + if (dist_to_parking_start_pose > parameters.decide_path_distance) { return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } // if object recognition for path collision check is enabled, transition to DECIDING to check // collision for a certain period of time. Otherwise, transition to DECIDED directly. - if (parameters_->use_object_recognition) { + if (parameters.use_object_recognition) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " @@ -1144,7 +1282,9 @@ void GoalPlannerModule::decideVelocity() BehaviorModuleOutput GoalPlannerModule::planPullOver() { - if (!hasDecidedPath()) { + if (!hasDecidedPath( + planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_, goal_searcher_)) { return planPullOverAsCandidate(); } @@ -1188,7 +1328,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() return getPreviousModuleOutput(); } - if (hasNotDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { + if ( + hasNotDecidedPath( + planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_, goal_searcher_) && + needPathUpdate( + planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *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_ @@ -1197,9 +1342,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() thread_safe_data_.clearPullOverPath(); // update goal candidates - goal_searcher_->setPlannerData(planner_data_); auto goal_candidates = thread_safe_data_.get_goal_candidates(); - goal_searcher_->update(goal_candidates); + goal_searcher_->update(goal_candidates, occupancy_grid_map_, planner_data_); // update pull over path candidates const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority( @@ -1260,9 +1404,14 @@ void GoalPlannerModule::postProcess() return; } - const bool has_decided_path = hasDecidedPath(); const auto distance_to_path_change = calcDistanceToPathChange(); + // TODO(Mamoru Sobue): repetitive call to checkDecidingPathStatus() in the main thread is a + // waste of time because it gives same result throughout the main thread. + const bool has_decided_path = hasDecidedPath( + planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_, goal_searcher_); + if (has_decided_path) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } @@ -1280,27 +1429,35 @@ void GoalPlannerModule::updatePreviousData() { // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - prev_data_.found_path = thread_safe_data_.foundPullOverPath(); + // TODO(Mamoru Sobue): put prev_data_ out of ThreadSafeData + auto prev_data = thread_safe_data_.get_prev_data(); + prev_data.found_path = thread_safe_data_.foundPullOverPath(); - prev_data_.deciding_path_status = checkDecidingPathStatus(); + prev_data.deciding_path_status = checkDecidingPathStatus( + planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_, goal_searcher_); // This is related to safety check, so if it is disabled, return here. if (!parameters_->safety_check_params.enable_safety_check) { - prev_data_.safety_status.is_safe = true; - return; - } - - // Even if the current path is safe, it will not be safe unless it stands for a certain period of - // time. Record the time when the current path has become safe - const auto [is_safe, current_is_safe] = isSafePath(); - if (current_is_safe) { - if (!prev_data_.safety_status.safe_start_time) { - prev_data_.safety_status.safe_start_time = clock_->now(); - } + prev_data.safety_status.is_safe = true; } else { - prev_data_.safety_status.safe_start_time = std::nullopt; + // Even if the current path is safe, it will not be safe unless it stands for a certain period + // of time. Record the time when the current path has become safe + const auto [is_safe, current_is_safe] = isSafePath( + planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, + safety_check_params_); + if (current_is_safe) { + if (!prev_data.safety_status.safe_start_time) { + prev_data.safety_status.safe_start_time = clock_->now(); + } + } else { + prev_data.safety_status.safe_start_time = std::nullopt; + } + prev_data.safety_status.is_safe = is_safe; } - prev_data_.safety_status.is_safe = is_safe; + + // commit the change + thread_safe_data_.set_prev_data(prev_data); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1377,8 +1534,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible // stop point is found, stop at this position. - const auto closest_goal_candidate = - goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); + const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( + thread_safe_data_.get_goal_candidates(), planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); @@ -1506,10 +1663,13 @@ bool GoalPlannerModule::isStopped() return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } -bool GoalPlannerModule::isStuck() +bool GoalPlannerModule::isStuck( + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const GoalPlannerParameters & parameters) { const std::lock_guard lock(mutex_); - if (isOnModifiedGoal()) { + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, parameters)) { return false; } @@ -1529,17 +1689,18 @@ bool GoalPlannerModule::isStuck() } if ( - parameters_->use_object_recognition && + parameters.use_object_recognition && checkObjectsCollision( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), + thread_safe_data_.get_pull_over_path()->getCurrentPath(), planner_data, parameters, /*extract_static_objects=*/false, - parameters_->object_recognition_collision_check_hard_margins.back())) { + parameters.object_recognition_collision_check_hard_margins.back())) { return true; } if ( - parameters_->use_occupancy_grid_for_path_collision_check && - checkOccupancyGridCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath())) { + parameters.use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision( + thread_safe_data_.get_pull_over_path()->getCurrentPath(), occupancy_grid_map)) { return true; } @@ -1585,15 +1746,15 @@ bool GoalPlannerModule::hasFinishedCurrentPath() parameters_->th_arrived_distance; } -bool GoalPlannerModule::isOnModifiedGoal() const +bool GoalPlannerModule::isOnModifiedGoal( + const Pose & current_pose, const GoalPlannerParameters & parameters) const { if (!thread_safe_data_.get_modified_goal_pose()) { return false; } - const Pose current_pose = planner_data_->self_odometry->pose.pose; return calcDistance2d(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose) < - parameters_->th_arrived_distance; + parameters.th_arrived_distance; } TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() @@ -1690,31 +1851,34 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() // return turn_signal; } -bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const +bool GoalPlannerModule::checkOccupancyGridCollision( + const PathWithLaneId & path, + const std::shared_ptr occupancy_grid_map) const { - if (!occupancy_grid_map_) { + if (!occupancy_grid_map) { return false; } const bool check_out_of_range = false; - return occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range); + return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); } bool GoalPlannerModule::checkObjectsCollision( - const PathWithLaneId & path, const double collision_check_margin, + const PathWithLaneId & path, const std::shared_ptr planner_data, + const GoalPlannerParameters & parameters, const double collision_check_margin, const bool extract_static_objects, const bool update_debug_data) const { const auto target_objects = std::invoke([&]() { - const auto & p = parameters_; - const auto & rh = *(planner_data_->route_handler); - const auto objects = *(planner_data_->dynamic_object); + const auto & p = parameters; + const auto & rh = *(planner_data->route_handler); + const auto objects = *(planner_data->dynamic_object); if (extract_static_objects) { return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( - rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, - p->detection_bound_offset, objects, p->th_moving_object_velocity); + rh, left_side_parking_, p.backward_goal_search_length, p.forward_goal_search_length, + p.detection_bound_offset, objects, p.th_moving_object_velocity); } return goal_planner_utils::extractObjectsInExpandedPullOverLanes( - rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, - p->detection_bound_offset, objects); + rh, left_side_parking_, p.backward_goal_search_length, p.forward_goal_search_length, + p.detection_bound_offset, objects); }); std::vector obj_polygons; @@ -1733,8 +1897,8 @@ bool GoalPlannerModule::checkObjectsCollision( const auto p = path.points.at(i); const double extra_stopping_margin = std::min( - std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, - parameters_->object_recognition_collision_check_max_extra_stopping_margin); + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin); // The square is meant to imply centrifugal force, but it is not a very well-founded formula. // TODO(kosuke55): It is needed to consider better way because there is an inherently different @@ -1745,9 +1909,9 @@ bool GoalPlannerModule::checkObjectsCollision( const auto ego_polygon = tier4_autoware_utils::toFootprint( p.point.pose, - planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, - planner_data_->parameters.base_link2rear + collision_check_margin, - planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + + planner_data->parameters.base_link2front + collision_check_margin + extra_stopping_margin, + planner_data->parameters.base_link2rear + collision_check_margin, + planner_data->parameters.vehicle_width + collision_check_margin * 2.0 + extra_lateral_margin * 2.0); ego_polygons_expanded.push_back(ego_polygon); } @@ -1831,8 +1995,8 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { // decelerate before the search area start - const auto closest_goal_candidate = - goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); + const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( + thread_safe_data_.get_goal_candidates(), planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); @@ -2013,6 +2177,7 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } +/* void GoalPlannerModule::updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, const std::vector & ego_predicted_path) const @@ -2021,6 +2186,7 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( goal_planner_data_.target_objects_on_lane = target_objects_on_lane; goal_planner_data_.ego_predicted_path = ego_predicted_path; } +*/ static std::vector filterObjectsByWithinPolicy( const std::shared_ptr & objects, @@ -2066,26 +2232,29 @@ static std::vector filterOb return refined_filtered_objects; } -std::pair GoalPlannerModule::isSafePath() const +std::pair GoalPlannerModule::isSafePath( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & objects_filtering_params, + const std::shared_ptr & safety_check_params) const { if (!thread_safe_data_.get_pull_over_path()) { return {false, false}; } const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_pose = planner_data->self_odometry->pose.pose; const double current_velocity = std::hypot( - planner_data_->self_odometry->twist.twist.linear.x, - planner_data_->self_odometry->twist.twist.linear.y); - const auto & dynamic_object = planner_data_->dynamic_object; - const auto & route_handler = planner_data_->route_handler; + planner_data->self_odometry->twist.twist.linear.x, + planner_data->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data->dynamic_object; + const auto & route_handler = planner_data->route_handler; const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + planner_data, parameters.backward_goal_search_length, parameters.forward_goal_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *route_handler, left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + *route_handler, left_side_parking_, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = utils::parking_departure::getPairsTerminalVelocityAndAccel( thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, @@ -2093,15 +2262,15 @@ std::pair GoalPlannerModule::isSafePath() const RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - utils::parking_departure::updatePathProperty( - ego_predicted_path_params_, terminal_velocity_and_accel); + auto temp_param = std::make_shared(*ego_predicted_path_params); + utils::parking_departure::updatePathProperty(temp_param, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; const bool limit_to_max_velocity = true; const auto ego_predicted_path = behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, - ego_seg_idx, is_object_front, limit_to_max_velocity); + ego_predicted_path_params, pull_over_path.points, current_pose, current_velocity, ego_seg_idx, + is_object_front, limit_to_max_velocity); // ========================================================================================== // if ego is before the entry of pull_over_lanes, the beginning of the safety check area @@ -2143,36 +2312,38 @@ std::pair GoalPlannerModule::isSafePath() const const auto expanded_pull_over_lanes_between_ego = goal_planner_utils::generateBetweenEgoAndExpandedPullOverLanes( pull_over_lanes, left_side_parking_, ego_pose_for_expand, - planner_data_->parameters.vehicle_info, parameters_->outer_road_detection_offset, - parameters_->inner_road_detection_offset); + planner_data->parameters.vehicle_info, parameters.outer_road_detection_offset, + parameters.inner_road_detection_offset); const auto merged_expanded_pull_over_lanes = lanelet::utils::combineLaneletsShape(expanded_pull_over_lanes_between_ego); debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes; const auto filtered_objects = filterObjectsByWithinPolicy( - dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params_); + dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params); + const auto prev_data = thread_safe_data_.get_prev_data(); const double hysteresis_factor = - prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; + prev_data.safety_status.is_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; + CollisionCheckDebugMap collision_check{}; const bool current_is_safe = std::invoke([&]() { - if (parameters_->safety_check_params.method == "RSS") { + if (parameters.safety_check_params.method == "RSS") { return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( - pull_over_path, ego_predicted_path, filtered_objects, goal_planner_data_.collision_check, - planner_data_->parameters, safety_check_params_->rss_params, - objects_filtering_params_->use_all_predicted_path, hysteresis_factor); - } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") { + pull_over_path, ego_predicted_path, filtered_objects, collision_check, + planner_data->parameters, safety_check_params->rss_params, + objects_filtering_params->use_all_predicted_path, hysteresis_factor); + } else if (parameters.safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( ego_predicted_path, vehicle_info_, filtered_objects, - objects_filtering_params_->check_all_predicted_path, - parameters_->safety_check_params.integral_predicted_polygon_params, - goal_planner_data_.collision_check); + objects_filtering_params->check_all_predicted_path, + parameters.safety_check_params.integral_predicted_polygon_params, collision_check); } RCLCPP_ERROR( getLogger(), " [pull_over] invalid safety check method: %s", - parameters_->safety_check_params.method.c_str()); + parameters.safety_check_params.method.c_str()); throw std::domain_error("[pull_over] invalid safety check method"); }); + thread_safe_data_.set_collision_check(collision_check); /* * ==== is_safe @@ -2189,9 +2360,9 @@ std::pair GoalPlannerModule::isSafePath() const */ if (current_is_safe) { if ( - prev_data_.safety_status.safe_start_time && - (clock_->now() - prev_data_.safety_status.safe_start_time.value()).seconds() > - parameters_->safety_check_params.keep_unsafe_time) { + prev_data.safety_status.safe_start_time && + (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds() > + parameters.safety_check_params.keep_unsafe_time) { return {true /*is_safe*/, true /*current_is_safe*/}; } } @@ -2225,8 +2396,12 @@ void GoalPlannerModule::setDebugData() }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const auto color = + hasDecidedPath( + planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_, goal_searcher_) + ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); @@ -2239,9 +2414,11 @@ void GoalPlannerModule::setDebugData() // Visualize previous module output add(createPathMarkerArray( getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); - if (last_previous_module_output_.has_value()) { + + const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); + if (last_previous_module_output.has_value()) { add(createPathMarkerArray( - last_previous_module_output_.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); + last_previous_module_output.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); } // Visualize path and related pose @@ -2298,6 +2475,7 @@ void GoalPlannerModule::setDebugData() } } + auto collision_check = thread_safe_data_.get_collision_check(); // safety check if (parameters_->safety_check_params.enable_safety_check) { if (goal_planner_data_.ego_predicted_path.size() > 0) { @@ -2312,33 +2490,35 @@ void GoalPlannerModule::setDebugData() } if (parameters_->safety_check_params.method == "RSS") { - add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info")); + add(showSafetyCheckInfo(collision_check, "object_debug_info")); } - add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); - add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); + add(showPredictedPath(collision_check, "ego_predicted_path")); + add(showPolygon(collision_check, "ego_and_target_polygon_relation")); // set objects of interest - for (const auto & [uuid, data] : goal_planner_data_.collision_check) { + for (const auto & [uuid, data] : collision_check) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); + // TODO(Mamoru Sobue): it is not clear where ThreadSafeData::collision_check should be cleared + utils::parking_departure::initializeCollisionCheckDebugMap(collision_check); // visualize safety status maker { + const auto prev_data = thread_safe_data_.get_prev_data(); visualization_msgs::msg::MarkerArray marker_array{}; - const auto color = prev_data_.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = prev_data.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "safety_status", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = planner_data_->self_odometry->pose.pose; - marker.text += "is_safe: " + std::to_string(prev_data_.safety_status.is_safe) + "\n"; - if (prev_data_.safety_status.safe_start_time) { + marker.text += "is_safe: " + std::to_string(prev_data.safety_status.is_safe) + "\n"; + if (prev_data.safety_status.safe_start_time) { const double elapsed_time_from_safe_start = - (clock_->now() - prev_data_.safety_status.safe_start_time.value()).seconds(); + (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds(); marker.text += "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; } @@ -2366,7 +2546,7 @@ void GoalPlannerModule::setDebugData() std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); } - if (isStuck()) { + if (isStuck(planner_data_, occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; } else if (isStopped()) { marker.text += " stopped"; @@ -2416,9 +2596,12 @@ void GoalPlannerModule::printParkingPositionError() const distance_from_real_shoulder); } -bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const +bool GoalPlannerModule::needPathUpdate( + const Pose & current_pose, const double path_update_duration, + const GoalPlannerParameters & parameters) const { - return !isOnModifiedGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); + return !isOnModifiedGoal(current_pose, parameters) && + hasEnoughTimePassedSincePathUpdate(path_update_duration); } bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const @@ -2429,4 +2612,58 @@ bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration return (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > duration; } + +void GoalPlannerModule::GoalPlannerData::initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters) +{ + OccupancyGridMapParam occupancy_grid_map_param{}; + const double margin = parameters.occupancy_grid_collision_check_margin; + occupancy_grid_map_param.vehicle_shape.length = + planner_data.parameters.vehicle_length + 2 * margin; + occupancy_grid_map_param.vehicle_shape.width = planner_data.parameters.vehicle_width + 2 * margin; + occupancy_grid_map_param.vehicle_shape.base2back = + planner_data.parameters.base_link2rear + margin; + occupancy_grid_map_param.theta_size = parameters.theta_size; + occupancy_grid_map_param.obstacle_threshold = parameters.obstacle_threshold; + occupancy_grid_map = std::make_shared(); + occupancy_grid_map->setParam(occupancy_grid_map_param); +} + +GoalPlannerModule::GoalPlannerData GoalPlannerModule::GoalPlannerData::clone() const +{ + GoalPlannerModule::GoalPlannerData gp_planner_data(planner_data, parameters); + gp_planner_data.update( + parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, + planner_data, current_status, previous_module_output, goal_searcher, vehicle_footprint); + return gp_planner_data; +} + +void GoalPlannerModule::GoalPlannerData::update( + const GoalPlannerParameters & parameters_, + const std::shared_ptr & ego_predicted_path_params_, + const std::shared_ptr & objects_filtering_params_, + const std::shared_ptr & safety_check_params_, + const PlannerData & planner_data_, const ModuleStatus & current_status_, + const BehaviorModuleOutput & previous_module_output_, + const std::shared_ptr goal_searcher_, + const tier4_autoware_utils::LinearRing2d & vehicle_footprint_) +{ + parameters = parameters_; + ego_predicted_path_params = ego_predicted_path_params_; + objects_filtering_params = objects_filtering_params_; + safety_check_params = safety_check_params_; + vehicle_footprint = vehicle_footprint_; + + planner_data = planner_data_; + planner_data.route_handler = std::make_shared(*(planner_data_.route_handler)); + current_status = current_status_; + previous_module_output = previous_module_output_; + occupancy_grid_map->setMap(*(planner_data.occupancy_grid)); + // to create a deepcopy of GoalPlanner(not GoalPlannerBase), goal_searcher_ is not enough, so + // recreate it here + goal_searcher = std::make_shared(parameters, vehicle_footprint); + // and then copy the reference goal + goal_searcher->setReferenceGoal(goal_searcher_->getReferenceGoal()); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index bd19871c482fc..df91687622349 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -90,35 +90,35 @@ struct SortByWeightedDistance }; GoalSearcher::GoalSearcher( - const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, - const std::shared_ptr & occupancy_grid_map) + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint) : GoalSearcherBase{parameters}, vehicle_footprint_{vehicle_footprint}, - occupancy_grid_map_{occupancy_grid_map}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } -GoalCandidates GoalSearcher::search() +GoalCandidates GoalSearcher::search( + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) { GoalCandidates goal_candidates{}; - const auto & route_handler = planner_data_->route_handler; + const auto & route_handler = planner_data->route_handler; const double forward_length = parameters_.forward_goal_search_length; const double backward_length = parameters_.backward_goal_search_length; const double margin_from_boundary = parameters_.margin_from_boundary; const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; - const double vehicle_width = planner_data_->parameters.vehicle_width; - const double base_link2front = planner_data_->parameters.base_link2front; - const double base_link2rear = planner_data_->parameters.base_link2rear; + const double vehicle_width = planner_data->parameters.vehicle_width; + const double base_link2front = planner_data->parameters.base_link2front; + const double base_link2rear = planner_data->parameters.base_link2rear; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); auto lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_length, forward_length, + planner_data, backward_length, forward_length, /*forward_only_in_route*/ false); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); @@ -193,17 +193,18 @@ GoalCandidates GoalSearcher::search() goal_candidates.push_back(goal_candidate); } } - createAreaPolygons(original_search_poses); + createAreaPolygons(original_search_poses, planner_data); - update(goal_candidates); + update(goal_candidates, occupancy_grid_map, planner_data); return goal_candidates; } void GoalSearcher::countObjectsToAvoid( - GoalCandidates & goal_candidates, const PredictedObjects & objects) const + GoalCandidates & goal_candidates, const PredictedObjects & objects, + const std::shared_ptr & planner_data) const { - const auto & route_handler = planner_data_->route_handler; + const auto & route_handler = planner_data->route_handler; const double forward_length = parameters_.forward_goal_search_length; const double backward_length = parameters_.backward_goal_search_length; @@ -225,7 +226,7 @@ void GoalSearcher::countObjectsToAvoid( // generate current lane center line path to check collision with objects const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); const auto current_center_line_path = std::invoke([&]() -> PathWithLaneId { const double s_start = @@ -264,16 +265,19 @@ void GoalSearcher::countObjectsToAvoid( } } -void GoalSearcher::update(GoalCandidates & goal_candidates) const +void GoalSearcher::update( + GoalCandidates & goal_candidates, + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) const { const auto pull_over_lane_stop_objects = goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, parameters_.detection_bound_offset, - *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + *(planner_data->dynamic_object), parameters_.th_moving_object_velocity); if (parameters_.prioritize_goals_before_objects) { - countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); + countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects, planner_data); } if (parameters_.goal_priority == "minimum_weighted_distance") { @@ -293,7 +297,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const const Pose goal_pose = goal_candidate.goal_pose; // check collision with footprint - if (checkCollision(goal_pose, pull_over_lane_stop_objects)) { + if (checkCollision(goal_pose, pull_over_lane_stop_objects, occupancy_grid_map)) { goal_candidate.is_safe = false; continue; } @@ -301,9 +305,10 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const // check longitudinal margin with pull over lane objects constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( - goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, + goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_hard_margins.back(), filter_inside); - if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { + if (checkCollisionWithLongitudinalDistance( + goal_pose, target_objects, occupancy_grid_map, planner_data)) { goal_candidate.is_safe = false; continue; } @@ -316,7 +321,9 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const // current planner_data_ and margin scale factor. // And is_safe is not updated in this function. bool GoalSearcher::isSafeGoalWithMarginScaleFactor( - const GoalCandidate & goal_candidate, const double margin_scale_factor) const + const GoalCandidate & goal_candidate, const double margin_scale_factor, + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) const { if (!parameters_.use_object_recognition) { return true; @@ -326,9 +333,9 @@ bool GoalSearcher::isSafeGoalWithMarginScaleFactor( const auto pull_over_lane_stop_objects = goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, parameters_.detection_bound_offset, - *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + *(planner_data->dynamic_object), parameters_.th_moving_object_velocity); const double margin = parameters_.object_recognition_collision_check_hard_margins.back() * margin_scale_factor; @@ -341,23 +348,26 @@ bool GoalSearcher::isSafeGoalWithMarginScaleFactor( // check longitudinal margin with pull over lane objects constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( - goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, margin, + goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects, margin, filter_inside); - if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { + if (checkCollisionWithLongitudinalDistance( + goal_pose, target_objects, occupancy_grid_map, planner_data)) { return false; } return true; } -bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const +bool GoalSearcher::checkCollision( + const Pose & pose, const PredictedObjects & objects, + const std::shared_ptr occupancy_grid_map) const { if (parameters_.use_occupancy_grid_for_goal_search) { - const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); + const Pose pose_grid_coords = global2local(occupancy_grid_map->getMap(), pose); const auto idx = pose2index( - occupancy_grid_map_->getMap(), pose_grid_coords, occupancy_grid_map_->getParam().theta_size); + occupancy_grid_map->getMap(), pose_grid_coords, occupancy_grid_map->getParam().theta_size); const bool check_out_of_range = false; - if (occupancy_grid_map_->detectCollision(idx, check_out_of_range)) { + if (occupancy_grid_map->detectCollision(idx, check_out_of_range)) { return true; } } @@ -373,7 +383,9 @@ bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & ob } bool GoalSearcher::checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & objects) const + const Pose & ego_pose, const PredictedObjects & objects, + const std::shared_ptr occupancy_grid_map, + const std::shared_ptr & planner_data) const { if ( parameters_.use_occupancy_grid_for_goal_search && @@ -385,22 +397,22 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( // check forward collision const Pose ego_pose_moved_forward = calcOffsetPose(ego_pose, offset, 0, 0); const Pose forward_pose_grid_coords = - global2local(occupancy_grid_map_->getMap(), ego_pose_moved_forward); + global2local(occupancy_grid_map->getMap(), ego_pose_moved_forward); const auto forward_idx = pose2index( - occupancy_grid_map_->getMap(), forward_pose_grid_coords, - occupancy_grid_map_->getParam().theta_size); - if (occupancy_grid_map_->detectCollision(forward_idx, check_out_of_range)) { + occupancy_grid_map->getMap(), forward_pose_grid_coords, + occupancy_grid_map->getParam().theta_size); + if (occupancy_grid_map->detectCollision(forward_idx, check_out_of_range)) { return true; } // check backward collision const Pose ego_pose_moved_backward = calcOffsetPose(ego_pose, -offset, 0, 0); const Pose backward_pose_grid_coords = - global2local(occupancy_grid_map_->getMap(), ego_pose_moved_backward); + global2local(occupancy_grid_map->getMap(), ego_pose_moved_backward); const auto backward_idx = pose2index( - occupancy_grid_map_->getMap(), backward_pose_grid_coords, - occupancy_grid_map_->getParam().theta_size); - if (occupancy_grid_map_->detectCollision(backward_idx, check_out_of_range)) { + occupancy_grid_map->getMap(), backward_pose_grid_coords, + occupancy_grid_map->getParam().theta_size); + if (occupancy_grid_map->detectCollision(backward_idx, check_out_of_range)) { return true; } } @@ -408,23 +420,24 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( if (parameters_.use_object_recognition) { if ( utils::calcLongitudinalDistanceFromEgoToObjects( - ego_pose, planner_data_->parameters.base_link2front, - planner_data_->parameters.base_link2rear, objects) < parameters_.longitudinal_margin) { + ego_pose, planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, + objects) < parameters_.longitudinal_margin) { return true; } } return false; } -void GoalSearcher::createAreaPolygons(std::vector original_search_poses) +void GoalSearcher::createAreaPolygons( + std::vector original_search_poses, const std::shared_ptr & planner_data) { using tier4_autoware_utils::MultiPolygon2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; - const double vehicle_width = planner_data_->parameters.vehicle_width; - const double base_link2front = planner_data_->parameters.base_link2front; - const double base_link2rear = planner_data_->parameters.base_link2rear; + const double vehicle_width = planner_data->parameters.vehicle_width; + const double base_link2front = planner_data->parameters.base_link2front; + const double base_link2rear = planner_data->parameters.base_link2rear; const double max_lateral_offset = parameters_.max_lateral_offset; const auto appendPointToPolygon = @@ -506,10 +519,11 @@ bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons } GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( - const GoalCandidates & goal_candidates) const + const GoalCandidates & goal_candidates, + const std::shared_ptr & planner_data) const { const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); // Define a lambda function to compute the arc length for a given goal candidate. diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index b168da61e6239..33d1eb78e14f6 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -28,11 +28,9 @@ namespace behavior_path_planner { ShiftPullOver::ShiftPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, - const std::shared_ptr & occupancy_grid_map) + const LaneDepartureChecker & lane_departure_checker) : PullOverPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}, - occupancy_grid_map_{occupancy_grid_map}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { }