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(autoware_map_based_prediction): fix syntaxError #7813

Merged
merged 5 commits into from
Jul 3, 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
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in perception/autoware_map_based_prediction/src/map_based_prediction_node.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 1894 to 1895, 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.
//
// 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 @@ -1771,11 +1771,12 @@
object.kinematics.acceleration_with_covariance.accel.linear.y)
: 0.0;
const double t_h = time_horizon;
const double λ = std::log(2) / acceleration_exponential_half_life_;
const double lambda = std::log(2) / acceleration_exponential_half_life_;

Check warning on line 1774 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp#L1774

Added line #L1774 was not covered by tests

auto get_search_distance_with_decaying_acc = [&]() -> double {
const double acceleration_distance =
obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
obj_acc * (1.0 / lambda) * t_h +
obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);

Check warning on line 1779 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp#L1778-L1779

Added lines #L1778 - L1779 were not covered by tests
double search_dist = acceleration_distance + obj_vel * t_h;
return search_dist;
};
Expand All @@ -1787,13 +1788,13 @@
return obj_vel * t_h;
}
// Time to reach final speed
const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc);
const double t_f = (-1.0 / lambda) * std::log(1 - ((final_speed - obj_vel) * lambda) / obj_acc);

Check warning on line 1791 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp#L1791

Added line #L1791 was not covered by tests
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
const double search_dist =
// Distance covered while accelerating
obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) +
obj_vel * t_f +
obj_acc * (1.0 / lambda) * t_f +
obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) + obj_vel * t_f +

Check warning on line 1797 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp#L1796-L1797

Added lines #L1796 - L1797 were not covered by tests
// Distance covered at constant speed
final_speed * (t_h - t_f);
return search_dist;
Expand All @@ -1807,7 +1808,7 @@
const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());

double final_speed_after_acceleration =
obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h));
obj_vel + obj_acc * (1.0 / lambda) * (1.0 - std::exp(-lambda * t_h));

Check warning on line 1811 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

MapBasedPredictionNode::getPredictedReferencePath already has high cyclomatic complexity, and now it increases in Lines of Code from 112 to 113. 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 1811 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp#L1811

Added line #L1811 was not covered by tests

const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_;
const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit;
Expand Down
24 changes: 13 additions & 11 deletions perception/autoware_map_based_prediction/src/path_generator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -421,7 +421,7 @@

// Using a decaying acceleration model. Consult the README for more information about the model.
const double t_h = duration;
const float λ = std::log(2) / acceleration_exponential_half_life_;
const float lambda = std::log(2) / acceleration_exponential_half_life_;

Check warning on line 424 in perception/autoware_map_based_prediction/src/path_generator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/path_generator.cpp#L424

Added line #L424 was not covered by tests

auto have_same_sign = [](double a, double b) -> bool {
return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0);
Expand All @@ -434,7 +434,7 @@
return v;
}
// Get velocity after time horizon
const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h));
const auto terminal_velocity = v + a * (1.0 / lambda) * (1 - std::exp(-lambda * t_h));

// If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at
// most stop, not reverse its direction)
Expand All @@ -443,15 +443,16 @@
// if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
// time t_stop

// 0 = Vo + acc(1/λ)(1-e^(-λt_stop))
// e^(-λt_stop) = 1 - (-Vo* λ)/acc
// t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc)
// t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc)
auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a));
// 0 = Vo + acc(1/lambda)(1-e^(-lambda t_stop))
// e^(-lambda t_stop) = 1 - (-Vo* lambda)/acc
// t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
// t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
auto t_stop = (-1.0 / lambda) * std::log1p(v * lambda / a);

Check warning on line 450 in perception/autoware_map_based_prediction/src/path_generator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/path_generator.cpp#L450

Added line #L450 was not covered by tests

// Calculate the distance traveled until stopping
auto distance_to_reach_zero_speed =
v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
v * t_stop + a * t_stop * (1.0 / lambda) +
a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);

Check warning on line 455 in perception/autoware_map_based_prediction/src/path_generator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/path_generator.cpp#L454-L455

Added lines #L454 - L455 were not covered by tests
// Output an equivalent constant speed
return distance_to_reach_zero_speed / t_h;
}
Expand All @@ -461,17 +462,18 @@
// assume it will continue accelerating (reckless driving)
const bool object_has_surpassed_limit_already = v > speed_limit;
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1);
return v + a * (1.0 / lambda) + (a / (t_h * std::pow(lambda, 2))) * std::expm1(-lambda * t_h);

Check warning on line 465 in perception/autoware_map_based_prediction/src/path_generator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/path_generator.cpp#L465

Added line #L465 was not covered by tests

// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
// So, we calculate the time it takes to reach the speed limit and compute how far the vehicle
// would go if it accelerated until reaching the speed limit, and then continued at a constant
// speed.
const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a);
const double t_f = (-1.0 / lambda) * std::log(1 - ((speed_limit - v) * lambda) / a);

Check warning on line 472 in perception/autoware_map_based_prediction/src/path_generator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/path_generator.cpp#L472

Added line #L472 was not covered by tests
const double distance_covered =
// Distance covered while accelerating
a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f +
a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) +
v * t_f +

Check warning on line 476 in perception/autoware_map_based_prediction/src/path_generator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/autoware_map_based_prediction/src/path_generator.cpp#L475-L476

Added lines #L475 - L476 were not covered by tests
// Distance covered at constant speed for the rest of the horizon time
speed_limit * (t_h - t_f);
return distance_covered / t_h;
Expand Down
Loading