Skip to content

Commit

Permalink
refactor(goal_planner): remove reference_goal_pose getter/setter
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Nov 8, 2024
1 parent 488df21 commit 647dc0b
Show file tree
Hide file tree
Showing 5 changed files with 41 additions and 43 deletions.
2 changes: 0 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 @@ -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
3 changes: 2 additions & 1 deletion ..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
View file
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
11 changes: 2 additions & 9 deletions ..._planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,15 +43,9 @@ using GoalCandidates = std::vector<GoalCandidate>;
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<const PlannerData> & planner_data) = 0;
virtual void update(
Expand All @@ -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
Expand Down
38 changes: 12 additions & 26 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,7 +208,6 @@ 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::shared_ptr<GoalSearcherBase> goal_searcher{nullptr};

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

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 @@ void GoalPlannerModule::onFreespaceParkingTimer()
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};

// begin of critical section
{
Expand All @@ -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;
Expand Down Expand Up @@ -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<GoalSearcher>(parameters, vehicle_footprint);

planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map);
}
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -585,14 +586,6 @@ void GoalPlannerModule::updateData()

// 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 @@ -2574,15 +2567,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<GoalSearcherBase> goal_searcher_,
const autoware::universe_utils::LinearRing2d & vehicle_footprint_)
{
parameters = parameters_;
Expand All @@ -2594,11 +2585,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));

Check notice on line 2587 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
30 changes: 25 additions & 5 deletions ...ng/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,16 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & 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;
Expand All @@ -122,7 +132,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & 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
Expand Down Expand Up @@ -163,7 +173,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & 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));

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 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p

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 @@ 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(
Expand Down Expand Up @@ -302,8 +312,18 @@ void GoalSearcher::update(
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_);

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

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.
}

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

0 comments on commit 647dc0b

Please sign in to comment.