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(behavior_path_planner_common): drivable are bug #7182

Open
wants to merge 13 commits into
base: main
Choose a base branch
from
Original file line number Diff line number Diff line change
Expand Up @@ -80,8 +80,12 @@
if (diff_angle <= 1e-7 && diff_angle >= -1e-7) {
return std::make_pair(normal_vector_angle, width);
}

return std::make_pair(normal_vector_angle, width / std::sin(diff_angle));
const float adjusted_width = width / std::sin(diff_angle);
if (std::abs(adjusted_width) > 1.0) {
return std::make_pair(normal_vector_angle, adjusted_width / std::abs(adjusted_width));

Check warning on line 85 in common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp#L83-L85

Added lines #L83 - L85 were not covered by tests
} else {
return std::make_pair(normal_vector_angle, adjusted_width);
}
}();

normal_vector_angles.push_back(normal_vector_angle);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -207,8 +207,7 @@ void PlannerManager::generateCombinedDrivableArea(
output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward);
} else if (di.is_already_expanded) {
// for single side shift
utils::generateDrivableArea(
output.path, di.drivable_lanes, false, false, false, data, is_driving_forward);
utils::generateDrivableArea(output.path, di.drivable_lanes, false, false, false, data);
} else {
const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes);

Expand All @@ -220,8 +219,7 @@ void PlannerManager::generateCombinedDrivableArea(
// for other modules where multiple modules may be launched
utils::generateDrivableArea(
output.path, expanded_lanes, di.enable_expanding_hatched_road_markings,
di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data,
is_driving_forward);
di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data);
}

// extract obstacles from drivable area
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ void generateDrivableArea(
PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
const bool enable_expanding_freespace_areas,
const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
const std::shared_ptr<const PlannerData> planner_data);

void generateDrivableArea(
PathWithLaneId & path, const double vehicle_length, const double offset,
Expand Down Expand Up @@ -81,14 +81,12 @@ std::vector<geometry_msgs::msg::Point> calcBound(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
const std::vector<DrivableLanes> & drivable_lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
const bool enable_expanding_freespace_areas, const bool is_left,
const bool is_driving_forward = true);
const bool enable_expanding_freespace_areas, const bool is_left);

std::vector<geometry_msgs::msg::Point> postProcess(
const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
const std::shared_ptr<const PlannerData> planner_data,
const std::vector<DrivableLanes> & drivable_lanes, const bool is_left,
const bool is_driving_forward = true);
const std::vector<DrivableLanes> & drivable_lanes, const bool is_left);

DrivableAreaInfo combineDrivableAreaInfo(
const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);
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_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.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 1437 to 1434, 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_planner_common/src/utils/drivable_area_expansion/static_drivable_area.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 9.12 to 9.15, 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 @@ -800,24 +800,22 @@
PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
const bool enable_expanding_freespace_areas,
const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward)
const std::shared_ptr<const PlannerData> planner_data)
{
if (path.points.empty()) {
return;
}

path.left_bound.clear();
path.right_bound.clear();

// Insert Position
path.left_bound = calcBound(
path, planner_data, lanes, enable_expanding_hatched_road_markings,
enable_expanding_intersection_areas, enable_expanding_freespace_areas, true,
is_driving_forward);
enable_expanding_intersection_areas, enable_expanding_freespace_areas, true);
path.right_bound = calcBound(
path, planner_data, lanes, enable_expanding_hatched_road_markings,
enable_expanding_intersection_areas, enable_expanding_freespace_areas, false,
is_driving_forward);
enable_expanding_intersection_areas, enable_expanding_freespace_areas, false);

Check notice on line 818 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Excess Number of Function Arguments

generateDrivableArea decreases from 7 to 6 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

if (path.left_bound.empty() || path.right_bound.empty()) {
auto clock{rclcpp::Clock{RCL_ROS_TIME}};
Expand Down Expand Up @@ -1459,128 +1457,127 @@
std::vector<geometry_msgs::msg::Point> postProcess(
const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
const std::shared_ptr<const PlannerData> planner_data,
const std::vector<DrivableLanes> & drivable_lanes, const bool is_left,
const bool is_driving_forward)
const std::vector<DrivableLanes> & drivable_lanes, const bool is_left)
{
const auto lanelets = utils::transformToLanelets(drivable_lanes);
const auto & current_pose = planner_data->self_odometry->pose.pose;
const auto & route_handler = planner_data->route_handler;
const auto & vehicle_length = planner_data->parameters.vehicle_length;
constexpr double overlap_threshold = 0.01;

if (original_bound.size() < 2) {
return original_bound;
}

const auto addPoints =
[](const lanelet::ConstLineString3d & points, std::vector<geometry_msgs::msg::Point> & bound) {
for (const auto & bound_p : points) {
const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p);
if (bound.empty()) {
bound.push_back(cp);
} else if (autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) {
bound.push_back(cp);
}
}
};

const auto has_overlap =
[&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) {
for (const auto & transformed_lane : lanelets) {
if (checkHasSameLane(ignore_lanelets, transformed_lane)) {
continue;
}
if (boost::geometry::intersects(
lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) {
return true;
}
}
return false;
};

std::vector<geometry_msgs::msg::Point> processed_bound;
std::vector<geometry_msgs::msg::Point> tmp_bound = original_bound;

// Insert points after goal
lanelet::ConstLanelet goal_lanelet;
if (route_handler->getGoalLanelet(&goal_lanelet) && checkHasSameLane(lanelets, goal_lanelet)) {
const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length);
const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet);
const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet);
const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet);
lanelet::ConstLanelets goal_lanelets = {goal_lanelet};
if (goal_left_lanelet.has_value()) {
goal_lanelets.push_back(goal_left_lanelet.value());
}
if (goal_right_lanelet.has_value()) {
goal_lanelets.push_back(goal_right_lanelet.value());
}

for (const auto & lane : lanes_after_goal) {
// If lane is already in the transformed lanes, ignore it
if (checkHasSameLane(lanelets, lane)) {
continue;
}
// Check if overlapped
const bool is_overlapped =
(checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets)
: has_overlap(lane));
if (is_overlapped) {
continue;
}

if (is_left) {
addPoints(lane.leftBound3d(), tmp_bound);
} else {
addPoints(lane.rightBound3d(), tmp_bound);
}
}
}

if (!is_driving_forward) {
std::reverse(tmp_bound.begin(), tmp_bound.end());
}

const auto start_idx = [&]() {
const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points);
const auto cropped_path_points = autoware::motion_utils::cropPoints(
path.points, current_pose.position, current_seg_idx,
planner_data->parameters.forward_path_length,
planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval);

constexpr double front_length = 0.5;
const auto front_pose =
cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose;
const size_t front_start_idx =
findNearestSegmentIndexFromLateralDistance(tmp_bound, front_pose, M_PI_2);
const auto start_point =
calcLongitudinalOffsetStartPoint(tmp_bound, front_pose, front_start_idx, -front_length);

// Insert a start point
processed_bound.push_back(start_point);

const auto p_tmp =
geometry_msgs::build<Pose>().position(start_point).orientation(front_pose.orientation);
return findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2);
}();

// Get Closest segment for the goal point
const auto [goal_idx, goal_point] = [&]() {
const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose;
const size_t goal_start_idx =
findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2);
const auto goal_point =
calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length);
const auto p_tmp =
geometry_msgs::build<Pose>().position(goal_point).orientation(goal_pose.orientation);
const size_t goal_idx = std::max(
goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2));

const size_t goal_nearest_idx =
findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2);
const size_t goal_idx = ((goal_start_idx - start_idx) * (goal_start_idx - start_idx) >
(goal_nearest_idx - start_idx) * (goal_nearest_idx - start_idx))
? goal_start_idx
: goal_nearest_idx;
return std::make_pair(goal_idx, goal_point);
}();

// Insert middle points
for (size_t i = start_idx + 1; i <= goal_idx; ++i) {
size_t step = (start_idx < goal_idx) ? 1 : -1;
for (size_t i = start_idx + step; i != goal_idx + step; i += step) {

Check warning on line 1580 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

postProcess increases in cyclomatic complexity from 26 to 27, 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 1580 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Excess Number of Function Arguments

postProcess decreases from 6 to 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
const auto & next_point = tmp_bound.at(i);
const double dist =
autoware::universe_utils::calcDistance2d(processed_bound.back(), next_point);
Expand All @@ -1604,53 +1601,51 @@
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
const std::vector<DrivableLanes> & drivable_lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward)
const bool enable_expanding_freespace_areas, const bool is_left)
{
using autoware::motion_utils::removeOverlapPoints;

const auto & route_handler = planner_data->route_handler;

// a function to convert drivable lanes to points without duplicated points
const auto convert_to_points = [](const auto & drivable_lanes, const auto is_left) {
constexpr double overlap_threshold = 0.01;

std::vector<lanelet::ConstPoint3d> points;
for (const auto & drivable_lane : drivable_lanes) {
const auto bound =
is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d();
for (const auto & point : bound) {
if (
points.empty() ||
overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) {
points.push_back(point);
}
}
}
return points;
};

const auto to_ros_point = [](const std::vector<lanelet::ConstPoint3d> & bound) {
std::vector<Point> ret{};
std::for_each(bound.begin(), bound.end(), [&](const auto & p) {
ret.push_back(lanelet::utils::conversion::toGeomMsgPt(p));
});
return ret;
};

// Step1. create drivable bound from drivable lanes.
auto [bound_points, skip_post_process] = [&]() {
if (!enable_expanding_freespace_areas) {
return std::make_pair(convert_to_points(drivable_lanes, is_left), false);
}
return getBoundWithFreeSpaceAreas(
convert_to_points(drivable_lanes, is_left), convert_to_points(drivable_lanes, !is_left),
planner_data, is_left);
}();

const auto post_process = [&](const auto & bound, const auto skip) {
return skip
? bound
: postProcess(bound, path, planner_data, drivable_lanes, is_left, is_driving_forward);
return skip ? bound : postProcess(bound, path, planner_data, drivable_lanes, is_left);

Check notice on line 1648 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Excess Number of Function Arguments

calcBound decreases from 8 to 7 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
};

// Step2. if there is no drivable area defined by polygon, return original drivable bound.
Expand Down
Loading