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

fix(route_handler): route handler overlap removal is too conservative #7156

Merged
Show file tree
Hide file tree
Changes from 13 commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
b5b7169
add flag to enable/disable loop check in getLaneletSequence functions
mkquda May 22, 2024
342005f
implement function to get closest route lanelet based on previous clo…
mkquda May 22, 2024
23ad54d
refactor DefaultPlanner::plan function
mkquda May 24, 2024
f678d7c
modify loop check logic in getLaneletSequenceUpTo function
mkquda May 24, 2024
8f8555a
improve logic in isEgoOutOfRoute function
mkquda May 27, 2024
708d538
fix format
mkquda May 27, 2024
297146a
check if prev lanelet is a goal lanelet in getLaneletSequenceUpTo fun…
mkquda May 27, 2024
2ff00af
separate function to update current route lanelet in planner manager
mkquda May 28, 2024
553c77a
rename function and add docstring
mkquda May 29, 2024
c7f36c2
modify functions extendNextLane and extendPrevLane to account for ove…
mkquda May 31, 2024
bf76077
refactor function getClosestRouteLaneletFromLanelet
mkquda Jun 3, 2024
11a4478
add route handler unit tests for overlapping route case
mkquda Jun 3, 2024
14553d1
fix function getClosestRouteLaneletFromLanelet
mkquda Jun 3, 2024
1477895
Merge branch 'awf-latest' into RT1-6335-route-handler-overlap-removal…
mkquda Jun 11, 2024
98bf31f
Merge branch 'awf-latest' into RT1-6335-route-handler-overlap-removal…
mkquda Jun 11, 2024
55cc848
Merge branch 'awf-latest' into RT1-6335-route-handler-overlap-removal…
mkquda Jun 11, 2024
cf5135e
Merge branch 'awf-latest' into RT1-6335-route-handler-overlap-removal…
mkquda Jun 12, 2024
fdd1a61
Merge branch 'main' into RT1-6335-route-handler-overlap-removal-is-to…
mkquda Jun 12, 2024
b978e7f
Merge branch 'main' into RT1-6335-route-handler-overlap-removal-is-to…
mkquda Jun 12, 2024
653e6c3
format fix
mkquda Jun 12, 2024
ec1f99d
move test map to autoware_test_utils
mkquda Jun 13, 2024
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 @@ -231,7 +231,8 @@ bool isInLaneletWithYawThreshold(
const double radius = 0.0);

bool isEgoOutOfRoute(
const Pose & self_pose, const std::optional<PoseWithUuidStamped> & modified_goal,
const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
const std::optional<PoseWithUuidStamped> & modified_goal,
const std::shared_ptr<RouteHandler> & route_handler);

bool isEgoWithinOriginalLane(
Expand Down
81 changes: 40 additions & 41 deletions planning/autoware_behavior_path_planner_common/src/utils/utils.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/autoware_behavior_path_planner_common/src/utils/utils.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 1289 to 1285, 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/autoware_behavior_path_planner_common/src/utils/utils.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.39 to 4.49, 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/autoware_behavior_path_planner_common/src/utils/utils.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 30.99% to 30.81%, 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 @@ -489,7 +489,8 @@
}

bool isEgoOutOfRoute(
const Pose & self_pose, const std::optional<PoseWithUuidStamped> & modified_goal,
const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
const std::optional<PoseWithUuidStamped> & modified_goal,
const std::shared_ptr<RouteHandler> & route_handler)
{
const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
Expand All @@ -509,12 +510,16 @@

// If ego vehicle is over goal on goal lane, return true
const double yaw_threshold = tier4_autoware_utils::deg2rad(90);
if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) {
if (
closest_road_lane.id() == goal_lane.id() &&
isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) {
constexpr double buffer = 1.0;
const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose);
const auto goal_arc_coord =
lanelet::utils::getArcCoordinates({goal_lane}, route_handler->getGoalPose());
if (ego_arc_coord.length > goal_arc_coord.length + buffer) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("behavior_path_planner").get_child("util"), "ego pose is beyond goal");
return true;
} else {
return false;
Expand All @@ -526,14 +531,6 @@
const bool is_in_shoulder_lane = !route_handler->getShoulderLaneletsAtPose(self_pose).empty();
// Check if ego vehicle is in road lane
const bool is_in_road_lane = std::invoke([&]() {
lanelet::ConstLanelet closest_road_lane;
if (!route_handler->getClosestLaneletWithinRoute(self_pose, &closest_road_lane)) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("behavior_path_planner").get_child("util"),
"cannot find closest road lanelet");
return false;
}

if (lanelet::utils::isInLanelet(self_pose, closest_road_lane)) {
return true;
}
Expand All @@ -548,6 +545,7 @@

return false;
});

