Skip to content

Commit

Permalink
feat(goal_planner): move goal_candidates from ThreadSafeData to GoalP…
Browse files Browse the repository at this point in the history 
…lannerData (#9292)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
@soblin
soblin authored Nov 18, 2024
1 parent d813446 commit 2a2bffa
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 35 deletions.
8 changes: 6 additions & 2 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -233,6 +233,8 @@ class GoalPlannerModule : public SceneModuleInterface
ModuleStatus current_status;
BehaviorModuleOutput previous_module_output;
BehaviorModuleOutput last_previous_module_output; //<! previous "previous_module_output"
GoalCandidates goal_candidates; //<! only the positional information of goal_candidates

// collision detector
// need to be shared_ptr to be used in planner and goal searcher
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map;
Expand All @@ -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(
Expand Down Expand Up @@ -309,6 +312,7 @@ class GoalPlannerModule : public SceneModuleInterface

// goal searcher
std::shared_ptr<GoalSearcherBase> 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.
Expand Down Expand Up @@ -409,7 +413,7 @@ class GoalPlannerModule : public SceneModuleInterface
// freespace parking
bool planFreespacePath(
std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<GoalSearcherBase> goal_searcher,
const std::shared_ptr<GoalSearcherBase> goal_searcher, const GoalCandidates & goal_candidates,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map);
bool canReturnToLaneParking(const PullOverContextData & context_data);

Expand Down
6 changes: 1 addition & 5 deletions ...th_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,6 @@ class ThreadSafeData
const std::lock_guard<std::recursive_mutex> 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{};
Expand All @@ -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)

Expand All @@ -125,8 +125,6 @@ class ThreadSafeData
// main <--> lane/freespace
DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
DEFINE_GETTER_WITH_MUTEX(std::optional<rclcpp::Time>, last_path_update_time)

DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates)
DEFINE_SETTER_GETTER_WITH_MUTEX(
utils::path_safety_checker::CollisionCheckDebugMap, collision_check)

Expand All @@ -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<PullOverPath> & arg) { pull_over_path_candidates_ = arg; }
void set_no_lock(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path_no_lock(arg); }
void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); }
Expand All @@ -155,7 +152,6 @@ class ThreadSafeData

std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
std::vector<PullOverPath> pull_over_path_candidates_;
GoalCandidates goal_candidates_{};
std::optional<rclcpp::Time> last_path_update_time_;
std::optional<Pose> closest_start_pose_{};
utils::path_safety_checker::CollisionCheckDebugMap collision_check_{};
Expand Down
61 changes: 33 additions & 28 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,6 +210,7 @@ void GoalPlannerModule::onTimer()
std::optional<BehaviorModuleOutput> last_previous_module_output_opt{std::nullopt};
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map{nullptr};
std::optional<GoalPlannerParameters> parameters_opt{std::nullopt};
std::optional<GoalCandidates> goal_candidates_opt{std::nullopt};

// begin of critical section
{
Expand All @@ -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 "
Expand All @@ -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;
}

Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -376,6 +378,7 @@ void GoalPlannerModule::onFreespaceParkingTimer()
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map{nullptr};
std::optional<GoalPlannerParameters> parameters_opt{std::nullopt};
std::optional<autoware::universe_utils::LinearRing2d> vehicle_footprint_opt{std::nullopt};
std::optional<GoalCandidates> goal_candidates_opt{std::nullopt};

// begin of critical section
{
Expand All @@ -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 "
Expand All @@ -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;
Expand Down Expand Up @@ -440,7 +447,7 @@ void GoalPlannerModule::onFreespaceParkingTimer()
parameters)) {
auto goal_searcher = std::make_shared<GoalSearcher>(parameters, vehicle_footprint);

planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map);
planFreespacePath(local_planner_data, goal_searcher, goal_candidates, occupancy_grid_map);
}
}

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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()) {
Expand Down Expand Up @@ -760,15 +767,13 @@ double GoalPlannerModule::calcModuleRequestLength() const

bool GoalPlannerModule::planFreespacePath(
std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<GoalSearcherBase> goal_searcher,
const std::shared_ptr<GoalSearcherBase> goal_searcher, const GoalCandidates & goal_candidates_arg,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> 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;

Expand Down Expand Up @@ -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);

Expand All @@ -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()
Expand All @@ -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);
}
}

Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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_;
Expand All @@ -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(
Expand Down

0 comments on commit 2a2bffa

Please sign in to comment.