From 39ba4f1ce908663b1bbc64fab7c0e0494f20dc30 Mon Sep 17 00:00:00 2001
From: v-sugahara7993-esol
 <109567391+v-sugahara7993-esol@users.noreply.github.com>
Date: Mon, 15 May 2023 14:11:12 +0900
Subject: [PATCH] fix(interpolation): modify implementation of template
 function (#458)

Signed-off-by: v-sugahara7993-esol <v-sugahara7993@esol.co.jp>
---
 .../spline_interpolation_points_2d.hpp        | 10 +++++-
 .../src/spline_interpolation_points_2d.cpp    | 36 +++++++++----------
 2 files changed, 27 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);
+}