if (!is_in_shoulder_lane && !is_in_road_lane) {
return true;
}
Expand Down Expand Up @@ -1308,26 +1306,26 @@
{
if (lanes.empty()) return lanes;

auto extended_lanes = lanes;
const auto next_lanes = route_handler->getNextLanelets(lanes.back());
if (next_lanes.empty()) return lanes;

// Add next lane
const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back());
if (!next_lanes.empty()) {
std::optional<lanelet::ConstLanelet> target_next_lane;
if (!only_in_route) {
target_next_lane = next_lanes.front();
}
// use the next lane in route if it exists
for (const auto & next_lane : next_lanes) {
if (route_handler->isRouteLanelet(next_lane)) {
target_next_lane = next_lane;
}
}
if (target_next_lane) {
extended_lanes.push_back(*target_next_lane);
auto extended_lanes = lanes;
std::optional<lanelet::ConstLanelet> target_next_lane;
for (const auto & next_lane : next_lanes) {
// skip overlapping lanes
if (next_lane.id() == lanes.front().id()) continue;

// if route lane, set target and break
if (route_handler->isRouteLanelet(next_lane)) {
target_next_lane = next_lane;
break;
}
if (!only_in_route && !target_next_lane) target_next_lane = next_lane;
}
if (target_next_lane) {
extended_lanes.push_back(*target_next_lane);
}

Check notice on line 1328 in planning/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead

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

Check warning on line 1328 in planning/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

extendNextLane has a cyclomatic complexity of 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.

return extended_lanes;
}

Expand All @@ -1337,24 +1335,25 @@
{
if (lanes.empty()) return lanes;

auto extended_lanes = lanes;
const auto prev_lanes = route_handler->getPreviousLanelets(lanes.front());
if (prev_lanes.empty()) return lanes;

// Add previous lane
const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front());
if (!prev_lanes.empty()) {
std::optional<lanelet::ConstLanelet> target_prev_lane;
if (!only_in_route) {
target_prev_lane = prev_lanes.front();
}
// use the previous lane in route if it exists
for (const auto & prev_lane : prev_lanes) {
if (route_handler->isRouteLanelet(prev_lane)) {
target_prev_lane = prev_lane;
}
}
if (target_prev_lane) {
extended_lanes.insert(extended_lanes.begin(), *target_prev_lane);
auto extended_lanes = lanes;
std::optional<lanelet::ConstLanelet> target_prev_lane;
for (const auto & prev_lane : prev_lanes) {
// skip overlapping lanes
if (prev_lane.id() == lanes.back().id()) continue;

// if route lane, set target and break
if (route_handler->isRouteLanelet(prev_lane)) {
target_prev_lane = prev_lane;
break;
}
if (!only_in_route && !target_prev_lane) target_prev_lane = prev_lane;
}
if (target_prev_lane) {
extended_lanes.insert(extended_lanes.begin(), *target_prev_lane);

Check notice on line 1356 in planning/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead

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

Check warning on line 1356 in planning/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

extendPrevLane has a cyclomatic complexity of 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.
}
return extended_lanes;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,12 @@ class PlannerManager
return result;
}

/**
* @brief find and set the closest lanelet within the route to current route lanelet
* @param planner data.
*/
void updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data);

void generateCombinedDrivableArea(
BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const;

Expand All @@ -307,12 +313,12 @@ class PlannerManager
* @param planner data.
* @return reference path.
*/
BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data);
BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data) const;

