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

fix(avoidance): limit drivable lane only when the ego in on original lane #6349

Merged
merged 1 commit into from
Feb 15, 2024
Merged

fix(avoidance): limit drivable lane only when the ego in on original lane #6349

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
4 changes: 2 additions & 2 deletions planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,8 +155,8 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
// expand drivable lanes
std::for_each(
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
lanelet, planner_data_, avoidance_parameters_, false));
data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes(
lanelet, planner_data_, avoidance_parameters_));
});

// calc drivable bound
Expand Down
9 changes: 7 additions & 2 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj);
double calcShiftLength(
const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);

bool isWithinLanes(
const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data);

bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length);

bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);
Expand Down Expand Up @@ -157,9 +160,11 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const std::shared_ptr<AvoidanceParameters> & parameters,
const double object_check_forward_distance, DebugData & debug);

DrivableLanes generateExpandDrivableLanes(
DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet);

DrivableLanes generateExpandedDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver);
const std::shared_ptr<AvoidanceParameters> & parameters);

double calcDistanceToReturnDeadLine(
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
Expand Down
48 changes: 39 additions & 9 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Open in desktop
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_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 1141 to 1171, 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_avoidance_module/src/scene.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.76 to 4.86, 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 All @@ -21,6 +21,7 @@
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
#include "behavior_path_planner_common/utils/path_utils.hpp"
#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
#include "behavior_path_planner_common/utils/utils.hpp"

#include <lanelet2_extension/utility/message_conversion.hpp>
Expand All @@ -32,8 +33,6 @@
#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
#include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>

#include <boost/geometry/algorithms/centroid.hpp>

#include <algorithm>
#include <limits>
#include <memory>
Expand Down Expand Up @@ -223,24 +222,51 @@
utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);

// expand drivable lanes
const auto has_shift_point = !path_shifter_.getShiftLines().empty();
const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted();
const auto is_within_current_lane =
utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_);
const auto red_signal_lane_itr = std::find_if(
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet);
return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_);
});
const auto not_use_adjacent_lane =
is_within_current_lane && red_signal_lane_itr != data.current_lanelets.end();

std::for_each(
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
lanelet, planner_data_, parameters_, in_avoidance_maneuver));
if (!not_use_adjacent_lane) {
data.drivable_lanes.push_back(
utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
} else if (red_signal_lane_itr->id() != lanelet.id()) {
data.drivable_lanes.push_back(
utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));

Check warning on line 242 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L241-L242 

Added lines #L241 - L242 were not covered by tests
} else {
data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet));

Check warning on line 244 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L244 

Added line #L244 was not covered by tests
}
});

// calc drivable bound
auto tmp_path = getPreviousModuleOutput().path;
const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
const auto use_left_side_hatched_road_marking_area = [&]() {
if (!not_use_adjacent_lane) {
return true;
}
return !planner_data_->route_handler->getRoutingGraphPtr()->left(*red_signal_lane_itr);
}();
const auto use_right_side_hatched_road_marking_area = [&]() {
if (!not_use_adjacent_lane) {
return true;
}
return !planner_data_->route_handler->getRoutingGraphPtr()->right(*red_signal_lane_itr);
}();
data.left_bound = utils::calcBound(
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
use_left_side_hatched_road_marking_area, parameters_->use_intersection_areas,
parameters_->use_freespace_areas, true);
data.right_bound = utils::calcBound(
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
use_right_side_hatched_road_marking_area, parameters_->use_intersection_areas,

Check warning on line 269 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method 

AvoidanceModule::fillFundamentalData has 76 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
parameters_->use_freespace_areas, false);

