feat(motion_velocity_smoother): delete unnecessary linear interpolati…

…on (autowarefoundation#1729) Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com>
boyali · Sep 28, 2022 · 4d2f5e4 · 4d2f5e4
1 parent cbe4c10
commit 4d2f5e4
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 164 deletions.
diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/motion_velocity_smoother/CMakeLists.txt
@@ -25,7 +25,6 @@ set(SMOOTHER_SRC
   src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
   src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
   src/trajectory_utils.cpp
-  src/linear_interpolation.cpp
   src/resample.cpp
 )
 

diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp
diff --git a/...locity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/...locity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp
@@ -16,7 +16,6 @@
 #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_  // NOLINT
 
 #include "motion_utils/trajectory/trajectory.hpp"
-#include "motion_velocity_smoother/linear_interpolation.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"

diff --git a/planning/motion_velocity_smoother/src/linear_interpolation.cpp b/planning/motion_velocity_smoother/src/linear_interpolation.cpp
diff --git a/...ty_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/...ty_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
@@ -14,6 +14,8 @@
 
 #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp"
 
+#include "interpolation/linear_interpolation.hpp"
+
 #include <algorithm>
 #include <vector>
 
@@ -232,28 +234,24 @@ bool calcStopVelocityWithConstantJerkAccLimit(
     dists.push_back(dist);
   }
 
-  const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, dists);
-  const auto acc_at_wp = linear_interpolation::interpolate(xs, as, dists);
-  const auto jerk_at_wp = linear_interpolation::interpolate(xs, js, dists);
-  if (!vel_at_wp || !acc_at_wp || !jerk_at_wp) {
-    RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Interpolation error");
-    return false;
-  }
+  const auto vel_at_wp = interpolation::lerp(xs, vs, dists);
+  const auto acc_at_wp = interpolation::lerp(xs, as, dists);
+  const auto jerk_at_wp = interpolation::lerp(xs, js, dists);
 
   // for debug
   std::stringstream ssi;
   for (unsigned int i = 0; i < dists.size(); ++i) {
-    ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp->at(i) << ", a: " << acc_at_wp->at(i)
-        << ", j: " << jerk_at_wp->at(i) << std::endl;
+    ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i)
+        << ", j: " << jerk_at_wp.at(i) << std::endl;
   }
   RCLCPP_DEBUG(
     rclcpp::get_logger("velocity_planning_utils"), "Interpolated = %s", ssi.str().c_str());
 
-  for (size_t 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 (size_t 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 (size_t i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) {
+  for (size_t 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;
   }

diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp
@@ -16,7 +16,6 @@
 
 #include "interpolation/linear_interpolation.hpp"
 #include "interpolation/spline_interpolation.hpp"
-#include "motion_velocity_smoother/linear_interpolation.hpp"
 
 #include <algorithm>
 #include <limits>
@@ -499,21 +498,14 @@ boost::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
     distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); });
   const std::vector<double> distance{distance_all.begin() + start_index, it_end};
 
-  const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, distance);
-  const auto acc_at_wp = linear_interpolation::interpolate(xs, as, distance);
+  const auto vel_at_wp = interpolation::lerp(xs, vs, distance);
+  const auto acc_at_wp = interpolation::lerp(xs, as, distance);
 
-  if (!vel_at_wp || !acc_at_wp) {
-    RCLCPP_WARN_STREAM(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
-      "interpolation error");
-    return {};
-  }
-
-  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 = 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) {
+  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;
   }