Skip to content

Commit

Permalink
fix
Browse files Browse the repository at this point in the history 
Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 committed Jun 3, 2024
1 parent 9d94968 commit 1c2b489
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 47 deletions.
6 changes: 3 additions & 3 deletions planning/route_handler/include/route_handler/route_handler.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021-2023 Tier IV, Inc.
// Copyright 2021-2024 TIER IV, Inc.
//
// 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 @@ -288,8 +288,8 @@ class RouteHandler
void removeOverlappedCenterlineWithWaypoints(
std::vector<PiecewiseReferencePoints> & piecewise_ref_points_vec,
const std::vector<geometry_msgs::msg::Point> & piecewise_waypoints,
const lanelet::ConstLanelets & lanelet_sequence, const size_t lanelet_sequence_index,
const bool is_first) const;
const lanelet::ConstLanelets & lanelet_sequence,
const size_t piecewise_waypoints_lanelet_sequence_index, const bool is_waypoint_first) const;
std::optional<lanelet::ConstLanelet> getLaneChangeTarget(
const lanelet::ConstLanelets & lanelets, const Direction direction = Direction::NONE) const;
std::optional<lanelet::ConstLanelet> getLaneChangeTargetExceptPreferredLane(
Expand Down
93 changes: 49 additions & 44 deletions planning/route_handler/src/route_handler.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021-2023 Tier IV, Inc.
// Copyright 2021-2024 TIER IV, Inc.

Check notice on line 1 in planning/route_handler/src/route_handler.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 1609 to 1788, 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/route_handler/src/route_handler.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 94 to 101, 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/route_handler/src/route_handler.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.56 to 4.64, 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 @@ -155,14 +155,13 @@ bool isIndexWithinVector(const std::vector<T> & vec, const int index)
}

