From 1bd10db2b3bf839d7a87e1056daf4e905a96f99d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 8 Nov 2024 18:41:19 +0900 Subject: [PATCH] refactor(goal_planner): remove reference_goal_pose getter/setter Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 2 - .../goal_searcher.hpp | 3 +- .../goal_searcher_base.hpp | 11 +----- .../src/goal_planner_module.cpp | 38 ++++++------------- .../src/goal_searcher.cpp | 30 ++++++++++++--- 5 files changed, 41 insertions(+), 43 deletions(-) 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 6caeff97c2607..907bce430f46d 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 @@ -237,7 +237,6 @@ class GoalPlannerModule : public SceneModuleInterface // collision detector // need to be shared_ptr to be used in planner and goal searcher std::shared_ptr occupancy_grid_map; - std::shared_ptr goal_searcher; const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; } const ModuleStatus & getCurrentStatus() const { return current_status; } @@ -246,7 +245,6 @@ class GoalPlannerModule : public SceneModuleInterface void update( const GoalPlannerParameters & parameters, const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, - const std::shared_ptr goal_searcher_, const autoware::universe_utils::LinearRing2d & vehicle_footprint); private: 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 923fb6ae5bd44..ac4ffe97d6f2d 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 @@ -51,7 +51,8 @@ class GoalSearcher : public GoalSearcherBase private: void countObjectsToAvoid( GoalCandidates & goal_candidates, const PredictedObjects & objects, - const std::shared_ptr & planner_data) const; + const std::shared_ptr & planner_data, + const Pose & reference_goal_pose) const; void createAreaPolygons( std::vector original_search_poses, const std::shared_ptr & planner_data); 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 b7d6ffe0710fe..23a51d1f8c86a 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 @@ -43,15 +43,9 @@ using GoalCandidates = std::vector; class GoalSearcherBase { public: - explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; } + explicit GoalSearcherBase(const GoalPlannerParameters & parameters) : parameters_{parameters} {} virtual ~GoalSearcherBase() = default; - void setReferenceGoal(const Pose & reference_goal_pose) - { - reference_goal_pose_ = reference_goal_pose; - } - const Pose & getReferenceGoal() const { return reference_goal_pose_; } - MultiPolygon2d getAreaPolygons() const { return area_polygons_; } virtual GoalCandidates search(const std::shared_ptr & planner_data) = 0; virtual void update( @@ -72,8 +66,7 @@ class GoalSearcherBase const PredictedObjects & objects) const = 0; protected: - GoalPlannerParameters parameters_{}; - Pose reference_goal_pose_{}; + const GoalPlannerParameters parameters_; MultiPolygon2d area_polygons_{}; }; } // namespace autoware::behavior_path_planner 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 ebc3130afe931..64871bd416f70 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 @@ -208,7 +208,6 @@ void GoalPlannerModule::onTimer() std::optional last_previous_module_output_opt{std::nullopt}; std::shared_ptr occupancy_grid_map{nullptr}; std::optional parameters_opt{std::nullopt}; - std::shared_ptr goal_searcher{nullptr}; // begin of critical section { @@ -221,18 +220,17 @@ void GoalPlannerModule::onTimer() last_previous_module_output_opt = gp_planner_data.last_previous_module_output; occupancy_grid_map = gp_planner_data.occupancy_grid_map; parameters_opt = gp_planner_data.parameters; - goal_searcher = gp_planner_data.goal_searcher; } } // end of critical section if ( !local_planner_data || !current_status_opt || !previous_module_output_opt || - !last_previous_module_output_opt || !occupancy_grid_map || !parameters_opt || !goal_searcher) { + !last_previous_module_output_opt || !occupancy_grid_map || !parameters_opt) { RCLCPP_ERROR( getLogger(), "failed to get valid " - "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt/" - "goal_searcher in onTimer"); + "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt " + "in onTimer"); return; } const auto & current_status = current_status_opt.value(); @@ -375,7 +373,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() std::optional current_status_opt{std::nullopt}; std::shared_ptr occupancy_grid_map{nullptr}; std::optional parameters_opt{std::nullopt}; - std::shared_ptr goal_searcher{nullptr}; + std::optional vehicle_footprint_opt{std::nullopt}; // begin of critical section { @@ -386,20 +384,21 @@ void GoalPlannerModule::onFreespaceParkingTimer() current_status_opt = gp_planner_data.current_status; occupancy_grid_map = gp_planner_data.occupancy_grid_map; parameters_opt = gp_planner_data.parameters; - goal_searcher = gp_planner_data.goal_searcher; + vehicle_footprint_opt = gp_planner_data.vehicle_footprint; } } // end of critical section - if (!local_planner_data || !current_status_opt || !parameters_opt || !goal_searcher) { + if (!local_planner_data || !current_status_opt || !parameters_opt || !vehicle_footprint_opt) { RCLCPP_ERROR( getLogger(), - "failed to get valid planner_data/current_status/parameters/goal_searcher in " + "failed to get valid planner_data/current_status/parameters in " "onFreespaceParkingTimer"); return; } const auto & current_status = current_status_opt.value(); const auto & parameters = parameters_opt.value(); + const auto & vehicle_footprint = vehicle_footprint_opt.value(); if (current_status == ModuleStatus::IDLE) { return; @@ -437,6 +436,8 @@ void GoalPlannerModule::onFreespaceParkingTimer() needPathUpdate( local_planner_data->self_odometry->pose.pose, path_update_duration, modified_goal_opt, parameters)) { + auto goal_searcher = std::make_shared(parameters, vehicle_footprint); + planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); } } @@ -522,7 +523,7 @@ void GoalPlannerModule::updateData() // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) // and if these two coincided, only the reference count is affected gp_planner_data.update( - *parameters_, *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), goal_searcher_, + *parameters_, *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), vehicle_footprint_); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since @@ -585,14 +586,6 @@ void GoalPlannerModule::updateData() // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { - const auto refined_goal = goal_planner_utils::calcRefinedGoal( - planner_data_->route_handler->getOriginalGoalPose(), planner_data_->route_handler, - left_side_parking_, planner_data_->parameters.vehicle_width, - planner_data_->parameters.base_link2front, planner_data_->parameters.base_link2rear, - *parameters_); - if (refined_goal) { - goal_searcher_->setReferenceGoal(refined_goal.value()); - } thread_safe_data_.set_goal_candidates(generateGoalCandidates()); } @@ -2594,15 +2587,13 @@ GoalPlannerModule::GoalPlannerData GoalPlannerModule::GoalPlannerData::clone() c GoalPlannerModule::GoalPlannerData gp_planner_data( planner_data, parameters, last_previous_module_output); gp_planner_data.update( - parameters, planner_data, current_status, previous_module_output, goal_searcher, - vehicle_footprint); + parameters, planner_data, current_status, previous_module_output, vehicle_footprint); return gp_planner_data; } void GoalPlannerModule::GoalPlannerData::update( const GoalPlannerParameters & parameters_, const PlannerData & planner_data_, const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, - const std::shared_ptr goal_searcher_, const autoware::universe_utils::LinearRing2d & vehicle_footprint_) { parameters = parameters_; @@ -2614,11 +2605,6 @@ void GoalPlannerModule::GoalPlannerData::update( last_previous_module_output = previous_module_output; previous_module_output = previous_module_output_; occupancy_grid_map->setMap(*(planner_data.occupancy_grid)); - // to create a deepcopy of GoalPlanner(not GoalPlannerBase), goal_searcher_ is not enough, so - // recreate it here - goal_searcher = std::make_shared(parameters, vehicle_footprint); - // and then copy the reference goal - goal_searcher->setReferenceGoal(goal_searcher_->getReferenceGoal()); } void PathDecisionStateController::transit_state( 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 0bca5ba7a7864..5607729e1030d 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 @@ -102,6 +102,16 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p { GoalCandidates goal_candidates{}; + const auto reference_goal_pose_opt = goal_planner_utils::calcRefinedGoal( + planner_data->route_handler->getOriginalGoalPose(), planner_data->route_handler, + left_side_parking_, planner_data->parameters.vehicle_width, + planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, parameters_); + + if (!reference_goal_pose_opt) { + return goal_candidates; + } + const auto & reference_goal_pose = reference_goal_pose_opt.value(); + const auto & route_handler = planner_data->route_handler; const double forward_length = parameters_.forward_goal_search_length; const double backward_length = parameters_.backward_goal_search_length; @@ -122,7 +132,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( 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 @@ -163,7 +173,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(autoware::motion_utils::calcSignedArcLength( - center_line_path.points, reference_goal_pose_.position, original_search_pose.position)); + center_line_path.points, reference_goal_pose.position, original_search_pose.position)); original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; // search goal_pose in lateral direction @@ -232,7 +242,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p void GoalSearcher::countObjectsToAvoid( GoalCandidates & goal_candidates, const PredictedObjects & objects, - const std::shared_ptr & planner_data) const + const std::shared_ptr & planner_data, const Pose & reference_goal_pose) const { const auto & route_handler = planner_data->route_handler; const double forward_length = parameters_.forward_goal_search_length; @@ -244,7 +254,7 @@ void GoalSearcher::countObjectsToAvoid( *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( @@ -302,8 +312,18 @@ void GoalSearcher::update( const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, const PredictedObjects & objects) const { + const auto refined_goal_opt = goal_planner_utils::calcRefinedGoal( + planner_data->route_handler->getOriginalGoalPose(), planner_data->route_handler, + left_side_parking_, planner_data->parameters.vehicle_width, + planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, parameters_); + + if (!refined_goal_opt) { + return; + } + + const auto & refined_goal = refined_goal_opt.value(); if (parameters_.prioritize_goals_before_objects) { - countObjectsToAvoid(goal_candidates, objects, planner_data); + countObjectsToAvoid(goal_candidates, objects, planner_data, refined_goal); } if (parameters_.goal_priority == "minimum_weighted_distance") {