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(avoidance): the module ignored merging objects unexpectedly #6601

Merged
merged 6 commits into from
Mar 15, 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 @@ -132,7 +132,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
}

{
const std::string ns = "avoidance.target_filtering.force_avoidance.";
const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
p.enable_force_avoidance_for_stopped_vehicle =
getOrDeclareParameter<bool>(*node, ns + "enable");
p.threshold_time_force_avoidance_for_stopped_vehicle =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,7 @@
backward_distance: 10.0 # [m]

# params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
force_avoidance:
avoidance_for_ambiguous_vehicle:
enable: false # [-]
time_threshold: 1.0 # [s]
distance_threshold: 1.0 # [m]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -384,7 +384,8 @@ struct ObjectData // avoidance target
rclcpp::Time last_move;
double stop_time{0.0};

// store the information of the lanelet which the object's overhang is currently occupying
// It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to
// check whether the object is on the ego lane.
lanelet::ConstLanelet overhang_lanelet;

// the position at the detected moment
Expand Down Expand Up @@ -420,6 +421,12 @@ struct ObjectData // avoidance target
// is parked vehicle on road shoulder
bool is_parked{false};

// is driving on ego current lane
bool is_on_ego_lane{false};

// is ambiguous stopped vehicle.
bool is_ambiguous{false};

// object direction.
Direction direction{Direction::NONE};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include "behavior_path_avoidance_module/data_structs.hpp"
#include "behavior_path_avoidance_module/utils.hpp"
#include "behavior_path_planner_common/utils/utils.hpp"

#include <motion_utils/distance/distance.hpp>

Expand Down Expand Up @@ -134,6 +135,24 @@
shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed());
}

double getFrontConstantDistance(const ObjectData & object) const

Check warning on line 138 in planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp#L138

Added line #L138 was not covered by tests
{
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);

Check warning on line 141 in planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp#L140-L141

Added lines #L140 - L141 were not covered by tests
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
: 0.0;
return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;

Check warning on line 145 in planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp#L145

Added line #L145 was not covered by tests
}

double getRearConstantDistance(const ObjectData & object) const

Check warning on line 148 in planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp#L148

Added line #L148 was not covered by tests
{
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear +
object.length;

Check warning on line 153 in planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp#L150-L153

Added lines #L150 - L153 were not covered by tests
}

double getEgoShift() const
{
validate();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
}

{
const std::string ns = "avoidance.target_filtering.force_avoidance.";
const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
p.enable_force_avoidance_for_stopped_vehicle =
getOrDeclareParameter<bool>(*node, ns + "enable");
p.threshold_time_force_avoidance_for_stopped_vehicle =
Expand Down
25 changes: 23 additions & 2 deletions planning/behavior_path_avoidance_module/src/debug.cpp
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_avoidance_module/src/debug.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession

The ratio of primitive types in function arguments increases from 45.24% to 45.45%, 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 @@ -188,10 +188,25 @@
return msg;
}

MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, std::string && ns)
Copy link
Contributor

Choose a reason for hiding this comment

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

Will there be additional documentation or image showing the usage of this marker?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

@Owen-Liuyuxuan Thank you for your comment.

Unfortunatelly, there is no document about overhang lanelet. It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to check whether the object is on the ego lane.

Actually, it is difficult to understand without any comment. So, I'll add above brief explanation on code.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

@Owen-Liuyuxuan I updated comment in code in 1344e6c.

