From 41788d51a69b5c540dd0dac6ad61e89c4b473baf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 8 Aug 2022 15:07:19 +0000 Subject: [PATCH] ci(pre-commit): autofix --- .../smoother/smoother_base.hpp | 8 +-- .../src/smoother/smoother_base.cpp | 67 +++++++++---------- 2 files changed, 37 insertions(+), 38 deletions(-) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 37572cf3bb291..2192ff6a893cf 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -52,10 +52,10 @@ class SmootherBase double max_steering_angle_rate; // max steering angle rate [degree/s] double wheel_base; // wheel base [m] double sample_ds; // distance between trajectory points [m] - double curvature_threshold; // look-up distance of Trajectory point for calculation of steering angle - // limit [m] - double - curvature_calculation_distance; // threshold steering degree limit to trigger steeringRateLimit [degree] + double curvature_threshold; // look-up distance of Trajectory point for calculation of steering + // angle limit [m] + double curvature_calculation_distance; // threshold steering degree limit to trigger + // steeringRateLimit [degree] resampling::ResampleParam resample_param; }; diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index e2c188933e92d..1657695a954f1 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -14,9 +14,9 @@ #include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "motion_common/motion_common.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" -#include "motion_common/motion_common.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -67,8 +67,7 @@ double SmootherBase::getMinDecel() const { return base_param_.min_decel; } double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } -std::vector calcCurvatureFrom3Points( - const TrajectoryPoints & trajectory, size_t idx_dist) +std::vector calcCurvatureFrom3Points(const TrajectoryPoints & trajectory, size_t idx_dist) { using tier4_autoware_utils::calcCurvature; using tier4_autoware_utils::getPoint; @@ -91,7 +90,6 @@ std::vector calcCurvatureFrom3Points( // for first point curvature = 0; k_arr.push_back(0.0); for (size_t i = 1; i + 1 < trajectory.size(); i++) { - const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); const auto p1 = getPoint(trajectory.at(i)); const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); @@ -168,8 +166,6 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( return output; } - - boost::optional SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input) const { @@ -192,43 +188,46 @@ boost::optional SmootherBase::applySteeringRateLimit( auto output = motion_utils::convertToTrajectoryPointArray(output_traj); output.back() = input.back(); // keep the final speed. - const size_t idx_dist = - static_cast(std::max(static_cast((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1)); + const size_t idx_dist = static_cast(std::max( + static_cast((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1)); // Calculate curvature assuming the trajectory points interval is constant const auto curvature_v = calcCurvatureFrom3Points(output, idx_dist); for (size_t i = 0; i + 1 < output.size(); i++) { - - if(fabs(curvature_v.at(i)) > base_param_.curvature_threshold){ - - // calculate the just 2 steering angle - output.at(i).front_wheel_angle_rad = std::atan(base_param_.wheel_base * curvature_v.at(i)); - output.at(i + 1).front_wheel_angle_rad = std::atan(base_param_.wheel_base * curvature_v.at(i + 1)); - - const double mean_vel = (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; - const double dt = std::max(base_param_.sample_ds / mean_vel, std::numeric_limits::epsilon()); - const double steering_diff = fabs(output.at(i).front_wheel_angle_rad - output.at(i + 1).front_wheel_angle_rad); - const double dt_steering = - steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate); - - if (dt_steering > dt) { - const double target_mean_vel = (base_param_.sample_ds / dt_steering); - for(size_t k = 0; k < 2 ; k++){ - double temp_vel = output.at(i + k).longitudinal_velocity_mps * (target_mean_vel / mean_vel); - if (temp_vel < output.at(i + k).longitudinal_velocity_mps) { - output.at(i + k).longitudinal_velocity_mps = temp_vel; - } else { - if (target_mean_vel < output.at(i + k).longitudinal_velocity_mps) { - output.at(i + k).longitudinal_velocity_mps = target_mean_vel; - } - } - if (output.at(i + k).longitudinal_velocity_mps < base_param_.min_curve_velocity) { - output.at(i + k).longitudinal_velocity_mps = base_param_.min_curve_velocity; + if (fabs(curvature_v.at(i)) > base_param_.curvature_threshold) { + // calculate the just 2 steering angle + output.at(i).front_wheel_angle_rad = std::atan(base_param_.wheel_base * curvature_v.at(i)); + output.at(i + 1).front_wheel_angle_rad = + std::atan(base_param_.wheel_base * curvature_v.at(i + 1)); + + const double mean_vel = + (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; + const double dt = + std::max(base_param_.sample_ds / mean_vel, std::numeric_limits::epsilon()); + const double steering_diff = + fabs(output.at(i).front_wheel_angle_rad - output.at(i + 1).front_wheel_angle_rad); + const double dt_steering = + steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate); + + if (dt_steering > dt) { + const double target_mean_vel = (base_param_.sample_ds / dt_steering); + for (size_t k = 0; k < 2; k++) { + double temp_vel = + output.at(i + k).longitudinal_velocity_mps * (target_mean_vel / mean_vel); + if (temp_vel < output.at(i + k).longitudinal_velocity_mps) { + output.at(i + k).longitudinal_velocity_mps = temp_vel; + } else { + if (target_mean_vel < output.at(i + k).longitudinal_velocity_mps) { + output.at(i + k).longitudinal_velocity_mps = target_mean_vel; } } + if (output.at(i + k).longitudinal_velocity_mps < base_param_.min_curve_velocity) { + output.at(i + k).longitudinal_velocity_mps = base_param_.min_curve_velocity; + } } } + } } return output;