From 58661d9d5e7c0010e3d116bc5b97c3ce4ea452a2 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 30 Jan 2025 14:31:51 +0900 Subject: [PATCH] refactor(goal_planner): refactor goal_searcher and goal_candidates Signed-off-by: Mamoru Sobue --- .../decision_state.hpp | 6 +- .../goal_candidate.hpp | 36 +++++ .../goal_planner_module.hpp | 7 +- .../goal_searcher.hpp | 45 ++++-- .../goal_searcher_base.hpp | 74 ---------- .../pull_over_planner_base.hpp | 2 +- .../util.hpp | 2 +- .../src/decision_state.cpp | 6 +- .../src/goal_planner_module.cpp | 98 +++++++------ .../src/goal_searcher.cpp | 132 +++++++++--------- 10 files changed, 208 insertions(+), 200 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp delete mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index 1092047e65030..df17becd10360 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" @@ -56,7 +56,7 @@ class PathDecisionStateController const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path, std::vector & ego_polygons_expanded); @@ -77,7 +77,7 @@ class PathDecisionStateController const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp new file mode 100644 index 0000000000000..a9fe54ac36d80 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp @@ -0,0 +1,36 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_ + +#include + +#include + +namespace autoware::behavior_path_planner +{ +struct GoalCandidate +{ + geometry_msgs::msg::Pose goal_pose{}; + double distance_from_original_goal{0.0}; + double lateral_offset{0.0}; + size_t id{0}; + bool is_safe{true}; + size_t num_objects_to_avoid{0}; +}; +using GoalCandidates = std::vector; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_ 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..ec92a104f7865 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 @@ -335,8 +335,8 @@ class GoalPlannerModule : public SceneModuleInterface std::unique_ptr fixed_goal_planner_; // goal searcher - std::shared_ptr goal_searcher_; - GoalCandidates goal_candidates_{}; + std::optional goal_searcher_{}; + std::optional goal_candidates_{}; // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on // onFreespaceParkingTimer thread storage may point to while calculation. @@ -362,7 +362,7 @@ class GoalPlannerModule : public SceneModuleInterface mutable GoalPlannerDebugData debug_data_; // goal seach - GoalCandidates generateGoalCandidates() const; + GoalCandidates generateGoalCandidates(GoalSearcher & goal_searcher) const; /* * state transitions and plan function used in each state @@ -431,7 +431,6 @@ class GoalPlannerModule : public SceneModuleInterface std::optional selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const std::optional> sorted_bezier_indices_opt) const; // lanes and drivable area 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..23bb6631194c1 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 @@ -15,7 +15,12 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include #include #include @@ -24,31 +29,44 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::LinearRing2d; using BasicPolygons2d = std::vector; +using autoware::universe_utils::MultiPolygon2d; -class GoalSearcher : public GoalSearcherBase +class GoalSearcher { public: - GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint); + static GoalSearcher create( + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const std::shared_ptr & planner_data); - GoalCandidates search(const std::shared_ptr & planner_data) override; +public: + GoalCandidates search(const std::shared_ptr & planner_data); void update( GoalCandidates & goal_candidates, const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, - const PredictedObjects & objects) const override; + const PredictedObjects & objects) const; // todo(kosuke55): Functions for this specific use should not be in the interface, // so it is better to consider interface design when we implement other goal searchers. GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, - const std::shared_ptr & planner_data) const override; + const std::shared_ptr & planner_data) const; bool isSafeGoalWithMarginScaleFactor( const GoalCandidate & goal_candidate, const double margin_scale_factor, const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, - const PredictedObjects & objects) const override; + const PredictedObjects & objects) const; + + MultiPolygon2d getAreaPolygons() const { return area_polygons_; } private: + GoalSearcher( + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const bool left_side_parking, const lanelet::ConstLanelets & pull_over_lanes, + const lanelet::BasicPolygons2d & no_parking_area_polygons, + const lanelet::BasicPolygons2d & no_stopping_area_polygons, + const lanelet::BasicPolygons2d & bus_stop_area_polygons); + void countObjectsToAvoid( GoalCandidates & goal_candidates, const PredictedObjects & objects, const std::shared_ptr & planner_data, @@ -63,11 +81,16 @@ class GoalSearcher : public GoalSearcherBase const Pose & ego_pose, const PredictedObjects & objects, const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data) const; - BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; - BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; - LinearRing2d vehicle_footprint_{}; - bool left_side_parking_{true}; + const GoalPlannerParameters parameters_; + const LinearRing2d vehicle_footprint_; + const bool left_side_parking_; + const lanelet::ConstLanelets pull_over_lanes_; + const lanelet::BasicPolygons2d no_parking_area_polygons_; + const lanelet::BasicPolygons2d no_stopping_area_polygons_; + const lanelet::BasicPolygons2d bus_stop_area_polygons_; + + MultiPolygon2d area_polygons_{}; }; } // namespace autoware::behavior_path_planner 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 deleted file mode 100644 index 23a51d1f8c86a..0000000000000 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ - -#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware/behavior_path_planner_common/data_manager.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" - -#include - -#include -#include - -namespace autoware::behavior_path_planner -{ -using autoware::universe_utils::MultiPolygon2d; -using geometry_msgs::msg::Pose; - -struct GoalCandidate -{ - Pose goal_pose{}; - double distance_from_original_goal{0.0}; - double lateral_offset{0.0}; - size_t id{0}; - bool is_safe{true}; - size_t num_objects_to_avoid{0}; -}; -using GoalCandidates = std::vector; - -class GoalSearcherBase -{ -public: - explicit GoalSearcherBase(const GoalPlannerParameters & parameters) : parameters_{parameters} {} - virtual ~GoalSearcherBase() = default; - - MultiPolygon2d getAreaPolygons() const { return area_polygons_; } - virtual GoalCandidates search(const std::shared_ptr & planner_data) = 0; - virtual void update( - [[maybe_unused]] GoalCandidates & goal_candidates, - [[maybe_unused]] const std::shared_ptr occupancy_grid_map, - [[maybe_unused]] const std::shared_ptr & planner_data, - [[maybe_unused]] const PredictedObjects & objects) const - { - return; - } - virtual GoalCandidate getClosetGoalCandidateAlongLanes( - const GoalCandidates & goal_candidates, - const std::shared_ptr & planner_data) const = 0; - virtual bool isSafeGoalWithMarginScaleFactor( - const GoalCandidate & goal_candidate, const double margin_scale_factor, - const std::shared_ptr occupancy_grid_map, - const std::shared_ptr & planner_data, - const PredictedObjects & objects) const = 0; - -protected: - const GoalPlannerParameters parameters_; - MultiPolygon2d area_polygons_{}; -}; -} // namespace autoware::behavior_path_planner - -#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 7f404a4055b26..6e7bc6e14e307 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -14,8 +14,8 @@ #pragma once +#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 7f375da295fb9..8bbc53f49105a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index 1a4a6e5a25589..ca55cbd993650 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -32,7 +32,7 @@ void PathDecisionStateController::transit_state( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path, std::vector & ego_polygons_expanded) { @@ -50,7 +50,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const { @@ -105,7 +105,7 @@ PathDecisionState PathDecisionStateController::get_next_state( // check goal pose collision if ( - modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( + modified_goal_opt && !goal_searcher.isSafeGoalWithMarginScaleFactor( modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data, static_target_objects)) { RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); 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..7ada75204a533 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 @@ -89,10 +89,6 @@ GoalPlannerModule::GoalPlannerModule( // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); - // set selected goal searcher - // currently there is only one goal_searcher_type - goal_searcher_ = std::make_shared(parameters_, vehicle_footprint_); - // timer callback for generating lane parking candidate paths const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); lane_parking_timer_cb_group_ = @@ -210,8 +206,9 @@ std::optional planFreespacePath( std::shared_ptr freespace_planner) { auto goal_candidates = req.goal_candidates_; - auto goal_searcher = std::make_shared(req.parameters_, req.vehicle_footprint_); - goal_searcher->update( + auto goal_searcher = + GoalSearcher::create(req.parameters_, req.vehicle_footprint_, req.get_planner_data()); + goal_searcher.update( goal_candidates, req.get_occupancy_grid_map(), req.get_planner_data(), static_target_objects); for (size_t i = 0; i < goal_candidates.size(); i++) { @@ -639,6 +636,9 @@ void FreespaceParkingPlanner::onTimer() std::pair GoalPlannerModule::syncWithThreads() { + assert(goal_candidates_); + const auto & goal_candidates = goal_candidates_.value(); + // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then @@ -657,8 +657,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()); + lane_parking_request_.emplace(vehicle_footprint_, goal_candidates, getPreviousModuleOutput()); } // 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 @@ -689,7 +688,7 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(freespace_parking_mutex_); if (!freespace_parking_request_) { freespace_parking_request_.emplace( - parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); + parameters_, vehicle_footprint_, goal_candidates, *planner_data_); } constexpr double stuck_time = 5.0; freespace_parking_request_.value().update( @@ -711,10 +710,15 @@ void GoalPlannerModule::updateData() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // update goal searcher and generate goal candidates - if (goal_candidates_.empty()) { - goal_candidates_ = generateGoalCandidates(); + if (!goal_searcher_) { + goal_searcher_.emplace(GoalSearcher::create(parameters_, vehicle_footprint_, planner_data_)); } + const auto & goal_searcher = goal_searcher_.value(); + + if (!goal_candidates_) { + goal_candidates_ = generateGoalCandidates(goal_searcher_.value()); + } + auto & goal_candidates_mut = goal_candidates_.value(); const lanelet::ConstLanelets current_lanes = utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); @@ -775,7 +779,7 @@ void GoalPlannerModule::updateData() path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, parameters_, - goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); + goal_searcher, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); if (context_data_) { @@ -795,17 +799,20 @@ void GoalPlannerModule::updateData() parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } - auto & ctx_data = context_data_.value(); + const auto & ctx_data = context_data_.value(); + goal_searcher.update( + goal_candidates_mut, occupancy_grid_map_, planner_data_, ctx_data.static_target_objects); + auto & ctx_data_mut = context_data_.value(); if (!isActivated()) { return; } - if (hasFinishedCurrentPath(ctx_data)) { - if (ctx_data.pull_over_path_opt) { - auto & pull_over_path = ctx_data.pull_over_path_opt.value(); + if (hasFinishedCurrentPath(ctx_data_mut)) { + if (ctx_data_mut.pull_over_path_opt) { + auto & pull_over_path = ctx_data_mut.pull_over_path_opt.value(); if (pull_over_path.incrementPathIndex()) { - ctx_data.last_path_idx_increment_time = clock_->now(); + ctx_data_mut.last_path_idx_increment_time = clock_->now(); } } } @@ -814,7 +821,7 @@ void GoalPlannerModule::updateData() last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); // TODO(soblin): do not "plan" in updateData - if (ctx_data.pull_over_path_opt) decideVelocity(ctx_data.pull_over_path_opt.value()); + if (ctx_data_mut.pull_over_path_opt) decideVelocity(ctx_data_mut.pull_over_path_opt.value()); } } @@ -982,14 +989,14 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return true; } -GoalCandidates GoalPlannerModule::generateGoalCandidates() const +GoalCandidates GoalPlannerModule::generateGoalCandidates(GoalSearcher & goal_searcher) 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_); } // NOTE: @@ -1009,12 +1016,12 @@ BehaviorModuleOutput GoalPlannerModule::plan() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - if (!context_data_) { + if (!context_data_ || !goal_candidates_) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, " [pull_over] plan() is called without valid context_data"); } else { - auto & context_data = context_data_.value(); - return planPullOver(context_data); + auto & context_data_mut = context_data_.value(); + return planPullOver(context_data_mut); } } @@ -1199,11 +1206,13 @@ void sortPullOverPaths( std::optional GoalPlannerModule::selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const std::optional> sorted_bezier_indices_opt) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(goal_candidates_); + const auto & goal_candidates = goal_candidates_.value(); + const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); const double backward_length = parameters_.backward_goal_search_length + parameters_.decide_path_distance; @@ -1307,12 +1316,15 @@ void GoalPlannerModule::setOutput( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(!goal_candidates_); + const auto & goal_candidates = goal_candidates_.value(); + output.reference_path = getPreviousModuleOutput().reference_path; if (!selected_pull_over_path_with_velocity_opt) { // situation : not safe against static objects use stop_path output.path = generateStopPath( - context_data, (goal_candidates_.empty() ? "no goal candidate" : "no static safe path")); + context_data, (goal_candidates.empty() ? "no goal candidate" : "no static safe path")); RCLCPP_INFO_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); setDrivableAreaInfo(context_data, output); @@ -1436,11 +1448,14 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & conte { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(goal_candidates_); + const auto & goal_candidates = goal_candidates_.value(); + const auto current_state = path_decision_controller_.get_current_state(); if (current_state.state != PathDecisionState::DecisionKind::DECIDED) { const bool is_stable_safe = current_state.is_stable_safe; const std::string detail = - goal_candidates_.empty() ? "no goal candidate" + goal_candidates.empty() ? "no goal candidate" : context_data.lane_parking_response.pull_over_path_candidates.empty() ? "no path candidate" : !context_data.pull_over_path_opt ? "no static safe path" : !is_stable_safe ? "unsafe against dynamic objects" @@ -1525,16 +1540,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData context_data.last_path_update_time = std::nullopt; context_data.last_path_idx_increment_time = std::nullopt; - // update goal candidates - auto goal_candidates = goal_candidates_; - goal_searcher_->update( - goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); - // Select a path that is as safe as possible and has a high priority. const auto & pull_over_path_candidates = context_data.lane_parking_response.pull_over_path_candidates; const auto lane_pull_over_path_opt = selectPullOverPath( - context_data, pull_over_path_candidates, goal_candidates, + context_data, pull_over_path_candidates, context_data.lane_parking_response.sorted_bezier_indices_opt); // update thread_safe_data_ @@ -1637,14 +1647,14 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - if (!context_data_) { + if (!context_data_ || !goal_candidates_) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, " [pull_over] planWaitingApproval() is called without valid context_data. use fixed goal " "planner"); } else { - auto & context_data = context_data_.value(); - return planPullOverAsCandidate(context_data, "waiting approval"); + auto & context_data_mut = context_data_.value(); + return planPullOverAsCandidate(context_data_mut, "waiting approval"); } } @@ -1689,6 +1699,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath( const PullOverContextData & context_data, const std::string & detail) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(!goal_searcher_); + assert(!goal_candidates_); + const auto & goal_searcher = goal_searcher_.value(); + const auto & goal_candidates = goal_candidates_.value(); const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -1733,7 +1747,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // 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(goal_candidates_, planner_data_); + goal_searcher.getClosetGoalCandidateAlongLanes(goal_candidates, planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); @@ -2064,9 +2078,12 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(goal_candidates_); + const auto & goal_candidates = goal_candidates_.value(); + // decelerate before the search area start const auto closest_goal_candidate = - goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); + goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates, planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); @@ -2417,6 +2434,9 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(goal_candidates_); + const auto & goal_candidates = goal_candidates_.value(); + debug_marker_.markers.clear(); using autoware::motion_utils::createStopVirtualWallMarker; @@ -2450,7 +2470,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); + add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); // Visualize objects extraction polygon auto marker = autoware::universe_utils::createDefaultMarker( 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..e800cc56e7540 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 @@ -18,6 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" @@ -93,13 +94,59 @@ struct SortByWeightedDistance }; GoalSearcher::GoalSearcher( - const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint) -: GoalSearcherBase{parameters}, - vehicle_footprint_{vehicle_footprint}, - left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const bool left_side_parking, const lanelet::ConstLanelets & pull_over_lanes, + const lanelet::BasicPolygons2d & no_parking_area_polygons, + const lanelet::BasicPolygons2d & no_stopping_area_polygons, + const lanelet::BasicPolygons2d & bus_stop_area_polygons) +: parameters_(parameters), + vehicle_footprint_(vehicle_footprint), + left_side_parking_(left_side_parking), + pull_over_lanes_(pull_over_lanes), + no_parking_area_polygons_(no_parking_area_polygons), + no_stopping_area_polygons_(no_stopping_area_polygons), + bus_stop_area_polygons_(bus_stop_area_polygons) { } +GoalSearcher GoalSearcher::create( + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const std::shared_ptr & planner_data) +{ + const auto left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE; + 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 no_parking_areas = lanelet::utils::query::noParkingAreas(pull_over_lanes); + lanelet::BasicPolygons2d no_parking_area_polygons; + for (const auto & no_parking_area : no_parking_areas) { + for (const auto & area : no_parking_area->noParkingAreas()) { + no_parking_area_polygons.push_back(lanelet::utils::to2D(area).basicPolygon()); + } + } + + const auto no_stopping_areas = lanelet::utils::query::noStoppingAreas(pull_over_lanes); + lanelet::BasicPolygons2d no_stopping_area_polygons; + for (const auto & no_stopping_area : no_stopping_areas) { + for (const auto & area : no_stopping_area->noStoppingAreas()) { + no_stopping_area_polygons.push_back(lanelet::utils::to2D(area).basicPolygon()); + } + } + + const auto bus_stop_areas = lanelet::utils::query::busStopAreas(pull_over_lanes); + lanelet::BasicPolygons2d bus_stop_area_polygons; + for (const auto & bus_stop_area : bus_stop_areas) { + for (const auto & area : bus_stop_area->busStopAreas()) { + bus_stop_area_polygons.push_back(lanelet::utils::to2D(area).basicPolygon()); + } + } + + return GoalSearcher( + parameters, vehicle_footprint, left_side_parking, pull_over_lanes, no_parking_area_polygons, + no_stopping_area_polygons, bus_stop_area_polygons); +} + GoalCandidates GoalSearcher::search(const std::shared_ptr & planner_data) { GoalCandidates goal_candidates{}; @@ -128,24 +175,17 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const double base_link2front = planner_data->parameters.base_link2front; const double base_link2rear = planner_data->parameters.base_link2rear; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *route_handler, left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( - pull_over_lanes, *route_handler, left_side_parking_); + pull_over_lanes_, *route_handler, left_side_parking_); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose); + lanelet::utils::getArcCoordinates(pull_over_lanes_, reference_goal_pose); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; const double longitudinal_interval = use_bus_stop_area ? parameters_.bus_stop_area.goal_search_interval : parameters_.goal_search_interval; auto center_line_path = utils::resamplePathWithSpline( - route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval); - - const auto no_parking_area_polygons = getNoParkingAreaPolygons(pull_over_lanes); - const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); - const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); + route_handler->getCenterLinePath(pull_over_lanes_, s_start, s_end), longitudinal_interval); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -157,13 +197,13 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p // ignore goal_pose near lane start const double distance_from_lane_start = - lanelet::utils::getArcCoordinates(pull_over_lanes, center_pose).length; + lanelet::utils::getArcCoordinates(pull_over_lanes_, center_pose).length; if (distance_from_lane_start < ignore_distance_from_lane_start) { continue; } const auto distance_from_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + pull_over_lanes_, vehicle_width, base_link2front, base_link2rear, center_pose, left_side_parking_); if (!distance_from_bound) continue; @@ -187,18 +227,19 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p if ( parameters_.bus_stop_area.use_bus_stop_area && - !goal_planner_utils::isWithinAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) { + !goal_planner_utils::isWithinAreas( + transformed_vehicle_footprint, bus_stop_area_polygons_)) { continue; } if (goal_planner_utils::isIntersectingAreas( - transformed_vehicle_footprint, no_parking_area_polygons)) { + transformed_vehicle_footprint, no_parking_area_polygons_)) { // break here to exclude goals located laterally in no_parking_areas break; } if (goal_planner_utils::isIntersectingAreas( - transformed_vehicle_footprint, no_stopping_area_polygons)) { + transformed_vehicle_footprint, no_stopping_area_polygons_)) { // break here to exclude goals located laterally in no_stopping_areas break; } @@ -214,7 +255,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p (transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontLeftIndex) + transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontRightIndex)) / 2.0; - const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes); + const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes_); const auto vehicle_front_pose_for_bound_opt = goal_planner_utils::calcClosestPose( left_side_parking_ ? pull_over_lanelet.leftBound() : pull_over_lanelet.rightBound(), autoware::universe_utils::createPoint( @@ -249,15 +290,12 @@ void GoalSearcher::countObjectsToAvoid( // calculate search start/end pose in pull over lanes const auto search_start_end_poses = std::invoke([&]() -> std::pair { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *route_handler, left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose); + lanelet::utils::getArcCoordinates(pull_over_lanes_, reference_goal_pose); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; const auto center_line_path = utils::resamplePathWithSpline( - route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + route_handler->getCenterLinePath(pull_over_lanes_, s_start, s_end), parameters_.goal_search_interval); return std::make_pair( center_line_path.points.front().point.pose, center_line_path.points.back().point.pose); @@ -370,10 +408,6 @@ bool GoalSearcher::isSafeGoalWithMarginScaleFactor( const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, const PredictedObjects & objects) const { - if (!parameters_.use_object_recognition) { - return true; - } - const Pose goal_pose = goal_candidate.goal_pose; const double margin = parameters_.object_recognition_collision_check_hard_margins.back() * margin_scale_factor; @@ -454,13 +488,11 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( } } - if (parameters_.use_object_recognition) { - if ( - utils::calcLongitudinalDistanceFromEgoToObjects( - ego_pose, planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, - objects) < parameters_.longitudinal_margin) { - return true; - } + if ( + utils::calcLongitudinalDistanceFromEgoToObjects( + ego_pose, planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, + objects) < parameters_.longitudinal_margin) { + return true; } return false; } @@ -517,34 +549,6 @@ void GoalSearcher::createAreaPolygons( } } -BasicPolygons2d GoalSearcher::getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const -{ - BasicPolygons2d area_polygons{}; - for (const auto & ll : lanes) { - for (const auto & reg_elem : ll.regulatoryElementsAs()) { - for (const auto & area : reg_elem->noParkingAreas()) { - const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); - area_polygons.push_back(area_poly); - } - } - } - return area_polygons; -} - -BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const -{ - BasicPolygons2d area_polygons{}; - for (const auto & ll : lanes) { - for (const auto & reg_elem : ll.regulatoryElementsAs()) { - for (const auto & area : reg_elem->noStoppingAreas()) { - const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); - area_polygons.push_back(area_poly); - } - } - } - return area_polygons; -} - GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const