{
MarkerArray msg;
msg.markers.reserve(objects.size());

for (const auto & object : objects) {
appendMarkerArray(
marker_utils::createLaneletsAreaMarkerArray(
{object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0),
&msg);
}

return msg;
}

MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;
msg.markers.reserve(objects.size() * 4);
msg.markers.reserve(objects.size() * 5);

appendMarkerArray(
createObjectsCubeMarkerArray(
Expand All @@ -202,14 +217,15 @@
appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);

return msg;
}

MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;
msg.markers.reserve(objects.size() * 4);
msg.markers.reserve(objects.size() * 5);

appendMarkerArray(
createObjectsCubeMarkerArray(
Expand All @@ -220,6 +236,7 @@
appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);

return msg;
}
Expand Down Expand Up @@ -451,6 +468,10 @@
appendMarkerArray(
createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"),
&msg);
appendMarkerArray(
createOverhangLaneletMarkerArray(
filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"),
&msg);

return msg;
}
Expand Down
29 changes: 11 additions & 18 deletions planning/behavior_path_avoidance_module/src/scene.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_avoidance_module/src/scene.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 1192 to 1185, 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 better: Overall Code Complexity

The mean cyclomatic complexity decreases from 4.98 to 4.93, 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 @@ -638,21 +638,16 @@
const auto o_front = data.stop_target_object.value();
const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto constant_distance = helper_->getFrontConstantDistance(o_front);
const auto & vehicle_width = planner_data_->parameters.vehicle_width;

const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor +
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;

const auto variable = helper_->getSharpAvoidanceDistance(
const auto avoidance_distance = helper_->getSharpAvoidanceDistance(
helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
const auto constant = helper_->getNominalPrepareDistance() +
object_parameter.safety_buffer_longitudinal +
additional_buffer_longitudinal;
const auto total_avoid_distance = variable + constant;
const auto prepare_distance = helper_->getNominalPrepareDistance();
const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance;

Check warning on line 650 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#L650

Added line #L650 was not covered by tests

dead_pose_ = calcLongitudinalOffsetPose(
data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance);
Expand Down Expand Up @@ -1434,16 +1429,14 @@

const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
const auto variable = helper_->getMinAvoidanceDistance(
const auto avoidance_distance = helper_->getMinAvoidanceDistance(

Check warning on line 1432 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#L1432

Added line #L1432 was not covered by tests
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal +
additional_buffer_longitudinal + p->stop_buffer;

return object.longitudinal - std::min(variable + constant, p->stop_max_distance);
const auto constant_distance = helper_->getFrontConstantDistance(object);

Check warning on line 1434 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#L1434

Added line #L1434 was not covered by tests

return object.longitudinal -
std::min(
avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer,

Check warning on line 1438 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#L1437-L1438

Added lines #L1437 - L1438 were not covered by tests
p->stop_max_distance);
}

void AvoidanceModule::insertReturnDeadLine(
Expand Down
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/shift_line_generator.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 8.32 to 8.24, 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 @@ -122,31 +122,27 @@
AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
{
// To be consistent with changes in the ego position, the current shift length is considered.
const auto current_ego_shift = helper_->getEgoShift();
const auto & base_link2rear = data_->parameters.base_link2rear;

// Calculate feasible shift length
const auto get_shift_profile =
[&](
auto & object, const auto & desire_shift_length) -> std::optional<std::pair<double, double>> {
// use each object param
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
const auto is_object_on_right = utils::avoidance::isOnRight(object);

// use absolute dist for return-to-center, relative dist from current for avoiding.
const auto avoiding_shift = desire_shift_length - current_ego_shift;
const auto nominal_avoid_distance = helper_->getMaxAvoidanceDistance(avoiding_shift);

// calculate remaining distance.
const auto prepare_distance = helper_->getNominalPrepareDistance();
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
: 0.0;
const auto constant = object_parameter.safety_buffer_longitudinal +
additional_buffer_longitudinal + prepare_distance;
const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
const auto remaining_distance = object.longitudinal - constant;
const auto constant_distance = helper_->getFrontConstantDistance(object);

Check warning on line 142 in planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp#L142

Added line #L142 was not covered by tests
const auto has_enough_distance =
object.longitudinal > constant_distance + prepare_distance + nominal_avoid_distance;
const auto remaining_distance = object.longitudinal - constant_distance - prepare_distance;

Check warning on line 145 in planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp#L144-L145

Added lines #L144 - L145 were not covered by tests
const auto avoidance_distance =
has_enough_distance ? nominal_avoid_distance : remaining_distance;

Expand Down Expand Up @@ -278,11 +274,8 @@
}
}

// use each object param
const auto object_type = utils::getHighestProbLabel(o.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
// calculate feasible shift length based on behavior policy
const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);

if (!feasible_shift_profile.has_value()) {
if (o.avoid_required && is_forward_object(o)) {
break;
Expand All @@ -297,12 +290,8 @@

AvoidLine al_avoid;
{
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
: 0.0;
const auto offset =
object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
const auto to_shift_end = o.longitudinal - offset;
const auto constant_distance = helper_->getFrontConstantDistance(o);
const auto to_shift_end = o.longitudinal - constant_distance;

Check warning on line 294 in planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp#L294

Added line #L294 was not covered by tests
const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index);

// start point (use previous linear shift length as start shift length.)
Expand Down Expand Up @@ -338,8 +327,8 @@

AvoidLine al_return;
{
const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
const auto to_shift_start = o.longitudinal + offset;
const auto constant_distance = helper_->getRearConstantDistance(o);
const auto to_shift_start = o.longitudinal + constant_distance;

Check notice on line 331 in planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method

ShiftLineGenerator::generateAvoidOutline decreases in cyclomatic complexity from 50 to 48, 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.

// start point
al_return.start_shift_length = feasible_shift_profile.value().first;
Expand Down
39 changes: 23 additions & 16 deletions planning/behavior_path_avoidance_module/src/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/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 1895 to 1901, 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 worse: Number of Functions in a Single Module

The number of functions increases from 79 to 80, threshold = 75. This file contains too many functions. Beyond a certain threshold, more functions lower 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 4.96 to 4.94, 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 @@ -478,6 +478,14 @@
object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
}

bool isOnEgoLane(const ObjectData & object)

Check warning on line 481 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#L481

Added line #L481 was not covered by tests
{
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
return boost::geometry::within(

Check warning on line 484 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#L483-L484

Added lines #L483 - L484 were not covered by tests
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
object.overhang_lanelet.polygon2d().basicPolygon());
}

bool isParallelToEgoLane(const ObjectData & object, const double threshold)
{
const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
Expand Down Expand Up @@ -525,6 +533,10 @@
using lanelet::utils::to2D;
using lanelet::utils::conversion::toLaneletPoint;

if (object.is_within_intersection) {
return false;
}

Check warning on line 539 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: Complex Method

isParkedVehicle increases in cyclomatic complexity from 10 to 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.
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
const auto centerline_pos =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position;
Expand Down Expand Up @@ -613,15 +625,11 @@
return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
}

bool isForceAvoidanceTarget(
bool isAmbiguousStoppedVehicle(

Check warning on line 628 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#L628

Added line #L628 was not covered by tests
ObjectData & object, const AvoidancePlanningData & data,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
if (!parameters->enable_force_avoidance_for_stopped_vehicle) {
return false;
}

const auto stop_time_longer_than_threshold =
object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;

Expand Down Expand Up @@ -652,6 +660,10 @@
return false;
}

if (!object.is_on_ego_lane) {
return true;
}

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

isForceAvoidanceTarget 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 warning on line 666 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

isAmbiguousStoppedVehicle 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.
const auto & ego_pose = planner_data->self_odometry->pose.pose;

// force avoidance for stopped vehicle
Expand Down Expand Up @@ -777,25 +789,18 @@
ObjectData & object, const AvoidancePlanningData & data,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
using boost::geometry::within;
using lanelet::utils::to2D;
using lanelet::utils::conversion::toLaneletPoint;

object.behavior = getObjectBehavior(object, parameters);
object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
object.is_on_ego_lane = isOnEgoLane(object);
object.is_ambiguous = isAmbiguousStoppedVehicle(object, data, planner_data, parameters);

Check warning on line 795 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#L794-L795

Added lines #L794 - L795 were not covered by tests

// from here condition check for vehicle type objects.
if (isForceAvoidanceTarget(object, data, planner_data, parameters)) {
if (object.is_ambiguous && parameters->enable_force_avoidance_for_stopped_vehicle) {
return true;
}

// check vehicle shift ratio
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
const auto on_ego_driving_lane = within(
to2D(toLaneletPoint(object_pos)).basicPoint(),
object.overhang_lanelet.polygon2d().basicPolygon());
if (on_ego_driving_lane) {
if (object.is_on_ego_lane) {

Check warning on line 803 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: Complex Method

isSatisfiedWithVehicleCondition 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 (object.is_parked) {
return true;
} else {
Expand Down Expand Up @@ -1755,6 +1760,8 @@
}

if (filtering_utils::isVehicleTypeObject(o)) {
o.is_within_intersection =
filtering_utils::isWithinIntersection(o, planner_data->route_handler);
o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters);
o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);

Expand Down
Loading