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 95ea7d1ad04c6..28a4bce8703f0 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 @@ -68,64 +68,6 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using tier4_autoware_utils::Polygon2d; -#define DEFINE_SETTER(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER(TYPE, NAME) \ - DEFINE_SETTER(TYPE, NAME) \ - DEFINE_GETTER(TYPE, NAME) - -class PullOverStatus -{ -public: - // Reset all data members to their initial states - void reset() - { - lane_parking_pull_over_path_ = nullptr; - prev_stop_path_ = nullptr; - prev_stop_path_after_approval_ = nullptr; - - is_safe_ = false; - prev_found_path_ = false; - prev_is_safe_ = false; - } - - DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path) - DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path_after_approval) - DEFINE_SETTER_GETTER(bool, is_safe) - DEFINE_SETTER_GETTER(bool, prev_found_path) - DEFINE_SETTER_GETTER(bool, prev_is_safe) - DEFINE_SETTER_GETTER(Pose, refined_goal_pose) - DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) - -private: - std::shared_ptr lane_parking_pull_over_path_{nullptr}; - std::shared_ptr prev_stop_path_{nullptr}; - std::shared_ptr prev_stop_path_after_approval_{nullptr}; - bool is_safe_{false}; - bool prev_found_path_{false}; - bool prev_is_safe_{false}; - - Pose refined_goal_pose_{}; - Pose closest_goal_candidate_pose_{}; -}; - -#undef DEFINE_SETTER -#undef DEFINE_GETTER -#undef DEFINE_SETTER_GETTER - #define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ public: \ void set_##NAME(const TYPE & value) \ @@ -288,6 +230,22 @@ struct LastApprovalData Pose pose{}; }; +struct PreviousPullOverData +{ + void reset() + { + stop_path = nullptr; + stop_path_after_approval = nullptr; + found_path = false; + is_safe = false; + } + + std::shared_ptr stop_path{nullptr}; + std::shared_ptr stop_path_after_approval{nullptr}; + bool found_path{false}; + bool is_safe{false}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -361,10 +319,10 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::recursive_mutex mutex_; - PullOverStatus status_; ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; + PreviousPullOverData prev_data_{nullptr}; // 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. @@ -393,7 +351,7 @@ class GoalPlannerModule : public SceneModuleInterface // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; - void generateGoalCandidates(); + GoalCandidates generateGoalCandidates() const; // stop or decelerate void deceleratePath(PullOverPath & pull_over_path) const; @@ -418,7 +376,6 @@ class GoalPlannerModule : public SceneModuleInterface bool hasDecidedPath() const; void decideVelocity(); bool foundPullOverPath() const; - void updateStatus(const BehaviorModuleOutput & output); // validation bool hasEnoughDistance(const PullOverPath & pull_over_path) const; @@ -451,6 +408,7 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output) const; void setStopPath(BehaviorModuleOutput & output) const; + void updatePreviousData(const BehaviorModuleOutput & output); /** * @brief Sets a stop path in the current path based on safety conditions and previous paths. 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 e8f4b0c9e87ae..701827cca6479 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 @@ -121,7 +121,7 @@ GoalPlannerModule::GoalPlannerModule( goal_planner_data_.collision_check); } - status_.reset(); + prev_data_.reset(); } // This function is needed for waiting for planner_data_ @@ -240,7 +240,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() void GoalPlannerModule::updateData() { - if (getCurrentStatus() == ModuleStatus::IDLE) { + if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { return; } @@ -259,7 +259,13 @@ void GoalPlannerModule::updateData() updateOccupancyGrid(); - generateGoalCandidates(); + // 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()); + } if (!isActivated()) { return; @@ -305,7 +311,7 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - status_.reset(); + prev_data_.reset(); last_approval_data_.reset(); } @@ -542,33 +548,24 @@ bool GoalPlannerModule::canReturnToLaneParking() return true; } -void GoalPlannerModule::generateGoalCandidates() +GoalCandidates GoalPlannerModule::generateGoalCandidates() const { - if (!thread_safe_data_.get_goal_candidates().empty()) { - return; - } - // calculate goal candidates const auto & route_handler = planner_data_->route_handler; - const Pose goal_pose = route_handler->getOriginalGoalPose(); - status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { - const std::lock_guard lock(mutex_); - goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); - thread_safe_data_.set_goal_candidates(goal_searcher_->search()); - status_.set_closest_goal_candidate_pose( - goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()) - .goal_pose); - } else { - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = goal_pose; - goal_candidate.distance_from_original_goal = 0.0; - GoalCandidates goal_candidates{}; - goal_candidates.push_back(goal_candidate); - thread_safe_data_.set_goal_candidates(goal_candidates); - status_.set_closest_goal_candidate_pose(goal_pose); + return goal_searcher_->search(); } + + // NOTE: + // currently since pull over is performed only when isAllowedGoalModification is true, + // never be in the following process. + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = route_handler->getOriginalGoalPose(); + goal_candidate.distance_from_original_goal = 0.0; + GoalCandidates goal_candidates{}; + goal_candidates.push_back(goal_candidate); + + return goal_candidates; } BehaviorModuleOutput GoalPlannerModule::plan() @@ -692,14 +689,14 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const { - if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { + if (prev_data_.found_path || !prev_data_.stop_path) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path - output.path = status_.get_prev_stop_path(); + output.path = prev_data_.stop_path; // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE( @@ -710,7 +707,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (status_.get_prev_is_safe() || !status_.get_prev_stop_path_after_approval()) { + if (prev_data_.is_safe || !prev_data_.stop_path_after_approval) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -718,7 +715,6 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = std::make_shared(*stop_path); - // status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { output.path = @@ -727,10 +723,9 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } - // status_.set_last_path_update_time(std::make_shared(clock_->now())); } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = status_.get_prev_stop_path_after_approval(); + output.path = prev_data_.stop_path_after_approval; // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); @@ -925,7 +920,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver() return output; } - updateStatus(output); + updatePreviousData(output); return output; } @@ -952,17 +947,16 @@ void GoalPlannerModule::postProcess() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -void GoalPlannerModule::updateStatus(const BehaviorModuleOutput & output) +void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) { - if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { - status_.set_prev_stop_path(output.path); + if (prev_data_.found_path || !prev_data_.stop_path) { + prev_data_.stop_path = output.path; } // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.set_prev_found_path(thread_safe_data_.foundPullOverPath()); - status_.set_prev_is_safe( - parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); + prev_data_.found_path = thread_safe_data_.foundPullOverPath(); + prev_data_.is_safe = parameters_->safety_check_params.enable_safety_check ? isSafePath() : true; if (!isActivated()) { return; @@ -970,15 +964,15 @@ void GoalPlannerModule::updateStatus(const BehaviorModuleOutput & output) if ( !parameters_->safety_check_params.enable_safety_check || isSafePath() || - (!status_.get_prev_is_safe() && status_.get_prev_stop_path_after_approval())) { + (!prev_data_.is_safe && prev_data_.stop_path_after_approval)) { return; } - status_.set_prev_is_safe(false); + prev_data_.is_safe = false; const bool is_stop_path = std::any_of( output.path->points.begin(), output.path->points.end(), [](const auto & point) { return point.point.longitudinal_velocity_mps == 0.0; }); if (is_stop_path) { - status_.set_prev_stop_path_after_approval(output.path); + prev_data_.stop_path_after_approval = output.path; } } @@ -1051,8 +1045,10 @@ 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 decel_pose = calcLongitudinalOffsetPose( - reference_path.points, status_.get_closest_goal_candidate_pose().position, + reference_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); // if not approved stop road lane. @@ -1421,8 +1417,10 @@ 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 decel_pose = calcLongitudinalOffsetPose( - pull_over_path.getFullPath().points, status_.get_closest_goal_candidate_pose().position, + pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); auto & first_path = pull_over_path.partial_paths.front(); if (decel_pose) { @@ -1652,7 +1650,7 @@ bool GoalPlannerModule::isSafePath() const pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.get_prev_is_safe() ? 1.0 : parameters_->hysteresis_factor_expand_rate; + prev_data_.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1716,7 +1714,7 @@ void GoalPlannerModule::setDebugData() // 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 double z = status_.get_refined_goal_pose().position.z; + const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z));