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.

Sign up for GitHub

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): divide extract_dynamic_object/is_goal_in_lanes util function #10019

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

refactor(goal_planner): divide extract_dynamic_object/is_goal_in_lanes util function #10019

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 9 additions & 0 deletions ...vior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,15 @@ std::optional<Pose> calcRefinedGoal(
std::optional<Pose> calcClosestPose(
const lanelet::ConstLineString3d line, const Point & query_point);

autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects(
const autoware_perception_msgs::msg::PredictedObjects & original_objects,
const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters,
const double vehicle_width);

bool is_goal_reachable_on_path(
const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler,
const bool left_side_parking);

} // namespace autoware::behavior_path_planner::goal_planner_utils

#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_
64 changes: 8 additions & 56 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Open in desktop
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 1866 to 1823, 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.52 to 6.30, 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 @@ -615,29 +615,11 @@
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

// extract static and dynamic objects in extraction polygon for path collision check
const auto & p = parameters_;
const auto & rh = *(planner_data_->route_handler);
const auto objects = *(planner_data_->dynamic_object);
const double vehicle_width = planner_data_->parameters.vehicle_width;
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length);
const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
pull_over_lanes, left_side_parking_, p->detection_bound_offset,
p->margin_from_boundary + p->max_lateral_offset + vehicle_width);
if (objects_extraction_polygon.has_value()) {
debug_data_.objects_extraction_polygon = objects_extraction_polygon.value();
}
PredictedObjects dynamic_target_objects{};
for (const auto & object : objects.objects) {
const auto object_polygon = universe_utils::toPolygon2d(object);
if (
objects_extraction_polygon.has_value() &&
boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
dynamic_target_objects.objects.push_back(object);
}
}
const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects(
*(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_,
planner_data_->parameters.vehicle_width);
const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
dynamic_target_objects, p->th_moving_object_velocity);
dynamic_target_objects, parameters_->th_moving_object_velocity);

Check notice on line 622 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 18 to 14, 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 622 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 (goal_candidates_.empty()) {
Expand Down Expand Up @@ -753,47 +735,17 @@
// check if goal_pose is in current_lanes or neighboring road lanes
const lanelet::ConstLanelets current_lanes =
utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
const auto getNeighboringLane =
[&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true)
: route_handler->getRightLanelet(lane, false, true);
};
lanelet::ConstLanelets goal_check_lanes = current_lanes;
for (const auto & lane : current_lanes) {
auto neighboring_lane = getNeighboringLane(lane);
while (neighboring_lane) {
goal_check_lanes.push_back(neighboring_lane.value());
neighboring_lane = getNeighboringLane(neighboring_lane.value());
}
}
const bool goal_is_in_current_segment_lanes = std::any_of(
goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) {
return lanelet::utils::isInLanelet(goal_pose, lane);
});

// check that goal is in current neighbor shoulder lane
const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
for (const auto & lane : current_lanes) {
const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
: route_handler->getRightShoulderLanelet(lane);
if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) {
return true;
}
}
return false;
});

// if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner,
// because goal arc coordinates cannot be calculated.
if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) {
const bool is_goal_reachable = goal_planner_utils::is_goal_reachable_on_path(
current_lanes, *(planner_data_->route_handler), left_side_parking_);
if (!is_goal_reachable) {
return false;
}

// if goal modification is not allowed
// 1) goal_pose is in current_lanes, plan path to the original fixed goal
// 2) goal_pose is NOT in current_lanes, do not execute goal_planner
if (!utils::isAllowedGoalModification(route_handler)) {
return goal_is_in_current_segment_lanes;
return is_goal_reachable;

Check notice on line 748 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::isExecutionRequested decreases in cyclomatic complexity from 17 to 9, 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 748 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)

✅ No longer an issue: Bumpy Road Ahead 

GoalPlannerModule::isExecutionRequested is no longer above the threshold for logical blocks with deeply nested code. 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.
}

// if goal arc coordinates can be calculated, check if goal is in request_length
Expand Down
65 changes: 65 additions & 0 deletions planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
View file
Open in desktop
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/util.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 4.22 to 4.38, 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.

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession 

The ratio of primitive types in function arguments decreases from 36.00% to 35.51%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// 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 @@ -831,4 +831,69 @@
return closest_pose;
}

autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects(
const autoware_perception_msgs::msg::PredictedObjects & original_objects,
const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters,
const double vehicle_width)
{
const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE;
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
route_handler, left_side_parking, parameters.backward_goal_search_length,
parameters.forward_goal_search_length);
const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
pull_over_lanes, left_side_parking, parameters.detection_bound_offset,
parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width);

PredictedObjects dynamic_target_objects{};
for (const auto & object : original_objects.objects) {
const auto object_polygon = universe_utils::toPolygon2d(object);
if (
objects_extraction_polygon.has_value() &&
boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
dynamic_target_objects.objects.push_back(object);
}
}
return dynamic_target_objects;
}

bool is_goal_reachable_on_path(
const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler,
const bool left_side_parking)
{
const Pose goal_pose = route_handler.getOriginalGoalPose();
const auto getNeighboringLane =
[&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
return left_side_parking ? route_handler.getLeftLanelet(lane, false, true)
: route_handler.getRightLanelet(lane, false, true);
};
lanelet::ConstLanelets goal_check_lanes = current_lanes;
for (const auto & lane : current_lanes) {
auto neighboring_lane = getNeighboringLane(lane);
while (neighboring_lane) {
goal_check_lanes.push_back(neighboring_lane.value());
neighboring_lane = getNeighboringLane(neighboring_lane.value());
}
}
const bool goal_is_in_current_segment_lanes = std::any_of(
goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) {
return lanelet::utils::isInLanelet(goal_pose, lane);
});

// check that goal is in current neighbor shoulder lane
const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
for (const auto & lane : current_lanes) {
const auto shoulder_lane = left_side_parking ? route_handler.getLeftShoulderLanelet(lane)
: route_handler.getRightShoulderLanelet(lane);
if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) {
return true;
}
}
return false;
});

// if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner,
// because goal arc coordinates cannot be calculated.
return goal_is_in_current_segment_lanes || goal_is_in_current_shoulder_lanes;
}

Check warning on line 897 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

is_goal_reachable_on_path has a cyclomatic complexity of 9, 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 897 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

is_goal_reachable_on_path has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested 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.

} // namespace autoware::behavior_path_planner::goal_planner_utils
Loading