From 2a2bffa96c8706b490b87dafaab57f4778566acf Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 18 Nov 2024 20:38:52 +0900 Subject: [PATCH] feat(goal_planner): move goal_candidates from ThreadSafeData to GoalPlannerData (#9292) Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 8 ++- .../thread_data.hpp | 6 +- .../src/goal_planner_module.cpp | 61 ++++++++++--------- 3 files changed, 40 insertions(+), 35 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 e8c82c0a4600b..2edd0714eceff 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 @@ -233,6 +233,8 @@ class GoalPlannerModule : public SceneModuleInterface ModuleStatus current_status; BehaviorModuleOutput previous_module_output; BehaviorModuleOutput last_previous_module_output; // occupancy_grid_map; @@ -244,7 +246,8 @@ class GoalPlannerModule : public SceneModuleInterface void update( const GoalPlannerParameters & parameters, const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, - const autoware::universe_utils::LinearRing2d & vehicle_footprint); + const autoware::universe_utils::LinearRing2d & vehicle_footprint, + const GoalCandidates & goal_candidates); private: void initializeOccupancyGridMap( @@ -309,6 +312,7 @@ class GoalPlannerModule : public SceneModuleInterface // goal searcher std::shared_ptr goal_searcher_; + GoalCandidates goal_candidates_{}; // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on // onFreespaceParkingTimer thread storage may point to while calculation. @@ -409,7 +413,7 @@ class GoalPlannerModule : public SceneModuleInterface // freespace parking bool planFreespacePath( std::shared_ptr planner_data, - const std::shared_ptr goal_searcher, + const std::shared_ptr goal_searcher, const GoalCandidates & goal_candidates, const std::shared_ptr occupancy_grid_map); bool canReturnToLaneParking(const PullOverContextData & context_data); 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 a0d5d40d8c8d5..9df48fb5f0a5f 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 @@ -106,7 +106,6 @@ class ThreadSafeData const std::lock_guard lock(mutex_); pull_over_path_ = nullptr; pull_over_path_candidates_.clear(); - goal_candidates_.clear(); last_path_update_time_ = std::nullopt; closest_start_pose_ = std::nullopt; prev_data_ = PathDecisionState{}; @@ -115,6 +114,7 @@ class ThreadSafeData // main --> lane/freespace DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) + // main --> lane DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) @@ -125,8 +125,6 @@ class ThreadSafeData // main <--> lane/freespace DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - - DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) DEFINE_SETTER_GETTER_WITH_MUTEX( utils::path_safety_checker::CollisionCheckDebugMap, collision_check) @@ -144,7 +142,6 @@ class ThreadSafeData last_path_update_time_ = clock_->now(); } - void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } @@ -155,7 +152,6 @@ class ThreadSafeData std::shared_ptr pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; - GoalCandidates goal_candidates_{}; std::optional last_path_update_time_; std::optional closest_start_pose_{}; utils::path_safety_checker::CollisionCheckDebugMap collision_check_{}; 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 4204d8177c5b1..f8a35134e0407 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 @@ -210,6 +210,7 @@ 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::optional goal_candidates_opt{std::nullopt}; // begin of critical section { @@ -222,12 +223,14 @@ 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_candidates_opt = gp_planner_data.goal_candidates; } } // 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) { + !last_previous_module_output_opt || !occupancy_grid_map || !parameters_opt || + !goal_candidates_opt) { RCLCPP_ERROR( getLogger(), "failed to get valid " @@ -239,13 +242,14 @@ void GoalPlannerModule::onTimer() const auto & previous_module_output = previous_module_output_opt.value(); const auto & last_previous_module_output = last_previous_module_output_opt.value(); const auto & parameters = parameters_opt.value(); + const auto & goal_candidates = goal_candidates_opt.value(); if (current_status == ModuleStatus::IDLE) { return; } // goals are not yet available. - if (thread_safe_data_.get_goal_candidates().empty()) { + if (goal_candidates.empty()) { return; } @@ -296,8 +300,6 @@ void GoalPlannerModule::onTimer() return; } - const auto goal_candidates = thread_safe_data_.get_goal_candidates(); - // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( local_planner_data, parameters.backward_goal_search_length, @@ -376,6 +378,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() std::shared_ptr occupancy_grid_map{nullptr}; std::optional parameters_opt{std::nullopt}; std::optional vehicle_footprint_opt{std::nullopt}; + std::optional goal_candidates_opt{std::nullopt}; // begin of critical section { @@ -387,10 +390,13 @@ void GoalPlannerModule::onFreespaceParkingTimer() occupancy_grid_map = gp_planner_data.occupancy_grid_map; parameters_opt = gp_planner_data.parameters; vehicle_footprint_opt = gp_planner_data.vehicle_footprint; + goal_candidates_opt = gp_planner_data.goal_candidates; } } // end of critical section - if (!local_planner_data || !current_status_opt || !parameters_opt || !vehicle_footprint_opt) { + if ( + !local_planner_data || !current_status_opt || !parameters_opt || !vehicle_footprint_opt || + !goal_candidates_opt) { RCLCPP_ERROR( getLogger(), "failed to get valid planner_data/current_status/parameters in " @@ -401,6 +407,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() const auto & current_status = current_status_opt.value(); const auto & parameters = parameters_opt.value(); const auto & vehicle_footprint = vehicle_footprint_opt.value(); + const auto & goal_candidates = goal_candidates_opt.value(); if (current_status == ModuleStatus::IDLE) { return; @@ -440,7 +447,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() parameters)) { auto goal_searcher = std::make_shared(parameters, vehicle_footprint); - planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); + planFreespacePath(local_planner_data, goal_searcher, goal_candidates, occupancy_grid_map); } } @@ -501,6 +508,11 @@ void GoalPlannerModule::updateData() thread_safe_data_.set_static_target_objects(static_target_objects); thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects); + // update goal searcher and generate goal candidates + if (goal_candidates_.empty()) { + goal_candidates_ = generateGoalCandidates(); + } + // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then @@ -526,7 +538,7 @@ void GoalPlannerModule::updateData() // and if these two coincided, only the reference count is affected gp_planner_data.update( *parameters_, *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), - vehicle_footprint_); + vehicle_footprint_, goal_candidates_); // 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 // behavior_path_planner::run() updates @@ -586,11 +598,6 @@ void GoalPlannerModule::updateData() thread_safe_data_.get_pull_over_path_candidates(), prev_decision_state); auto & ctx_data = context_data_.value(); - // update goal searcher and generate goal candidates - if (thread_safe_data_.get_goal_candidates().empty()) { - thread_safe_data_.set_goal_candidates(generateGoalCandidates()); - } - thread_safe_data_.set_prev_data(path_decision_controller_.get_current_state()); if (!isActivated()) { @@ -760,15 +767,13 @@ double GoalPlannerModule::calcModuleRequestLength() const bool GoalPlannerModule::planFreespacePath( std::shared_ptr planner_data, - const std::shared_ptr goal_searcher, + const std::shared_ptr goal_searcher, const GoalCandidates & goal_candidates_arg, const std::shared_ptr occupancy_grid_map) { - GoalCandidates goal_candidates{}; - goal_candidates = thread_safe_data_.get_goal_candidates(); + auto goal_candidates = goal_candidates_arg; goal_searcher->update( goal_candidates, occupancy_grid_map, planner_data, thread_safe_data_.get_static_target_objects()); - thread_safe_data_.set_goal_candidates(goal_candidates); debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); debug_data_.freespace_planner.is_planning = true; @@ -1370,7 +1375,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( thread_safe_data_.clearPullOverPath(); // update goal candidates - auto goal_candidates = thread_safe_data_.get_goal_candidates(); + auto goal_candidates = goal_candidates_; goal_searcher_->update( goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); @@ -1392,7 +1397,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( * As the next action item, only set this selected pull_over_path to only * FreespaceThreadSafeData. */ - thread_safe_data_.set(goal_candidates, pull_over_path); + thread_safe_data_.set(pull_over_path); if (pull_over_path_with_velocity_opt) { auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); // copy the path for later setOutput() @@ -1403,8 +1408,6 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( RCLCPP_DEBUG( getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), pull_over_path.modified_goal().id); - } else { - thread_safe_data_.set(goal_candidates); } } @@ -1566,8 +1569,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath(const PullOverContextData & c // calculate search start offset pose from the closest goal candidate pose with // 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( - thread_safe_data_.get_goal_candidates(), planner_data_); + const auto closest_goal_candidate = + 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_); @@ -1968,8 +1971,8 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // decelerate before the search area start - const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( - thread_safe_data_.get_goal_candidates(), planner_data_); + const auto closest_goal_candidate = + 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_); @@ -2371,8 +2374,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - const auto goal_candidates = thread_safe_data_.get_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( @@ -2595,14 +2597,16 @@ 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, vehicle_footprint); + parameters, planner_data, current_status, previous_module_output, vehicle_footprint, + goal_candidates); 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 autoware::universe_utils::LinearRing2d & vehicle_footprint_) + const autoware::universe_utils::LinearRing2d & vehicle_footprint_, + const GoalCandidates & goal_candidates_) { parameters = parameters_; vehicle_footprint = vehicle_footprint_; @@ -2613,6 +2617,7 @@ 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)); + goal_candidates = goal_candidates_; } void PathDecisionStateController::transit_state(