/**
* @brief publish the root reference path and current route lanelet
*/
void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path);
void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) const;

/**
* @brief stop and unregister the module from manager.
Expand Down
24 changes: 20 additions & 4 deletions planning/behavior_path_planner/src/planner_manager.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/src/planner_manager.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 7.19 to 6.88, 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 @@ -111,8 +111,11 @@
const bool is_any_module_running =
is_any_approved_module_running || is_any_candidate_module_running_or_idle;

updateCurrentRouteLanelet(data);

const bool is_out_of_route = utils::isEgoOutOfRoute(
data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler);
data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal,
data->route_handler);

Check warning on line 118 in planning/behavior_path_planner/src/planner_manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

PlannerManager::run already has high cyclomatic complexity, and now it increases in Lines of Code from 72 to 74. 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 (!is_any_module_running && is_out_of_route) {
BehaviorModuleOutput output = utils::createGoalAroundPath(data);
Expand Down Expand Up @@ -414,7 +417,7 @@
return output;
}

BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr<PlannerData> & data)
void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data)
{
const auto & route_handler = data->route_handler;
const auto & pose = data->self_odometry->pose.pose;
Expand All @@ -424,10 +427,18 @@
const auto backward_length =
std::max(p.backward_path_length, p.backward_path_length + extra_margin);

lanelet::ConstLanelet closest_lane{};

if (route_handler->getClosestRouteLaneletFromLanelet(
pose, current_route_lanelet_.value(), &closest_lane, p.ego_nearest_dist_threshold,
p.ego_nearest_yaw_threshold)) {
current_route_lanelet_ = closest_lane;
return;
}

const auto lanelet_sequence = route_handler->getLaneletSequence(
current_route_lanelet_.value(), pose, backward_length, p.forward_path_length);

lanelet::ConstLanelet closest_lane{};
const auto could_calculate_closest_lanelet =
lanelet::utils::query::getClosestLaneletWithConstrains(
lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold,
Expand All @@ -438,13 +449,18 @@
current_route_lanelet_ = closest_lane;
else
resetCurrentRouteLanelet(data);
}

BehaviorModuleOutput PlannerManager::getReferencePath(
const std::shared_ptr<PlannerData> & data) const
{
const auto reference_path = utils::getReferencePath(*current_route_lanelet_, data);
publishDebugRootReferencePath(reference_path);
return reference_path;
}

void PlannerManager::publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path)
void PlannerManager::publishDebugRootReferencePath(
const BehaviorModuleOutput & reference_path) const
{
using visualization_msgs::msg::Marker;
MarkerArray array;
Expand Down
21 changes: 3 additions & 18 deletions planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,20 +39,6 @@
namespace
{
using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
RouteSections combine_consecutive_route_sections(
const RouteSections & route_sections1, const RouteSections & route_sections2)
{
RouteSections route_sections;
route_sections.reserve(route_sections1.size() + route_sections2.size());
if (!route_sections1.empty()) {
// remove end route section because it is overlapping with first one in next route_section
route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1);
}
if (!route_sections2.empty()) {
route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end());
}
return route_sections;
}

bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point)
{
Expand Down Expand Up @@ -422,15 +408,14 @@
RCLCPP_WARN(logger, "Failed to plan route.");
return route_msg;
}

for (const auto & lane : path_lanelets) {
if (!all_route_lanelets.empty() && lane.id() == all_route_lanelets.back().id()) continue;
all_route_lanelets.push_back(lane);
}
// create local route sections
route_handler_.setRouteLanelets(path_lanelets);
const auto local_route_sections = route_handler_.createMapSegments(path_lanelets);
route_sections = combine_consecutive_route_sections(route_sections, local_route_sections);
}
route_handler_.setRouteLanelets(all_route_lanelets);
route_sections = route_handler_.createMapSegments(all_route_lanelets);

Check warning on line 418 in planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

DefaultPlanner::plan has a cyclomatic complexity of 11, 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 418 in planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead

DefaultPlanner::plan 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.

auto goal_pose = points.back();
if (param_.enable_correct_goal_pose) {
Expand Down
Loading
Loading