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[bot] authored and brkay54 committed Aug 19, 2022
1 parent 7c16bb8 commit d091ddc
Showing 1 changed file with 25 additions and 25 deletions.
50 changes: 25 additions & 25 deletions planning/motion_velocity_smoother/src/trajectory_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -194,36 +194,36 @@ std::vector<double> calcTrajectoryIntervalDistance(const TrajectoryPoints & traj
std::vector<double> calcTrajectoryCurvatureFrom3Points(
const TrajectoryPoints & trajectory, size_t idx_dist)
{
using tier4_autoware_utils::calcCurvature;
using tier4_autoware_utils::getPoint;
using tier4_autoware_utils::calcCurvature;
using tier4_autoware_utils::getPoint;

if (trajectory.size() < 3) {
const std::vector<double> k_arr(trajectory.size(), 0.0);
return k_arr;
}
if (trajectory.size() < 3) {
const std::vector<double> k_arr(trajectory.size(), 0.0);
return k_arr;
}

// if the idx size is not enough, change the idx_dist
const auto max_idx_dist = static_cast<size_t>(std::floor((trajectory.size() - 1) / 2.0));
idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist));
// if the idx size is not enough, change the idx_dist
const auto max_idx_dist = static_cast<size_t>(std::floor((trajectory.size() - 1) / 2.0));
idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist));

if (idx_dist < 1) {
throw std::logic_error("idx_dist less than 1 is not expected");
}
if (idx_dist < 1) {
throw std::logic_error("idx_dist less than 1 is not expected");
}

// calculate curvature by circle fitting from three points
std::vector<double> k_arr;
// 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)));
k_arr.push_back(calcCurvature(p0, p1, p2));
}
// for last point curvature = 0;
k_arr.push_back(0.0);
// calculate curvature by circle fitting from three points
std::vector<double> k_arr;
// 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)));
k_arr.push_back(calcCurvature(p0, p1, p2));
}
// for last point curvature = 0;
k_arr.push_back(0.0);

return k_arr;
return k_arr;
}

void setZeroVelocity(TrajectoryPoints & trajectory)
Expand Down

0 comments on commit d091ddc

Please sign in to comment.