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): fix bugs in parked vehicle filtering logic #7072

Merged
merged 3 commits into from
May 31, 2024
+80 −15
Merged

fix(avoidance): fix bugs in parked vehicle filtering logic #7072

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
70f341e
fix(avoidance): check prev/next lane
satoshi-ota May 16, 2024
234b8fe
fix(avoidance): fix parked vehicle filtering
satoshi-ota May 20, 2024
6bd2315
fix(avoidance): check intersection location
satoshi-ota May 20, 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
95 changes: 80 additions & 15 deletions planning/autoware_behavior_path_static_obstacle_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/autoware_behavior_path_static_obstacle_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 1799 to 1855, 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_static_obstacle_avoidance_module/src/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 5.08 to 5.25, 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 @@ -318,25 +318,55 @@
bool isWithinIntersection(
const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
{
const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else");
if (id == "else") {
const std::string area_id = object.overhang_lanelet.attributeOr("intersection_area", "else");
if (area_id == "else") {
return false;
}

const std::string location = object.overhang_lanelet.attributeOr("location", "else");
if (location == "private") {
return false;
}

const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);

const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));
const auto polygon =
route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str()));

return boost::geometry::within(
object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
}

bool isOnEgoLane(const ObjectData & object)
bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
{
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
return boost::geometry::within(
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
object.overhang_lanelet.polygon2d().basicPolygon());
if (boost::geometry::within(
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
object.overhang_lanelet.polygon2d().basicPolygon())) {
return true;
}

// push previous lanelet
lanelet::ConstLanelets prev_lanelet;
if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) {
if (boost::geometry::within(
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
prev_lanelet.front().polygon2d().basicPolygon())) {
return true;
}
}

// push next lanelet
lanelet::ConstLanelet next_lanelet;
if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) {
if (boost::geometry::within(
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
next_lanelet.polygon2d().basicPolygon())) {
return true;
}
}

return false;

Check notice on line 369 in planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Bumpy Road Ahead 

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

bool isParallelToEgoLane(const ObjectData & object, const double threshold)
Expand Down Expand Up @@ -604,14 +634,49 @@

if (object.is_on_ego_lane) {
const auto right_lane =
planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false);
planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true);
if (right_lane.has_value() && isOnRight(object)) {
const lanelet::Attribute & right_lane_sub_type =
right_lane.value().attribute(lanelet::AttributeName::Subtype);
if (right_lane_sub_type != "road_shoulder") {
object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
RCLCPP_DEBUG(
rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
return true;
}

const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);
const auto is_disjoint_right_lane =
boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon());
if (is_disjoint_right_lane) {
object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
RCLCPP_DEBUG(
rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
return true;
}
}

const auto left_lane =
planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false);
if (right_lane.has_value() && left_lane.has_value()) {
object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
RCLCPP_DEBUG(
rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
return true;
planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true);
if (left_lane.has_value() && !isOnRight(object)) {
const lanelet::Attribute & left_lane_sub_type =
left_lane.value().attribute(lanelet::AttributeName::Subtype);
if (left_lane_sub_type != "road_shoulder") {
object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
RCLCPP_DEBUG(
rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
return true;
}

const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);
const auto is_disjoint_left_lane =
boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon());
if (is_disjoint_left_lane) {
object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
RCLCPP_DEBUG(
rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
return true;
}

Check warning on line 679 in planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

isNeverAvoidanceTarget increases in cyclomatic complexity from 24 to 30, 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 679 in planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead 

isNeverAvoidanceTarget increases from 5 to 6 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.
}
}

Expand Down Expand Up @@ -754,7 +819,7 @@
const std::shared_ptr<AvoidanceParameters> & parameters)
{
object.behavior = getObjectBehavior(object, parameters);
object.is_on_ego_lane = isOnEgoLane(object);
object.is_on_ego_lane = isOnEgoLane(object, planner_data->route_handler);

if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) {
return false;
Expand Down
Loading