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

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Nov 11, 2024
1 parent 1bd10db commit 8322a6c
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 @@ -234,6 +234,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 @@ -245,7 +247,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 @@ -310,6 +313,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 @@ -410,7 +414,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 @@ -208,6 +208,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 @@ -220,12 +221,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 @@ -237,13 +240,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 @@ -294,8 +298,6 @@ void GoalPlannerModule::onTimer()
return;
}

Check warning on line 300 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

GoalPlannerModule::onTimer already has high cyclomatic complexity, and now it increases in Lines of Code from 138 to 139. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
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 @@ -374,6 +376,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 @@ -385,10 +388,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 @@ -399,6 +405,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();

Check warning on line 408 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Conditional 

GoalPlannerModule::onFreespaceParkingTimer increases from 2 complex conditionals with 5 branches to 2 complex conditionals with 6 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.

if (current_status == ModuleStatus::IDLE) {
return;
Expand Down Expand Up @@ -438,7 +445,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);

Check warning on line 448 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

GoalPlannerModule::onFreespaceParkingTimer increases in cyclomatic complexity from 15 to 16, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
}
}

Expand Down Expand Up @@ -499,6 +506,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 @@ -524,7 +536,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 @@ -584,11 +596,6 @@ void GoalPlannerModule::updateData()
thread_safe_data_.get_pull_over_path_candidates(), prev_decision_state);
auto & ctx_data = context_data_.value();

Check notice on line 598 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

GoalPlannerModule::updateData decreases in cyclomatic complexity from 16 to 15, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 598 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Bumpy Road Ahead 

GoalPlannerModule::updateData decreases from 3 to 2 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
// 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 @@ -758,15 +765,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 @@ -1369,7 +1374,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 @@ -1391,7 +1396,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 @@ -1402,8 +1407,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 @@ -1563,8 +1566,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 @@ -1965,8 +1968,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 @@ -2363,8 +2366,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 @@ -2587,14 +2589,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 @@ -2605,6 +2609,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 8322a6c

Please sign in to comment.