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 c5b2209c16f19..df21bad862cc9 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 @@ -135,10 +135,8 @@ class PullOverStatus DEFINE_SETTER_GETTER(bool, has_decided_velocity) DEFINE_SETTER_GETTER(bool, has_requested_approval) DEFINE_SETTER_GETTER(bool, is_ready) - DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_time) DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) - DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_pose) DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) @@ -166,10 +164,8 @@ class PullOverStatus bool is_ready_{false}; // save last time and pose - std::shared_ptr last_approved_time_; std::shared_ptr last_increment_time_; std::shared_ptr last_path_update_time_; - std::shared_ptr last_approved_pose_; // goal modification std::optional modified_goal_pose_; @@ -199,6 +195,14 @@ struct GoalPlannerDebugData std::vector ego_polygons_expanded{}; }; +struct LastApprovalData +{ + LastApprovalData(rclcpp::Time time, Pose pose) : time(time), pose(pose) {} + + rclcpp::Time time{}; + Pose pose{}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -219,14 +223,11 @@ class GoalPlannerModule : public SceneModuleInterface } } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; + bool isExecutionRequested() const override; + bool isExecutionReady() const override; void processOnExit() override; void updateData() override; void setParameters(const std::shared_ptr & parameters); @@ -235,15 +236,15 @@ class GoalPlannerModule : public SceneModuleInterface { } - // not used, but need to override - CandidateOutput planCandidate() const override { return CandidateOutput{}; }; - private: + // The start_planner activates when it receives a new route, + // so there is no need to terminate the goal planner. + // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; @@ -276,6 +277,8 @@ class GoalPlannerModule : public SceneModuleInterface std::recursive_mutex mutex_; PullOverStatus status_; + std::unique_ptr last_approval_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. const double approximate_pull_over_distance_{20.0}; 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 d6f80697d9fd6..8b6ac5394c3d4 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 @@ -227,18 +227,6 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput GoalPlannerModule::run() -{ - current_state_ = ModuleStatus::RUNNING; - updateOccupancyGrid(); - - if (!isActivated()) { - return planWaitingApproval(); - } - - return plan(); -} - void GoalPlannerModule::updateData() { // Initialize Occupancy Grid Map @@ -287,6 +275,8 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); + status_.reset(); + last_approval_data_.reset(); } bool GoalPlannerModule::isExecutionRequested() const @@ -448,13 +438,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus GoalPlannerModule::updateState() -{ - // start_planner module will be run when setting new goal, so not need finishing pull_over module. - // Finishing it causes wrong lane_following path generation. - return current_state_; -} - bool GoalPlannerModule::planFreespacePath() { goal_searcher_->setPlannerData(planner_data_); @@ -914,6 +897,12 @@ void GoalPlannerModule::decideVelocity() status_.set_has_decided_velocity(true); } +CandidateOutput GoalPlannerModule::planCandidate() const +{ + return CandidateOutput( + status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); +} + BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { // if pull over path candidates generation is not finished, use previous module output @@ -931,14 +920,9 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // Use decided path if (status_.get_has_decided_path()) { - if (isActivated() && isWaitingApproval()) { - { - const std::lock_guard lock(mutex_); - status_.set_last_approved_time(std::make_shared(clock_->now())); - status_.set_last_approved_pose( - std::make_shared(planner_data_->self_odometry->pose.pose)); - } - clearWaitingApproval(); + if (isActivated() && !last_approval_data_) { + last_approval_data_ = + std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); decideVelocity(); } transitionToNextPathIfFinishingCurrentPath(); @@ -954,7 +938,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible @@ -1005,16 +989,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( return getPreviousModuleOutput(); } - waitApproval(); - BehaviorModuleOutput out; out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = - status_.get_is_safe_static_objects() - ? std::make_shared(status_.get_pull_over_path()->getFullPath()) - : out.path; + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); @@ -1191,12 +1170,11 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && status_.get_last_approved_time()) { + if (isActivated() && last_approval_data_) { // if using arc_path and finishing current_path, get next path // enough time for turn signal - const bool has_passed_enough_time = - (clock_->now() - *status_.get_last_approved_time()).seconds() > - planner_data_->parameters.turn_signal_search_time; + const bool has_passed_enough_time = (clock_->now() - last_approval_data_->time).seconds() > + planner_data_->parameters.turn_signal_search_time; if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { if (incrementPathIndex()) { @@ -1331,10 +1309,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over - turn_signal.desired_start_point = - status_.get_last_approved_pose() && status_.get_has_decided_path() - ? *status_.get_last_approved_pose() - : current_pose; + turn_signal.desired_start_point = last_approval_data_ && status_.get_has_decided_path() + ? last_approval_data_->pose + : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose;