diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index faeadf32ffd5f..27273fa37a3e8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -338,6 +338,8 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr goal_searcher_; GoalCandidates goal_candidates_{}; + bool use_bus_stop_area_{false}; + // 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 @@ -362,7 +364,7 @@ class GoalPlannerModule : public SceneModuleInterface mutable GoalPlannerDebugData debug_data_; // goal seach - GoalCandidates generateGoalCandidates() const; + GoalCandidates generateGoalCandidates(const bool use_bus_stop_area) const; /* * state transitions and plan function used in each state diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index ac4ffe97d6f2d..19231ed26b5c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -30,7 +30,8 @@ class GoalSearcher : public GoalSearcherBase public: GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint); - GoalCandidates search(const std::shared_ptr & planner_data) override; + GoalCandidates search( + const std::shared_ptr & planner_data, const bool use_bus_stop_area) override; void update( GoalCandidates & goal_candidates, const std::shared_ptr occupancy_grid_map, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp index 23a51d1f8c86a..cef9981e3cb14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -47,7 +47,8 @@ class GoalSearcherBase virtual ~GoalSearcherBase() = default; MultiPolygon2d getAreaPolygons() const { return area_polygons_; } - virtual GoalCandidates search(const std::shared_ptr & planner_data) = 0; + virtual GoalCandidates search( + const std::shared_ptr & planner_data, const bool use_bus_stop_area) = 0; virtual void update( [[maybe_unused]] GoalCandidates & goal_candidates, [[maybe_unused]] const std::shared_ptr occupancy_grid_map, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index af20c5f6debe5..d00379c63258f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -33,9 +33,11 @@ class LaneParkingRequest public: LaneParkingRequest( const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output) + const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output, + const bool use_bus_stop_area) : vehicle_footprint_(vehicle_footprint), goal_candidates_(goal_candidates), + use_bus_stop_area_(use_bus_stop_area), upstream_module_output_(upstream_module_output) { } @@ -48,6 +50,7 @@ class LaneParkingRequest const autoware::universe_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; + const bool use_bus_stop_area_; const std::shared_ptr & get_planner_data() const { return planner_data_; } const ModuleStatus & get_current_status() const { return current_status_; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index fa2cef68ed3e7..bf722d19744c9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -319,6 +319,7 @@ void LaneParkingPlanner::onTimer() const auto & pull_over_path_opt = local_request.get_pull_over_path(); const auto & prev_data = local_request.get_prev_data(); const auto trigger_thread_on_approach = local_request.trigger_thread_on_approach(); + const auto use_bus_stop_area = local_request.use_bus_stop_area_; if (!trigger_thread_on_approach) { return; @@ -385,7 +386,7 @@ void LaneParkingPlanner::onTimer() std::vector path_candidates{}; std::optional closest_start_pose{}; std::optional> sorted_indices_opt{std::nullopt}; - if (parameters_.bus_stop_area.use_bus_stop_area && switch_bezier_) { + if (use_bus_stop_area && switch_bezier_) { bezier_planning_helper( local_planner_data, goal_candidates, upstream_module_output, current_lanes, closest_start_pose, path_candidates, sorted_indices_opt); @@ -658,7 +659,7 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(lane_parking_mutex_); if (!lane_parking_request_) { lane_parking_request_.emplace( - vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); + vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(), use_bus_stop_area_); } // 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 @@ -713,7 +714,19 @@ void GoalPlannerModule::updateData() // update goal searcher and generate goal candidates if (goal_candidates_.empty()) { - goal_candidates_ = generateGoalCandidates(); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); + use_bus_stop_area_ = + parameters_.bus_stop_area.use_bus_stop_area && + std::any_of( + bus_stop_area_polygons.begin(), bus_stop_area_polygons.end(), [&](const auto & area) { + const auto & goal_position = planner_data_->route_handler->getOriginalGoalPose().position; + return boost::geometry::within( + universe_utils::Point2d{goal_position.x, goal_position.y}, area); + }); + goal_candidates_ = generateGoalCandidates(use_bus_stop_area_); } const lanelet::ConstLanelets current_lanes = @@ -982,14 +995,14 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return true; } -GoalCandidates GoalPlannerModule::generateGoalCandidates() const +GoalCandidates GoalPlannerModule::generateGoalCandidates(const bool use_bus_stop_area) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // calculate goal candidates const auto & route_handler = planner_data_->route_handler; if (utils::isAllowedGoalModification(route_handler)) { - return goal_searcher_->search(planner_data_); + return goal_searcher_->search(planner_data_, use_bus_stop_area); } // NOTE: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index b973cfd7ed549..6124955a39433 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -100,7 +100,8 @@ GoalSearcher::GoalSearcher( { } -GoalCandidates GoalSearcher::search(const std::shared_ptr & planner_data) +GoalCandidates GoalSearcher::search( + const std::shared_ptr & planner_data, const bool use_bus_stop_area) { GoalCandidates goal_candidates{}; @@ -118,7 +119,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p 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 bool use_bus_stop_area = parameters_.bus_stop_area.use_bus_stop_area; + const double lateral_offset_interval = use_bus_stop_area ? parameters_.bus_stop_area.lateral_offset_interval : parameters_.lateral_offset_interval;