// reference path
Expand Down Expand Up @@ -939,7 +965,11 @@
{
DrivableAreaInfo current_drivable_area_info;
// generate drivable lanes
current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes;
std::for_each(
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
current_drivable_area_info.drivable_lanes.push_back(
utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
});

Check warning on line 972 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

AvoidanceModule::plan already has high cyclomatic complexity, and now it increases in Lines of Code from 88 to 92. 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 972 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L969-L972 

Added lines #L969 - L972 were not covered by tests
// expand hatched road markings
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;
Expand Down
61 changes: 49 additions & 12 deletions planning/behavior_path_avoidance_module/src/utils.cpp
View file
Open in desktop
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_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 1821 to 1845, 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_avoidance_module/src/utils.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 5.29 to 5.16, 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 @@ -957,6 +957,44 @@
return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0;
}

bool isWithinLanes(
const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data)
{
const auto & rh = planner_data->route_handler;
const auto & ego_pose = planner_data->self_odometry->pose.pose;
const auto transform = tier4_autoware_utils::pose2transform(ego_pose);
const auto footprint = tier4_autoware_utils::transformVector(
planner_data->parameters.vehicle_info.createFootprint(), transform);

lanelet::ConstLanelet closest_lanelet{};
if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) {
return true;
}

lanelet::ConstLanelets concat_lanelets{};

// push previous lanelet
lanelet::ConstLanelets prev_lanelet;
if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) {
concat_lanelets.push_back(prev_lanelet.front());
}

// push nearest lanelet
{
concat_lanelets.push_back(closest_lanelet);
}

// push next lanelet
lanelet::ConstLanelet next_lanelet;
if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) {
concat_lanelets.push_back(next_lanelet);
}

const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets);

return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon());
}

bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length)
{
/**
Expand Down Expand Up @@ -2076,42 +2114,41 @@
return std::make_pair(target_objects, other_objects);
}

DrivableLanes generateExpandDrivableLanes(
DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet)

Check warning on line 2117 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L2117 

Added line #L2117 was not covered by tests
{
DrivableLanes current_drivable_lanes;

Check warning on line 2119 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L2119 

Added line #L2119 was not covered by tests
current_drivable_lanes.left_lane = lanelet;
current_drivable_lanes.right_lane = lanelet;

return current_drivable_lanes;

Check warning on line 2123 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/utils.cpp#L2123 

Added line #L2123 was not covered by tests
}

DrivableLanes generateExpandedDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver)
const std::shared_ptr<AvoidanceParameters> & parameters)
{
const auto & route_handler = planner_data->route_handler;

DrivableLanes current_drivable_lanes;
current_drivable_lanes.left_lane = lanelet;
current_drivable_lanes.right_lane = lanelet;

if (!parameters->use_adjacent_lane) {
return current_drivable_lanes;
}

// 1. get left/right side lanes
const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
const auto next_lanes = route_handler->getNextLanelets(target_lane);
const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
if (is_stop_signal && !in_avoidance_maneuver) {
return;
}
const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
target_lane, parameters->use_opposite_lane, true);
if (!all_left_lanelets.empty()) {
current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet
pushUniqueVector(
current_drivable_lanes.middle_lanes,
lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1));
}
};
const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {

Check notice on line 2151 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method 

generateExpandDrivableLanes is no longer above the threshold for cyclomatic complexity. 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 2151 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Complex Method 

generateExpandedDrivableLanes has a cyclomatic complexity of 24, 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 2151 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

generateExpandDrivableLanes 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 notice on line 2151 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Bumpy Road Ahead 

generateExpandedDrivableLanes has 4 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.

Check notice on line 2151 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Deep, Nested Complexity 

generateExpandDrivableLanes is no longer above the threshold for nested complexity depth. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.

Check notice on line 2151 in planning/behavior_path_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Deep, Nested Complexity 

generateExpandedDrivableLanes has a nested complexity depth of 4, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
const auto next_lanes = route_handler->getNextLanelets(target_lane);
const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
if (is_stop_signal && !in_avoidance_maneuver) {
return;
}
const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
target_lane, parameters->use_opposite_lane, true);
if (!all_right_lanelets.empty()) {
Expand Down
Loading