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 a072c77526ef5..34ffb61dcd8d5 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 @@ -240,31 +240,28 @@ 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; - void processOnEntry() override; + bool isExecutionRequested() const override; + bool isExecutionReady() const override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } - // 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_; 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 6c431ce5806c3..e71b1ac7936be 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 @@ -124,6 +124,13 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } + status_.reset(); } @@ -228,16 +235,18 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput GoalPlannerModule::run() +void GoalPlannerModule::updateData() { - current_state_ = ModuleStatus::RUNNING; - updateOccupancyGrid(); - - if (!isActivated()) { - return planWaitingApproval(); + // 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(); } - return plan(); + updateOccupancyGrid(); } void GoalPlannerModule::initializeOccupancyGridMap() @@ -264,22 +273,6 @@ void GoalPlannerModule::initializeSafetyCheckParameters() objects_filtering_params_, parameters_); } -void GoalPlannerModule::processOnEntry() -{ - // Initialize occupancy grid map - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - initializeOccupancyGridMap(); - } - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); - } -} - void GoalPlannerModule::processOnExit() { resetPathCandidate(); @@ -446,13 +439,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_); @@ -907,6 +893,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() { constexpr double path_update_duration = 1.0; @@ -945,7 +937,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 @@ -998,10 +990,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( 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();