Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(goal_planner): remove reference_goal_pose getter/setter #9270

Merged
merged 1 commit into from
Nov 13, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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<OccupancyGridBasedCollisionDetector> occupancy_grid_map;
std::shared_ptr<GoalSearcherBase> goal_searcher;

const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; }
const ModuleStatus & getCurrentStatus() const { return current_status; }
Expand All @@ -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<GoalSearcherBase> goal_searcher_,
const autoware::universe_utils::LinearRing2d & vehicle_footprint);

private:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,8 @@ class GoalSearcher : public GoalSearcherBase
private:
void countObjectsToAvoid(
GoalCandidates & goal_candidates, const PredictedObjects & objects,
const std::shared_ptr<const PlannerData> & planner_data) const;
const std::shared_ptr<const PlannerData> & planner_data,
const Pose & reference_goal_pose) const;
void createAreaPolygons(
std::vector<Pose> original_search_poses,
const std::shared_ptr<const PlannerData> & planner_data);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,15 +43,9 @@
class GoalSearcherBase
{
public:
explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; }
explicit GoalSearcherBase(const GoalPlannerParameters & parameters) : parameters_{parameters} {}

Check warning on line 46 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp#L46

Added line #L46 was not covered by tests
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<const PlannerData> & planner_data) = 0;
virtual void update(
Expand All @@ -72,8 +66,7 @@
const PredictedObjects & objects) const = 0;

protected:
GoalPlannerParameters parameters_{};
Pose reference_goal_pose_{};
const GoalPlannerParameters parameters_;
MultiPolygon2d area_polygons_{};
};
} // namespace autoware::behavior_path_planner
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 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: Lines of Code in a Single File

The lines of code decreases from 2018 to 2006, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 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: Overall Code Complexity

The mean cyclomatic complexity decreases from 6.03 to 6.00, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -207,32 +207,30 @@
std::optional<BehaviorModuleOutput> previous_module_output_opt{std::nullopt};
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::shared_ptr<GoalSearcherBase> goal_searcher{nullptr};

// begin of critical section
{
std::lock_guard<std::mutex> guard(gp_planner_data_mutex_);
if (gp_planner_data_) {
const auto & gp_planner_data = gp_planner_data_.value();
local_planner_data = std::make_shared<const PlannerData>(gp_planner_data.planner_data);
current_status_opt = gp_planner_data.current_status;
previous_module_output_opt = gp_planner_data.previous_module_output;
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) {

Check notice on line 228 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 Conditional

GoalPlannerModule::onTimer decreases from 1 complex conditionals with 6 branches to 1 complex conditionals with 5 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.
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");

Check notice on line 233 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::onTimer decreases in cyclomatic complexity from 36 to 35, 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.
return;
}
const auto & current_status = current_status_opt.value();
Expand Down Expand Up @@ -375,7 +373,7 @@
std::optional<ModuleStatus> current_status_opt{std::nullopt};
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map{nullptr};
std::optional<GoalPlannerParameters> parameters_opt{std::nullopt};
std::shared_ptr<GoalSearcherBase> goal_searcher{nullptr};
std::optional<autoware::universe_utils::LinearRing2d> vehicle_footprint_opt{std::nullopt};

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

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp#L376

Added line #L376 was not covered by tests

// begin of critical section
{
Expand All @@ -386,20 +384,21 @@
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;
Expand Down Expand Up @@ -437,6 +436,8 @@
needPathUpdate(
local_planner_data->self_odometry->pose.pose, path_update_duration, modified_goal_opt,
parameters)) {
auto goal_searcher = std::make_shared<GoalSearcher>(parameters, vehicle_footprint);

Comment on lines +439 to +440
Copy link
Contributor

@kosuke55 kosuke55 Nov 12, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
auto goal_searcher = std::make_shared<GoalSearcher>(parameters, vehicle_footprint);
const auto goal_searcher = std::make_shared<GoalSearcher>(parameters, vehicle_footprint);

?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since it is now only used within planFreespacePath(), it may be ok to generate it within that function.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(update() does not depend on search(), and there is no problem if we pass goal candidates. It seems like it would be ok to make update() a static method.)

planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map);
}
}
Expand Down Expand Up @@ -522,77 +523,69 @@
// **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
// behavior_path_planner::run() updates
// planner_data_->route_handler->lanelet_map_ptr_/routing_graph_ptr_ especially, we also have
// to copy route_handler as value to use lanelet_map_ptr_/routing_graph_ptr_ thread-safely in
// onTimer/onFreespaceParkingTimer
// TODO(Mamoru Sobue): If the copy of RouteHandler.road_lanelets/shoulder_lanelets is not
// lightweight, we should update gp_planner_data_.route_handler only when
// `planner_data_.is_route_handler_updated` variable is set true by behavior_path_planner
// (although this flag is not implemented yet). In that case, gp_planner_data members except
// for route_handler should be copied from planner_data_

