From 8e1542664af94f193a867b75b25920dde912c765 Mon Sep 17 00:00:00 2001 From: kobayu858 Date: Wed, 28 Aug 2024 10:55:01 +0900 Subject: [PATCH] fix:unusedFunction Signed-off-by: kobayu858 --- .../velocity_smoother/trajectory_utils.hpp | 11 -- .../src/trajectory_utils.cpp | 136 ------------------ 2 files changed, 147 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp index 29033b69d7a66..d20f82538e4d5 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp @@ -43,12 +43,6 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist); -void setZeroVelocity(TrajectoryPoints & trajectory); - -double getMaxVelocity(const TrajectoryPoints & trajectory); - -double getMaxAbsVelocity(const TrajectoryPoints & trajectory); - void applyMaximumVelocityLimit( const size_t from, const size_t to, const double max_vel, TrajectoryPoints & trajectory); @@ -63,11 +57,6 @@ bool isValidStopDist( const double v_end, const double a_end, const double v_target, const double a_target, const double v_margin, const double a_margin); -std::optional applyDecelFilterWithJerkConstraint( - const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0, - const double min_acc, const double decel_target_vel, - const std::map & jerk_profile); - std::optional> updateStateWithJerkConstraint( const double v0, const double a0, const std::map & jerk_profile, const double t); diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index b796e2aeb91de..68f05438d38c8 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -219,36 +219,6 @@ std::vector calcTrajectoryCurvatureFrom3Points( return k_arr; } -void setZeroVelocity(TrajectoryPoints & trajectory) -{ - for (auto & tp : trajectory) { - tp.longitudinal_velocity_mps = 0.0; - } -} - -double getMaxVelocity(const TrajectoryPoints & trajectory) -{ - double max_vel = 0.0; - for (const auto & tp : trajectory) { - if (tp.longitudinal_velocity_mps > max_vel) { - max_vel = tp.longitudinal_velocity_mps; - } - } - return max_vel; -} - -double getMaxAbsVelocity(const TrajectoryPoints & trajectory) -{ - double max_vel = 0.0; - for (const auto & tp : trajectory) { - double abs_vel = std::fabs(tp.longitudinal_velocity_mps); - if (abs_vel > max_vel) { - max_vel = abs_vel; - } - } - return max_vel; -} - void applyMaximumVelocityLimit( const size_t begin, const size_t end, const double max_vel, TrajectoryPoints & trajectory) { @@ -428,112 +398,6 @@ bool isValidStopDist( return true; } -std::optional applyDecelFilterWithJerkConstraint( - const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0, - const double min_acc, const double decel_target_vel, - const std::map & jerk_profile) -{ - double t_total{0.0}; - for (const auto & it : jerk_profile) { - t_total += it.second; - } - - std::vector ts; - std::vector xs; - std::vector vs; - std::vector as; - std::vector js; - const double dt{0.1}; - double x{0.0}; - double v{0.0}; - double a{0.0}; - double j{0.0}; - - for (double t = 0.0; t < t_total; t += dt) { - // Calculate state = (x, v, a, j) at t - const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total); - if (!state) { - return {}; - } - std::tie(x, v, a, j) = *state; - if (v > 0.0) { - a = std::max(a, min_acc); - ts.push_back(t); - xs.push_back(x); - vs.push_back(v); - as.push_back(a); - js.push_back(j); - } - } - // Calculate state = (x, v, a, j) at t_total - const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total); - if (!state) { - return {}; - } - std::tie(x, v, a, j) = *state; - if (v > 0.0 && !xs.empty() && xs.back() < x) { - a = std::max(a, min_acc); - ts.push_back(t_total); - xs.push_back(x); - vs.push_back(v); - as.push_back(a); - js.push_back(j); - } - - const double a_target{0.0}; - const double v_margin{0.3}; - const double a_margin{0.1}; - if (!isValidStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), - "validation check error"); - return {}; - } - - TrajectoryPoints output_trajectory{input}; - - if (xs.empty()) { - output_trajectory.at(start_index).longitudinal_velocity_mps = decel_target_vel; - output_trajectory.at(start_index).acceleration_mps2 = 0.0; - for (unsigned int i = start_index + 1; i < output_trajectory.size(); ++i) { - output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; - output_trajectory.at(i).acceleration_mps2 = 0.0; - } - return output_trajectory; - } - - const std::vector distance_all = calcArclengthArray(output_trajectory); - const auto it_end = std::find_if( - distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); }); - const std::vector distance{distance_all.begin() + start_index, it_end}; - - if ( - !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distance) || - !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distance)) { - return {}; - } - - if ( - xs.size() < 2 || vs.size() < 2 || as.size() < 2 || distance.empty() || - distance.front() < xs.front() || xs.back() < distance.back()) { - return {}; - } - - const auto vel_at_wp = interpolation::lerp(xs, vs, distance); - const auto acc_at_wp = interpolation::lerp(xs, as, distance); - - for (unsigned int i = 0; i < vel_at_wp.size(); ++i) { - output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); - output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp.at(i); - } - for (unsigned int i = start_index + vel_at_wp.size(); i < output_trajectory.size(); ++i) { - output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; - output_trajectory.at(i).acceleration_mps2 = 0.0; - } - - return output_trajectory; -} - std::optional> updateStateWithJerkConstraint( const double v0, const double a0, const std::map & jerk_profile, const double t) {