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(autoware_velocity_smoother): fix unusedFunction #8649

Merged
merged 1 commit into from
Aug 28, 2024
Merged

fix(autoware_velocity_smoother): fix unusedFunction #8649

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
11 changes: 0 additions & 11 deletions planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,12 +43,6 @@ std::vector<double> calcTrajectoryIntervalDistance(const TrajectoryPoints & traj
std::vector<double> calcTrajectoryCurvatureFrom3Points(
const TrajectoryPoints & trajectory, size_t idx_dist);

void setZeroVelocity(TrajectoryPoints & trajectory);

double getMaxVelocity(const TrajectoryPoints & trajectory);

double getMaxAbsVelocity(const TrajectoryPoints & trajectory);

void applyMaximumVelocityLimit(
const size_t from, const size_t to, const double max_vel, TrajectoryPoints & trajectory);

Expand All @@ -63,11 +57,6 @@ bool isValidStopDist(
const double v_end, const double a_end, const double v_target, const double a_target,
const double v_margin, const double a_margin);

std::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0,
const double min_acc, const double decel_target_vel,
const std::map<double, double> & jerk_profile);

std::optional<std::tuple<double, double, double, double>> updateStateWithJerkConstraint(
const double v0, const double a0, const std::map<double, double> & jerk_profile, const double t);

Expand Down
136 changes: 0 additions & 136 deletions planning/autoware_velocity_smoother/src/trajectory_utils.cpp
View file
Open in desktop
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/autoware_velocity_smoother/src/trajectory_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.85 to 4.06, 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.

Check notice on line 1 in planning/autoware_velocity_smoother/src/trajectory_utils.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 60.61% to 64.29%, 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 @@ -219,36 +219,6 @@
return k_arr;
}

void setZeroVelocity(TrajectoryPoints & trajectory)
{
for (auto & tp : trajectory) {
tp.longitudinal_velocity_mps = 0.0;
}
}

double getMaxVelocity(const TrajectoryPoints & trajectory)
{
double max_vel = 0.0;
for (const auto & tp : trajectory) {
if (tp.longitudinal_velocity_mps > max_vel) {
max_vel = tp.longitudinal_velocity_mps;
}
}
return max_vel;
}

double getMaxAbsVelocity(const TrajectoryPoints & trajectory)
{
double max_vel = 0.0;
for (const auto & tp : trajectory) {
double abs_vel = std::fabs(tp.longitudinal_velocity_mps);
if (abs_vel > max_vel) {
max_vel = abs_vel;
}
}
return max_vel;
}

void applyMaximumVelocityLimit(
const size_t begin, const size_t end, const double max_vel, TrajectoryPoints & trajectory)
{
Expand Down Expand Up @@ -427,113 +397,7 @@
}
return true;
}

Check notice on line 400 in planning/autoware_velocity_smoother/src/trajectory_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method 

applyDecelFilterWithJerkConstraint 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 400 in planning/autoware_velocity_smoother/src/trajectory_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

applyDecelFilterWithJerkConstraint 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 400 in planning/autoware_velocity_smoother/src/trajectory_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

applyDecelFilterWithJerkConstraint is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check notice on line 400 in planning/autoware_velocity_smoother/src/trajectory_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Conditional 

applyDecelFilterWithJerkConstraint no longer has a complex conditional
std::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0,
const double min_acc, const double decel_target_vel,
const std::map<double, double> & jerk_profile)
{
double t_total{0.0};
for (const auto & it : jerk_profile) {
t_total += it.second;
}

std::vector<double> ts;
std::vector<double> xs;
std::vector<double> vs;
std::vector<double> as;
std::vector<double> js;
const double dt{0.1};
double x{0.0};
double v{0.0};
double a{0.0};
double j{0.0};

for (double t = 0.0; t < t_total; t += dt) {
// Calculate state = (x, v, a, j) at t
const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total);
if (!state) {
return {};
}
std::tie(x, v, a, j) = *state;
if (v > 0.0) {
a = std::max(a, min_acc);
ts.push_back(t);
xs.push_back(x);
vs.push_back(v);
as.push_back(a);
js.push_back(j);
}
}
// Calculate state = (x, v, a, j) at t_total
const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total);
if (!state) {
return {};
}
std::tie(x, v, a, j) = *state;
if (v > 0.0 && !xs.empty() && xs.back() < x) {
a = std::max(a, min_acc);
ts.push_back(t_total);
xs.push_back(x);
vs.push_back(v);
as.push_back(a);
js.push_back(j);
}

const double a_target{0.0};
const double v_margin{0.3};
const double a_margin{0.1};
if (!isValidStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"),
"validation check error");
return {};
}

TrajectoryPoints output_trajectory{input};

if (xs.empty()) {
output_trajectory.at(start_index).longitudinal_velocity_mps = decel_target_vel;
output_trajectory.at(start_index).acceleration_mps2 = 0.0;
for (unsigned int i = start_index + 1; i < output_trajectory.size(); ++i) {
output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel;
output_trajectory.at(i).acceleration_mps2 = 0.0;
}
return output_trajectory;
}

const std::vector<double> distance_all = calcArclengthArray(output_trajectory);
const auto it_end = std::find_if(
distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); });
const std::vector<double> distance{distance_all.begin() + start_index, it_end};

if (
!interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distance) ||
!interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distance)) {
return {};
}

if (
xs.size() < 2 || vs.size() < 2 || as.size() < 2 || distance.empty() ||
distance.front() < xs.front() || xs.back() < distance.back()) {
return {};
}

const auto vel_at_wp = interpolation::lerp(xs, vs, distance);
const auto acc_at_wp = interpolation::lerp(xs, as, distance);

for (unsigned int i = 0; i < vel_at_wp.size(); ++i) {
output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i);
output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp.at(i);
}
for (unsigned int i = start_index + vel_at_wp.size(); i < output_trajectory.size(); ++i) {
output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel;
output_trajectory.at(i).acceleration_mps2 = 0.0;
}

return output_trajectory;
}

std::optional<std::tuple<double, double, double, double>> updateStateWithJerkConstraint(
const double v0, const double a0, const std::map<double, double> & jerk_profile, const double t)
{
Expand Down
Loading