Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci @brkay54
pre-commit-ci[bot] authored and brkay54 committed Aug 12, 2022
1 parent 1c4c2a5 commit 41788d5
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 38 deletions.
8 changes: 4 additions & 4 deletions ...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

Expand Down
67 changes: 33 additions & 34 deletions planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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<double> calcCurvatureFrom3Points(
const TrajectoryPoints & trajectory, size_t idx_dist)
std::vector<double> calcCurvatureFrom3Points(const TrajectoryPoints & trajectory, size_t idx_dist)
{
using tier4_autoware_utils::calcCurvature;
using tier4_autoware_utils::getPoint;
Expand All @@ -91,7 +90,6 @@ std::vector<double> 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)));
Expand Down Expand Up @@ -168,8 +166,6 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
return output;
}



boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
const TrajectoryPoints & input) const
{
Expand All @@ -192,43 +188,46 @@ boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
output.back() = input.back(); // keep the final speed.

const size_t idx_dist =
static_cast<size_t>(std::max(static_cast<int>((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1));
const size_t idx_dist = static_cast<size_t>(std::max(
static_cast<int>((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<double>::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<double>::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;
Expand Down

0 comments on commit 41788d5

Please sign in to comment.