diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 3841c71a272f0..95ea7d1ad04c6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -168,20 +168,39 @@ class ThreadSafeData return false; } - void set_pull_over_path(const PullOverPath & value) + void set_pull_over_path(const PullOverPath & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = std::make_shared(value); + pull_over_path_ = std::make_shared(path); + if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = std::make_shared(path); + } + last_path_update_time_ = clock_->now(); } - void set_pull_over_path(const std::shared_ptr & value) + void set_pull_over_path(const std::shared_ptr & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = value; + pull_over_path_ = path; + if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = path; + } last_path_update_time_ = clock_->now(); } + template + void set(Args... args) + { + std::lock_guard lock(mutex_); + (..., set(args)); + } + void set(const GoalCandidates & arg) { set_goal_candidates(arg); } + void set(const std::vector & arg) { set_pull_over_path_candidates(arg); } + void set(const std::shared_ptr & arg) { set_pull_over_path(arg); } + void set(const PullOverPath & arg) { set_pull_over_path(arg); } + void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); } + void clearPullOverPath() { const std::lock_guard lock(mutex_); @@ -221,6 +240,7 @@ class ThreadSafeData } DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) @@ -231,6 +251,7 @@ class ThreadSafeData private: std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; GoalCandidates goal_candidates_{}; std::optional modified_goal_pose_; @@ -375,6 +396,7 @@ class GoalPlannerModule : public SceneModuleInterface void generateGoalCandidates(); // stop or decelerate + void deceleratePath(PullOverPath & pull_over_path) const; void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; @@ -409,13 +431,15 @@ class GoalPlannerModule : public SceneModuleInterface // freespace parking bool planFreespacePath(); - void returnToLaneParking(); + bool canReturnToLaneParking(); // plan pull over path BehaviorModuleOutput planRunning(); BehaviorModuleOutput planPullOver(); BehaviorModuleOutput planPullOverAsCandidate(); - void selectSafePullOverPath(); + std::optional> selectPullOverPath( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const; std::vector sortPullOverPathCandidatesByGoalPriority( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 1a7c56c7de58e..e8f4b0c9e87ae 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -54,7 +54,7 @@ namespace behavior_path_planner GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map) + const std::unordered_map> & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, @@ -515,20 +515,20 @@ bool GoalPlannerModule::planFreespacePath() return false; } -void GoalPlannerModule::returnToLaneParking() +bool GoalPlannerModule::canReturnToLaneParking() { // return only before starting free space parking if (!isStopped()) { - return; + return false; } - if (!status_.get_lane_parking_pull_over_path()) { - return; + if (!thread_safe_data_.get_lane_parking_pull_over_path()) { + return false; } - const PathWithLaneId path = status_.get_lane_parking_pull_over_path()->getFullPath(); + const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath(); if (checkCollision(path)) { - return; + return false; } const Point & current_point = planner_data_->self_odometry->pose.pose.position; @@ -536,11 +536,10 @@ void GoalPlannerModule::returnToLaneParking() const bool is_close_to_path = std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; if (!is_close_to_path) { - return; + return false; } - thread_safe_data_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); - RCLCPP_INFO(getLogger(), "return to lane parking"); + return true; } void GoalPlannerModule::generateGoalCandidates() @@ -616,23 +615,10 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return sorted_pull_over_path_candidates; } -void GoalPlannerModule::selectSafePullOverPath() +std::optional> GoalPlannerModule::selectPullOverPath( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const { - // select safe lane pull over path from candidates - std::vector pull_over_path_candidates{}; - 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); - thread_safe_data_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( - thread_safe_data_.get_pull_over_path_candidates(), thread_safe_data_.get_goal_candidates())); - pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); - thread_safe_data_.clearPullOverPath(); - } - for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -651,42 +637,10 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - // found safe pull over path - { - const std::lock_guard lock(mutex_); - thread_safe_data_.set_pull_over_path(pull_over_path); - thread_safe_data_.set_modified_goal_pose(*goal_candidate_it); - status_.set_lane_parking_pull_over_path(thread_safe_data_.get_pull_over_path()); - } - break; + return std::make_pair(pull_over_path, *goal_candidate_it); } - if (!thread_safe_data_.foundPullOverPath()) { - return; - } - - // decelerate before the search area start - const auto decel_pose = calcLongitudinalOffsetPose( - thread_safe_data_.get_pull_over_path()->getFullPath().points, - status_.get_closest_goal_candidate_pose().position, -approximate_pull_over_distance_); - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); - if (decel_pose) { - decelerateBeforeSearchStart(*decel_pose, first_path); - return; - } - - // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - parameters_->pull_over_velocity); - for (auto & p : first_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); - if (min_decel_distance && distance_from_ego < *min_decel_distance) { - continue; - } - p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); - } + return {}; } std::vector GoalPlannerModule::generateDrivableLanes() const @@ -843,7 +797,7 @@ void GoalPlannerModule::updateSteeringFactor( pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, ""); } -// NOTE: this function returns true once it returns true. Because selectSafePullOverPath() will +// NOTE: this function returns true once it returns true. Because selectPullOverPath() will // not select new path. bool GoalPlannerModule::hasDecidedPath() const { @@ -922,8 +876,31 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver() if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { // 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_.get_pull_over_path() - selectSafePullOverPath(); + // and set it to thread_safe_data_ + + 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); + + // update pull over path candidates + const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority( + thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); + + // select pull over path which is safe against static objects and get it's goal + const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates); + + // update thread_safe_data_ + if (path_and_goal_opt) { + auto [pull_over_path, modified_goal] = *path_and_goal_opt; + deceleratePath(pull_over_path); + thread_safe_data_.set( + goal_candidates, pull_over_path_candidates, pull_over_path, modified_goal); + } else { + thread_safe_data_.set(goal_candidates, pull_over_path_candidates); + } } // set output and status @@ -931,8 +908,10 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver() setOutput(output); // return to lane parking if it is possible - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { - returnToLaneParking(); + if ( + thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && + canReturnToLaneParking()) { + thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } // For debug @@ -1121,7 +1100,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return reference_path; } - // if already passed the decle pose, set pull_over_velocity to reference_path. + // if already passed the decel pose, set pull_over_velocity to reference_path. const auto min_decel_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, pull_over_velocity); @@ -1439,6 +1418,32 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( path.points, current_pose.position, ego_idx, pose.position, target_idx); } +void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const +{ + // decelerate before the search area start + const auto decel_pose = calcLongitudinalOffsetPose( + pull_over_path.getFullPath().points, status_.get_closest_goal_candidate_pose().position, + -approximate_pull_over_distance_); + auto & first_path = pull_over_path.partial_paths.front(); + if (decel_pose) { + decelerateBeforeSearchStart(*decel_pose, first_path); + return; + } + + // if already passed the search start offset pose, set pull_over_velocity to first_path. + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); + for (auto & p : first_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); + if (min_decel_distance && distance_from_ego < *min_decel_distance) { + continue; + } + p.point.longitudinal_velocity_mps = std::min( + p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); + } +} + void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const { const double time = planner_data_->parameters.turn_signal_search_time;