From b2f90a0952bf37969517f72ccd5a1ea86f253ce5 Mon Sep 17 00:00:00 2001
From: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com>
Date: Tue, 19 Apr 2022 19:04:01 +0900
Subject: [PATCH] fix(interpolation): implementation of template function to
 header (#698)

* Add test for polymorphism (expect linking error)

* Move templatized function impl to header
---
 .../spline_interpolation_points_2d.hpp        | 10 +++++-
 .../src/spline_interpolation_points_2d.cpp    | 36 +++++++++----------
 .../test/src/test_spline_interpolation.cpp    | 26 ++++++++++++++
 3 files changed, 53 insertions(+), 19 deletions(-)

diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp
index 7d58213e115a5..c1f08a6d937ae 100644
--- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp
+++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp
@@ -46,7 +46,14 @@ class SplineInterpolationPoints2d
   SplineInterpolationPoints2d() = default;
 
   template <typename T>
-  void calcSplineCoefficients(const std::vector<T> & points);
+  void calcSplineCoefficients(const std::vector<T> & points)
+  {
+    std::vector<geometry_msgs::msg::Point> points_inner;
+    for (const auto & p : points) {
+      points_inner.push_back(tier4_autoware_utils::getPoint(p));
+    }
+    calcSplineCoefficientsInner(points_inner);
+  }
 
   // TODO(murooka) implement these functions
   // std::vector<geometry_msgs::msg::Point> getSplineInterpolatedPoints(const double width);
@@ -58,6 +65,7 @@ class SplineInterpolationPoints2d
   double getAccumulatedLength(const size_t idx) const;
 
 private:
+  void calcSplineCoefficientsInner(const std::vector<geometry_msgs::msg::Point> & points);
   SplineInterpolation slerp_x_;
   SplineInterpolation slerp_y_;
 
diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp
index e0c0caf27d0c0..2a34e53fc884e 100644
--- a/common/interpolation/src/spline_interpolation_points_2d.cpp
+++ b/common/interpolation/src/spline_interpolation_points_2d.cpp
@@ -35,16 +35,16 @@ std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vec
   return dist_v;
 }
 
-template <typename T>
-std::array<std::vector<double>, 3> getBaseValues(const std::vector<T> & points)
+std::array<std::vector<double>, 3> getBaseValues(
+  const std::vector<geometry_msgs::msg::Point> & points)
 {
   // calculate x, y
   std::vector<double> base_x;
   std::vector<double> base_y;
   for (size_t i = 0; i < points.size(); i++) {
-    const auto & current_pos = tier4_autoware_utils::getPoint(points.at(i));
+    const auto & current_pos = points.at(i);
     if (i > 0) {
-      const auto & prev_pos = tier4_autoware_utils::getPoint(points.at(i - 1));
+      const auto & prev_pos = points.at(i - 1);
       if (
         std::fabs(current_pos.x - prev_pos.x) < 1e-6 &&
         std::fabs(current_pos.y - prev_pos.y) < 1e-6) {
@@ -88,20 +88,6 @@ template std::vector<double> slerpYawFromPoints(
   const std::vector<geometry_msgs::msg::Point> & points);
 }  // namespace interpolation
 
-template <typename T>
-void SplineInterpolationPoints2d::calcSplineCoefficients(const std::vector<T> & points)
-{
-  const auto base = getBaseValues(points);
-
-  base_s_vec_ = base.at(0);
-  const auto & base_x_vec = base.at(1);
-  const auto & base_y_vec = base.at(2);
-
-  // calculate spline coefficients
-  slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec);
-  slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec);
-}
-
 geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint(
   const size_t idx, const double s) const
 {
@@ -153,3 +139,17 @@ double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const
   }
   return base_s_vec_.at(idx);
 }
+
+void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
+  const std::vector<geometry_msgs::msg::Point> & points)
+{
+  const auto base = getBaseValues(points);
+
+  base_s_vec_ = base.at(0);
+  const auto & base_x_vec = base.at(1);
+  const auto & base_y_vec = base.at(2);
+
+  // calculate spline coefficients
+  slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec);
+  slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec);
+}
diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp
index e6c368b00a0f4..9d9869903b614 100644
--- a/common/interpolation/test/src/test_spline_interpolation.cpp
+++ b/common/interpolation/test/src/test_spline_interpolation.cpp
@@ -272,3 +272,29 @@ TEST(spline_interpolation, SplineInterpolationPoints2d)
   single_points.push_back(createPoint(1.0, 0.0, 0.0));
   EXPECT_THROW(s.calcSplineCoefficients(single_points), std::logic_error);
 }
+
+TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism)
+{
+  using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+  using tier4_autoware_utils::createPoint;
+
+  std::vector<geometry_msgs::msg::Point> points;
+  points.push_back(createPoint(-2.0, -10.0, 0.0));
+  points.push_back(createPoint(2.0, 1.5, 0.0));
+  points.push_back(createPoint(3.0, 3.0, 0.0));
+
+  std::vector<TrajectoryPoint> trajectory_points;
+  for (const auto & p : points) {
+    TrajectoryPoint tp;
+    tp.pose.position = p;
+    trajectory_points.push_back(tp);
+  }
+
+  SplineInterpolationPoints2d s_point;
+  s_point.calcSplineCoefficients(points);
+  s_point.getSplineInterpolatedPoint(0, 0.);
+
+  SplineInterpolationPoints2d s_traj_point;
+  s_traj_point.calcSplineCoefficients(trajectory_points);
+  s_traj_point.getSplineInterpolatedPoint(0, 0.);
+}