template <typename T>
void removeIndicesFromVector(std::vector<T> & vec, const std::vector<size_t> & indices)
void removeIndicesFromVector(std::vector<T> & vec, std::vector<size_t> indices)
{
// sort indices in a descending order
auto copied_indices = indices;
std::sort(copied_indices.begin(), copied_indices.end(), std::greater<int>());
std::sort(indices.begin(), indices.end(), std::greater<int>());

// remove indices from vector
for (const size_t index : copied_indices) {
for (const size_t index : indices) {
vec.erase(vec.begin() + index);
}
}
Expand Down Expand Up @@ -1540,8 +1539,8 @@ PathWithLaneId RouteHandler::getCenterLinePath(
using lanelet::utils::to2D;
using lanelet::utils::conversion::toLaneletPoint;

// 1. calculate reference points by lanelets' centerlines
// NOTE: This vector alignes the vector lanelet_sequence.
// 1. calculate reference points by lanelets' centerline
// NOTE: This vector aligns the vector lanelet_sequence.
std::vector<PiecewiseReferencePoints> piecewise_ref_points_vec;
const auto add_ref_point = [&](const auto & pt) {
piecewise_ref_points_vec.back().push_back(
Expand All @@ -1550,8 +1549,8 @@ PathWithLaneId RouteHandler::getCenterLinePath(
double s = 0;
for (const auto & llt : lanelet_sequence) {
piecewise_ref_points_vec.push_back(std::vector<ReferencePoint>{});
const lanelet::ConstLineString3d centerline = llt.centerline();

const lanelet::ConstLineString3d centerline = llt.centerline();
for (size_t i = 0; i < centerline.size(); i++) {
const auto & pt = centerline[i];
const lanelet::ConstPoint3d next_pt =
Expand Down Expand Up @@ -1590,7 +1589,7 @@ PathWithLaneId RouteHandler::getCenterLinePath(
if (lanelet_sequence_itr == lanelet_sequence.end()) {
continue;
}
const size_t lanelet_sequence_index =
const size_t piecewise_waypoints_lanelet_sequence_index =
std::distance(lanelet_sequence.begin(), lanelet_sequence_itr);

// calculate reference points by waypoints
Expand All @@ -1601,24 +1600,26 @@ PathWithLaneId RouteHandler::getCenterLinePath(
piecewise_waypoints_itr != waypoints.begin() &&
piecewise_waypoints_itr != waypoints.end() - 1) {
// If piecewise_waypoints_itr is not the end (first or last) of piecewise_waypoints,
// remove all the reference points and add reference points generated by waypoints.
piecewise_ref_points_vec.at(lanelet_sequence_index) = ref_points_by_waypoints;
// remove all the reference points and add waypoints.
piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index) =
ref_points_by_waypoints;
} else {
// If piecewise_waypoints_itr is the end (first or last) of piecewise_waypoints
// TODO(murooka) change the name
const bool is_first = piecewise_waypoints_itr == waypoints.begin();
const bool is_waypoint_first = piecewise_waypoints_itr == waypoints.begin();

// concatenate waypoints and ref points
// NOTE: reference points by centerline and waypoints may overlap longitudinally.
auto & current_piecewise_ref_points = piecewise_ref_points_vec.at(lanelet_sequence_index);
// concatenate waypoints and reference points by centerline
// NOTE: They may overlap longitudinally.
auto & current_piecewise_ref_points =
piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index);
current_piecewise_ref_points.insert(
is_first ? current_piecewise_ref_points.end() : current_piecewise_ref_points.begin(),
is_waypoint_first ? current_piecewise_ref_points.end()
: current_piecewise_ref_points.begin(),
ref_points_by_waypoints.begin(), ref_points_by_waypoints.end());

// remove invalid ref points which overlap waypoints
// remove overlapped ref points which overlap waypoints
removeOverlappedCenterlineWithWaypoints(
piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, lanelet_sequence_index,
is_first);
piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence,
piecewise_waypoints_lanelet_sequence_index, is_waypoint_first);
}
}
}
Expand Down Expand Up @@ -1715,31 +1716,32 @@ std::vector<Waypoints> RouteHandler::calcWaypointsVector(
void RouteHandler::removeOverlappedCenterlineWithWaypoints(
std::vector<PiecewiseReferencePoints> & piecewise_ref_points_vec,
const std::vector<geometry_msgs::msg::Point> & piecewise_waypoints,
const lanelet::ConstLanelets & lanelet_sequence, const size_t lanelet_sequence_index,
const bool is_first) const
const lanelet::ConstLanelets & lanelet_sequence,
const size_t piecewise_waypoints_lanelet_sequence_index, const bool is_waypoint_first) const
{
const double waypoints_interpolation_arc_margin_ratio = 5.0;

// calculate arc length threshold
const double front_arc_length_threshold = [&]() {
const auto front_waypoint_arc_coordinates =
calcArcCoordinates(lanelet_sequence.at(lanelet_sequence_index), piecewise_waypoints.front());
return front_waypoint_arc_coordinates.length +
const auto front_waypoint_arc_coordinates = calcArcCoordinates(
lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index), piecewise_waypoints.front());
const double lanelet_arc_length = boost::geometry::length(
lanelet::utils::to2D(lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index)
.centerline()
.basicLineString()));
return -lanelet_arc_length + front_waypoint_arc_coordinates.length -
std::abs(front_waypoint_arc_coordinates.distance) *
waypoints_interpolation_arc_margin_ratio;
}();
const double back_arc_length_threshold = [&]() {
const auto back_waypoint_arc_coordinates =
calcArcCoordinates(lanelet_sequence.at(lanelet_sequence_index), piecewise_waypoints.back());
const double lanelet_arc_length = boost::geometry::length(lanelet::utils::to2D(
lanelet_sequence.at(lanelet_sequence_index).centerline().basicLineString()));
return -lanelet_arc_length + back_waypoint_arc_coordinates.length -
std::abs(back_waypoint_arc_coordinates.distance) *
waypoints_interpolation_arc_margin_ratio;
const auto back_waypoint_arc_coordinates = calcArcCoordinates(
lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index), piecewise_waypoints.back());
return back_waypoint_arc_coordinates.length + std::abs(back_waypoint_arc_coordinates.distance) *
waypoints_interpolation_arc_margin_ratio;
}();

double offset_arc_length = 0.0;
int target_lanelet_sequence_index = static_cast<int>(lanelet_sequence_index);
int target_lanelet_sequence_index = static_cast<int>(piecewise_waypoints_lanelet_sequence_index);
while (isIndexWithinVector(lanelet_sequence, target_lanelet_sequence_index)) {
auto & target_piecewise_ref_points = piecewise_ref_points_vec.at(target_lanelet_sequence_index);
const double target_lanelet_arc_length = boost::geometry::length(lanelet::utils::to2D(
Expand All @@ -1748,26 +1750,29 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints(
// search overlapped ref points in the target lanelet
std::vector<size_t> overlapped_ref_points_indices{};
const bool is_search_finished = [&]() {
for (size_t ref_point_tmp_index = 0; ref_point_tmp_index < target_piecewise_ref_points.size();
++ref_point_tmp_index) {
for (size_t ref_point_unsigned_index = 0;
ref_point_unsigned_index < target_piecewise_ref_points.size();
++ref_point_unsigned_index) {
const size_t ref_point_index =
is_first ? target_piecewise_ref_points.size() - 1 - ref_point_tmp_index
: ref_point_tmp_index;
is_waypoint_first ? target_piecewise_ref_points.size() - 1 - ref_point_unsigned_index
: ref_point_unsigned_index;
const auto & ref_point = target_piecewise_ref_points.at(ref_point_index);
if (ref_point.is_waypoint) { // skip waypoints

// skip waypoints
if (ref_point.is_waypoint) {
continue;
}

const double ref_point_arc_length =
(is_first ? -target_lanelet_arc_length : 0) +
(is_waypoint_first ? -target_lanelet_arc_length : 0) +
calcArcCoordinates(lanelet_sequence.at(target_lanelet_sequence_index), ref_point.point)
.length;
if (is_first) {
if (offset_arc_length + ref_point_arc_length < back_arc_length_threshold) {
if (is_waypoint_first) {
if (offset_arc_length + ref_point_arc_length < front_arc_length_threshold) {
return true;
}
} else {
if (front_arc_length_threshold < offset_arc_length + ref_point_arc_length) {
if (back_arc_length_threshold < offset_arc_length + ref_point_arc_length) {
return true;
}
}
Expand All @@ -1785,8 +1790,8 @@ void RouteHandler::removeOverlappedCenterlineWithWaypoints(
break;
}

target_lanelet_sequence_index += is_first ? -1 : 1;
offset_arc_length = (is_first ? -1 : 1) * target_lanelet_arc_length;
target_lanelet_sequence_index += is_waypoint_first ? -1 : 1;
offset_arc_length = (is_waypoint_first ? -1 : 1) * target_lanelet_arc_length;
}
}

Check warning on line 1796 in planning/route_handler/src/route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

RouteHandler::removeOverlappedCenterlineWithWaypoints has a cyclomatic complexity of 13, 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 1796 in planning/route_handler/src/route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Bumpy Road Ahead 

RouteHandler::removeOverlappedCenterlineWithWaypoints has 3 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 warning on line 1796 in planning/route_handler/src/route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Deep, Nested Complexity 

RouteHandler::removeOverlappedCenterlineWithWaypoints 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.

Expand Down

0 comments on commit 1c2b489

Please sign in to comment.