// GoalPlannerModule::occupancy_grid_map_ and gp_planner_data.occupancy_grid_map share the
// ownership, and gp_planner_data.occupancy_grid_map maybe also shared by the local
// planner_data on onFreespaceParkingTimer thread local memory space. So following operation
// is thread-safe because gp_planner_data.occupancy_grid_map is only re-pointed here and its
// prior resource is still owned by the onFreespaceParkingTimer thread locally.
occupancy_grid_map_ = gp_planner_data.occupancy_grid_map;
}
// end of critical section

if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) {
return;
}

resetPathCandidate();
resetPathReference();
path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);

const bool found_pull_over_path = thread_safe_data_.foundPullOverPath();
std::optional<PullOverPath> pull_over_path_recv =
found_pull_over_path ? std::make_optional<PullOverPath>(*thread_safe_data_.get_pull_over_path())
: std::nullopt;

const auto modified_goal_pose = [&]() -> std::optional<GoalCandidate> {
if (!pull_over_path_recv) {
return std::nullopt;
}
const auto & pull_over_path = pull_over_path_recv.value();
return pull_over_path.modified_goal();
}();

// save "old" state
const auto prev_decision_state = path_decision_controller_.get_current_state();
const bool is_current_safe = isSafePath(
planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_,
ego_predicted_path_params_, objects_filtering_params_, safety_check_params_);
// update to latest state
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);

context_data_.emplace(
path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
dynamic_target_objects, std::move(pull_over_path_recv),
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()) {

Check notice on line 588 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 588 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.
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());
}

Expand Down Expand Up @@ -2594,31 +2587,24 @@
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<GoalSearcherBase> goal_searcher_,
const autoware::universe_utils::LinearRing2d & vehicle_footprint_)
{
parameters = parameters_;
vehicle_footprint = vehicle_footprint_;

planner_data = planner_data_;
planner_data.route_handler = std::make_shared<RouteHandler>(*(planner_data_.route_handler));
current_status = current_status_;
last_previous_module_output = previous_module_output;
previous_module_output = previous_module_output_;
occupancy_grid_map->setMap(*(planner_data.occupancy_grid));

Check notice on line 2607 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: Excess Number of Function Arguments

GoalPlannerModule::GoalPlannerData::update decreases from 6 to 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
// to create a deepcopy of GoalPlanner(not GoalPlannerBase), goal_searcher_ is not enough, so
// recreate it here
goal_searcher = std::make_shared<GoalSearcher>(parameters, vehicle_footprint);
// and then copy the reference goal
goal_searcher->setReferenceGoal(goal_searcher_->getReferenceGoal());
}

void PathDecisionStateController::transit_state(
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity

The mean cyclomatic complexity increases from 5.64 to 5.79, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -102,6 +102,16 @@
{
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_);

Check warning on line 108 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp#L107-L108

Added lines #L107 - L108 were not covered by tests

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;
Expand All @@ -122,7 +132,7 @@
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
Expand Down Expand Up @@ -163,7 +173,7 @@
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));

Check warning on line 176 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

GoalSearcher::search increases in cyclomatic complexity from 21 to 22, 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.
original_search_poses.push_back(original_search_pose); // for createAreaPolygon
Pose search_pose{};
// search goal_pose in lateral direction
Expand Down Expand Up @@ -232,7 +242,7 @@

void GoalSearcher::countObjectsToAvoid(
GoalCandidates & goal_candidates, const PredictedObjects & objects,
const std::shared_ptr<const PlannerData> & planner_data) const
const std::shared_ptr<const PlannerData> & 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;
Expand All @@ -244,7 +254,7 @@
*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(
Expand Down Expand Up @@ -302,8 +312,18 @@
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & 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_);

Check warning on line 318 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp#L317-L318

Added lines #L317 - L318 were not covered by tests

if (!refined_goal_opt) {
return;

Check warning on line 321 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp#L321

Added line #L321 was not covered by tests
}

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);

Check warning on line 326 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

GoalSearcher::update increases in cyclomatic complexity from 9 to 10, 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 warning on line 326 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp#L326

Added line #L326 was not covered by tests
}

if (parameters_.goal_priority == "minimum_weighted_distance") {
Expand Down
Loading