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 1 commit
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
Expand Up @@ -1771,11 +1771,11 @@
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_;

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::exp(-lambda * t_h) - 1);

Check failure on line 1778 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View workflow job for this annotation

GitHub Actions / cppcheck-differential

Expression 'exp(x) - 1' can be replaced by 'expm1(x)' to avoid loss of precision. [unpreciseMathCall]
double search_dist = acceleration_distance + obj_vel * t_h;
return search_dist;
};
Expand All @@ -1787,12 +1787,12 @@
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);
// 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_acc * (1.0 / lambda) * t_f + obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) +

Check failure on line 1795 in perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

View workflow job for this annotation

GitHub Actions / cppcheck-differential

Expression 'exp(x) - 1' can be replaced by 'expm1(x)' to avoid loss of precision. [unpreciseMathCall]
obj_vel * t_f +
// Distance covered at constant speed
final_speed * (t_h - t_f);
Expand All @@ -1807,7 +1807,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));

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
22 changes: 11 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_;

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,15 @@
// 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^(-lambdat_stop))
// e^(-lambdat_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::log(1 + (v * lambda / a));

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

View workflow job for this annotation

GitHub Actions / cppcheck-differential

Expression 'log(1 + x)' can be replaced by 'log1p(x)' to avoid loss of precision. [unpreciseMathCall]

// 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::exp(-lambda * t_h) - 1);

Check failure on line 454 in perception/autoware_map_based_prediction/src/path_generator.cpp

View workflow job for this annotation

GitHub Actions / cppcheck-differential

Expression 'exp(x) - 1' can be replaced by 'expm1(x)' to avoid loss of precision. [unpreciseMathCall]
// Output an equivalent constant speed
return distance_to_reach_zero_speed / t_h;
}
Expand All @@ -461,17 +461,17 @@
// 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::exp(-lambda * t_h) - 1);

Check failure on line 464 in perception/autoware_map_based_prediction/src/path_generator.cpp

View workflow job for this annotation

GitHub Actions / cppcheck-differential

Expression 'exp(x) - 1' can be replaced by 'expm1(x)' to avoid loss of precision. [unpreciseMathCall]

// 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);
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::exp(-lambda * t_f) - 1) + v * t_f +

Check failure on line 474 in perception/autoware_map_based_prediction/src/path_generator.cpp

View workflow job for this annotation

GitHub Actions / cppcheck-differential

Expression 'exp(x) - 1' can be replaced by 'expm1(x)' to avoid loss of precision. [unpreciseMathCall]
// 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