add interpolation for mpc controller.

src/common/include/interpolation/interpolation_utils.hpp

@@ -0,0 +1,114 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_
+#define INTERPOLATION__INTERPOLATION_UTILS_HPP_
+
+#include <algorithm>
+#include <array>
+#include <stdexcept>
+#include <vector>
+
+namespace interpolation_utils
+{
+inline bool isIncreasing(const std::vector<double> & x)
+{
+  if (x.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    if (x.at(i) >= x.at(i + 1)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+inline bool isNotDecreasing(const std::vector<double> & x)
+{
+  if (x.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    if (x.at(i) > x.at(i + 1)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+inline std::vector<double> validateKeys(
+  const std::vector<double> & base_keys, const std::vector<double> & query_keys)
+{
+  // when vectors are empty
+  if (base_keys.empty() || query_keys.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()));
+  }
+
+  // when indices are not sorted
+  if (!isIncreasing(base_keys) || !isNotDecreasing(query_keys)) {
+    throw std::invalid_argument("Either base_keys or query_keys is not sorted.");
+  }
+
+  // when query_keys is out of base_keys (This function does not allow exterior division.)
+  constexpr double epsilon = 1e-3;
+  if (
+    query_keys.front() < base_keys.front() - epsilon ||
+    base_keys.back() + epsilon < query_keys.back()) {
+    throw std::invalid_argument("query_keys is out of base_keys");
+  }
+
+  // NOTE: Due to calculation error of double, a query key may be slightly out of base keys.
+  //       Therefore, query keys are cropped here.
+  auto validated_query_keys = query_keys;
+  validated_query_keys.front() = std::max(validated_query_keys.front(), base_keys.front());
+  validated_query_keys.back() = std::min(validated_query_keys.back(), base_keys.back());
+
+  return validated_query_keys;
+}
+
+template <class T>
+void validateKeysAndValues(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values)
+{
+  // when vectors are empty
+  if (base_keys.empty() || base_values.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2 || base_values.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
+      ", base_values.size() = " + std::to_string(base_values.size()));
+  }
+
+  // when sizes of indices and values are not same
+  if (base_keys.size() != base_values.size()) {
+    throw std::invalid_argument("The size of base_keys and base_values are not the same.");
+  }
+}
+}  // namespace interpolation_utils
+
+#endif  // INTERPOLATION__INTERPOLATION_UTILS_HPP_

src/common/include/interpolation/linear_interpolation.hpp

@@ -0,0 +1,36 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+
+#include "interpolation/interpolation_utils.hpp"
+
+#include <vector>
+
+namespace interpolation
+{
+double lerp(const double src_val, const double dst_val, const double ratio);
+
+std::vector<double> lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys);
+
+double lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const double query_key);
+
+}  // namespace interpolation
+
+#endif  // INTERPOLATION__LINEAR_INTERPOLATION_HPP_

src/common/include/interpolation/spherical_linear_interpolation.hpp

@@ -0,0 +1,41 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+#define INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+
+#include "interpolation/interpolation_utils.hpp"
+
+//#include <geometry_msgs/msg/quaternion.hpp>
+
+#include "ivtype/ivpose.h"
+
+
+
+
+#include <vector>
+
+namespace interpolation
+{
+iv::ADCQuaternion slerp(
+  const iv::ADCQuaternion & src_quat, const iv::ADCQuaternion & dst_quat,
+  const double ratio);
+
+std::vector<iv::ADCQuaternion> slerp(
+  const std::vector<double> & base_keys,
+  const std::vector<iv::ADCQuaternion> & base_values,
+  const std::vector<double> & query_keys);
+}  // namespace interpolation
+
+#endif  // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_

src/common/include/interpolation/spline_interpolation.hpp

@@ -0,0 +1,108 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_
+#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_
+
+#include "interpolation/interpolation_utils.hpp"
+//#include "tier4_autoware_utils/geometry/geometry.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <numeric>
+#include <vector>
+
+namespace interpolation
+{
+// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1)
+struct MultiSplineCoef
+{
+  MultiSplineCoef() = default;
+
+  explicit MultiSplineCoef(const size_t num_spline)
+  {
+    a.resize(num_spline);
+    b.resize(num_spline);
+    c.resize(num_spline);
+    d.resize(num_spline);
+  }
+
+  std::vector<double> a;
+  std::vector<double> b;
+  std::vector<double> c;
+  std::vector<double> d;
+};
+
+// static spline interpolation functions
+std::vector<double> spline(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys);
+std::vector<double> splineByAkima(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys);
+}  // namespace interpolation
+
+// non-static 1-dimensional spline interpolation
+//
+// Usage:
+// ```
+// SplineInterpolation spline;
+// // memorize pre-interpolation result internally
+// spline.calcSplineCoefficients(base_keys, base_values);
+// const auto interpolation_result1 = spline.getSplineInterpolatedValues(
+//   base_keys, query_keys1);
+// const auto interpolation_result2 = spline.getSplineInterpolatedValues(
+//   base_keys, query_keys2);
+// ```
+class SplineInterpolation
+{
+public:
+  SplineInterpolation() = default;
+  SplineInterpolation(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values)
+  {
+    calcSplineCoefficients(base_keys, base_values);
+  }
+
+  //!< @brief get values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be x(t) vector
+  std::vector<double> getSplineInterpolatedValues(const std::vector<double> & query_keys) const;
+
+  //!< @brief get 1st differential values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be dx/dt(t) vector
+  std::vector<double> getSplineInterpolatedDiffValues(const std::vector<double> & query_keys) const;
+
+  //!< @brief get 2nd differential values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be d^2/dt^2(t) vector
+  std::vector<double> getSplineInterpolatedQuadDiffValues(
+    const std::vector<double> & query_keys) const;
+
+  size_t getSize() const { return base_keys_.size(); }
+
+private:
+  std::vector<double> base_keys_;
+  interpolation::MultiSplineCoef multi_spline_coef_;
+
+  void calcSplineCoefficients(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values);
+};
+
+#endif  // INTERPOLATION__SPLINE_INTERPOLATION_HPP_

src/common/include/interpolation/spline_interpolation_points_2d.hpp

@@ -0,0 +1,93 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
+#define INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
+
+#include "interpolation/spline_interpolation.hpp"
+
+#include <vector>
+#include "ivtype/ivpose.h"
+
+namespace interpolation
+{
+std::array<std::vector<double>, 3> slerp2dFromXY(
+  const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
+  const std::vector<double> & base_y_values, const std::vector<double> & query_keys);
+
+template <typename T>
+std::vector<double> splineYawFromPoints(const std::vector<T> & points);
+}  // namespace interpolation
+
+// non-static points spline interpolation
+// NOTE: We can calculate yaw from the x and y by interpolation derivatives.
+//
+// Usage:
+// ```
+// SplineInterpolationPoints2d spline;
+// // memorize pre-interpolation result internally
+// spline.calcSplineCoefficients(base_keys, base_values);
+// const auto interpolation_result1 = spline.getSplineInterpolatedPoint(
+//   base_keys, query_keys1);
+// const auto interpolation_result2 = spline.getSplineInterpolatedPoint(
+//   base_keys, query_keys2);
+// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw(
+//   base_keys, query_keys1);
+// ```
+class SplineInterpolationPoints2d
+{
+public:
+  SplineInterpolationPoints2d() = default;
+  explicit SplineInterpolationPoints2d(const std::vector<iv::ADCPoint> & points)
+  {
+      std::vector<iv::ADCPoint> points_inner;
+    for (const auto & p : points) {
+      points_inner.push_back(p);
+    }
+    calcSplineCoefficientsInner(points_inner);
+  }
+
+  // TODO(murooka) implement these functions
+  // std::vector<geometry_msgs::msg::Point> getSplineInterpolatedPoints(const double width);
+  // std::vector<geometry_msgs::msg::Pose> getSplineInterpolatedPoses(const double width);
+
+  // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw)
+  iv::ADCPose getSplineInterpolatedPose(const size_t idx, const double s) const;
+
+  // point
+  iv::ADCPoint getSplineInterpolatedPoint(const size_t idx, const double s) const;
+
+  // yaw
+  double getSplineInterpolatedYaw(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedYaws() const;
+
+  // curvature
+  double getSplineInterpolatedCurvature(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedCurvatures() const;
+
+  size_t getSize() const { return base_s_vec_.size(); }
+  size_t getOffsetIndex(const size_t idx, const double offset) const;
+  double getAccumulatedLength(const size_t idx) const;
+
+private:
+  void calcSplineCoefficientsInner(const std::vector<iv::ADCPoint> & points);
+  iv::ADCQuaternion  createQuaternionFromYaw(double yaw) const;
+  SplineInterpolation spline_x_;
+  SplineInterpolation spline_y_;
+  SplineInterpolation spline_z_;
+
+  std::vector<double> base_s_vec_;
+};
+
+#endif  // INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_

src/common/include/interpolation/zero_order_hold.hpp

@@ -0,0 +1,81 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+#define INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+
+#include "interpolation/interpolation_utils.hpp"
+
+#include <vector>
+
+namespace interpolation
+{
+inline std::vector<size_t> calc_closest_segment_indices(
+  const std::vector<double> & base_keys, const std::vector<double> & query_keys,
+  const double overlap_threshold = 1e-3)
+{
+  // throw exception for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys);
+
+  std::vector<size_t> closest_segment_indices(validated_query_keys.size());
+  size_t closest_segment_idx = 0;
+  for (size_t i = 0; i < validated_query_keys.size(); ++i) {
+    // Check if query_key is closes to the terminal point of the base keys
+    if (base_keys.back() - overlap_threshold < validated_query_keys.at(i)) {
+      closest_segment_idx = base_keys.size() - 1;
+    } else {
+      for (size_t j = base_keys.size() - 1; j > closest_segment_idx; --j) {
+        if (
+          base_keys.at(j - 1) - overlap_threshold < validated_query_keys.at(i) &&
+          validated_query_keys.at(i) < base_keys.at(j)) {
+          // find closest segment in base keys
+          closest_segment_idx = j - 1;
+          break;
+        }
+      }
+    }
+
+    closest_segment_indices.at(i) = closest_segment_idx;
+  }
+
+  return closest_segment_indices;
+}
+
+template <class T>
+std::vector<T> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values,
+  const std::vector<size_t> & closest_segment_indices)
+{
+  // throw exception for invalid arguments
+  interpolation_utils::validateKeysAndValues(base_keys, base_values);
+
+  std::vector<T> query_values(closest_segment_indices.size());
+  for (size_t i = 0; i < closest_segment_indices.size(); ++i) {
+    query_values.at(i) = base_values.at(closest_segment_indices.at(i));
+  }
+
+  return query_values;
+}
+
+template <class T>
+std::vector<T> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values,
+  const std::vector<double> & query_keys, const double overlap_threshold = 1e-3)
+{
+  return zero_order_hold(
+    base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold));
+}
+}  // namespace interpolation
+
+#endif  // INTERPOLATION__ZERO_ORDER_HOLD_HPP_

src/common/include/ivtype/ivpose.h

@@ -0,0 +1,321 @@
+#ifndef IVPOSE_H
+#define IVPOSE_H
+
+#include <math.h>
+
+#include <assert.h>
+#include <eigen3/Eigen/Eigen>
+
+namespace  iv {
+class ADCQuaternion
+{
+public:
+    double w;
+    double x;
+    double y;
+    double z;
+
+    ADCQuaternion(double xv,double yv,double zv,double wv)
+    {
+        x = xv;y = yv; z = zv; w = wv;
+    }
+
+    ADCQuaternion()
+    {
+
+    }
+
+    inline void	setValue(const double & xv, const double & yv, const double& zv,const double& wv)
+    {
+        x=xv;
+        y=yv;
+        z=zv;
+        w=wv;
+    }
+
+    /**@brief Add two quaternions
+   * @param q The quaternion to add to this one */
+    inline ADCQuaternion& operator+=(const ADCQuaternion& q)
+    {
+        x += q.x; y += q.y; z += q.z; w += q.w;
+        return *this;
+    }
+
+
+
+    /**@brief Sutf2ract out a quaternion
+   * @param q The quaternion to sutf2ract from this one */
+    ADCQuaternion& operator-=(const ADCQuaternion& q)
+    {
+        x -= q.x; y -= q.y; z -= q.z; w -= q.w;
+        return *this;
+    }
+
+    /**@brief Scale this quaternion
+   * @param s The scalar to scale by */
+    ADCQuaternion& operator*=(const double& s)
+    {
+        x *= s; y *= s; z *= s; w *= s;
+        return *this;
+    }
+
+    /**@brief Multiply this quaternion by q on the right
+   * @param q The other quaternion
+   * Equivilant to this = this * q */
+    ADCQuaternion& operator*=(const ADCQuaternion& q)
+    {
+        setValue(w * q.x + x * q.w + y * q.z - z * q.y,
+                 w * q.y + y * q.w + z * q.x - x * q.z,
+                 w * q.z + z * q.w + x * q.y - y * q.x,
+                 w * q.w - x * q.x - y * q.y - z * q.z);
+        return *this;
+    }
+
+    double dot(const ADCQuaternion & q) const
+    {
+        return x * q.x  + y * q.y + z * q.z + w * q.w;
+    }
+
+    /**@brief Return the length squared of the quaternion */
+    double length2() const
+    {
+        return dot(*this);
+    }
+
+    /**@brief Return the length of the quaternion */
+    double length() const
+    {
+        return sqrt(length2());
+    }
+
+    /**@brief Normalize the quaternion
+   * Such that x^2 + y^2 + z^2 +w^2 = 1 */
+    ADCQuaternion& normalize()
+    {
+        return *this /= length();
+    }
+
+    /**@brief Return a scaled version of this quaternion
+   * @param s The scale factor */
+    inline ADCQuaternion
+    operator*(const double& s) const
+    {
+        return ADCQuaternion(x * s, y * s, z * s, w * s);
+    }
+
+
+    /**@brief Return an inversely scaled versionof this quaternion
+   * @param s The inverse scale factor */
+    ADCQuaternion operator/(const double& s) const
+    {
+        assert(s != 0.0);
+        return *this * (1.0 / s);
+    }
+
+    /**@brief Inversely scale this quaternion
+   * @param s The scale factor */
+    ADCQuaternion& operator/=(const double& s)
+    {
+        assert(s != 0.0);
+        return *this *= 1.0 / s;
+    }
+
+    /**@brief Return a normalized version of this quaternion */
+    ADCQuaternion normalized() const
+    {
+        return *this / length();
+    }
+
+    double ADCAcos(double val) const
+    {
+        if(val<-1)val = -1;
+        if(val>1)val = 1;
+        return acos(val);
+    }
+
+    double ADCAsin(double val) const
+    {
+        if(val<-1)val = -1;
+        if(val>1)val = 1;
+        return asin(val);
+    }
+
+    /**@brief Return the ***half*** angle between this quaternion and the other
+   * @param q The other quaternion */
+    double angle(const ADCQuaternion& q) const
+    {
+        double s = sqrt(length2() * q.length2());
+        assert(s != 0.0);
+        double val = dot(q)/s;
+        if(val<-1)val = -1;
+        if(val>1)val = 1;
+        return acos(val);
+    }
+
+    /**@brief Return the angle between this quaternion and the other along the shortest path
+    * @param q The other quaternion */
+    double angleShortestPath(const ADCQuaternion& q) const
+    {
+        double s = sqrt(length2() * q.length2());
+        assert(s != 0.0);
+        if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+            return ADCAcos(dot(-q) / s) * 2.0;
+        else
+            return ADCAcos(dot(q) / s) * 2.0;
+    }
+
+    /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */
+    double getAngle() const
+    {
+        double s = 2.0 * ADCAcos(w);
+        return s;
+    }
+
+    /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */
+    double getAngleShortestPath() const
+    {
+        double s;
+        if (w >= 0)
+            s =  2.0 * ADCAcos(w);
+        else
+            s = 2.0 * ADCAcos(-w);
+
+        return s;
+    }
+
+
+
+    /**@brief Return the inverse of this quaternion */
+    ADCQuaternion inverse() const
+    {
+        return ADCQuaternion(-x, -y, -z, w);
+    }
+
+    /**@brief Return the sum of this quaternion and the other
+   * @param q2 The other quaternion */
+    inline ADCQuaternion
+    operator+(const ADCQuaternion& q2) const
+    {
+        const ADCQuaternion& q1 = *this;
+        return ADCQuaternion(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w);
+    }
+
+    /**@brief Return the difference between this quaternion and the other
+   * @param q2 The other quaternion */
+    inline ADCQuaternion
+    operator-(const ADCQuaternion& q2) const
+    {
+        const ADCQuaternion& q1 = *this;
+        return ADCQuaternion(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w);
+    }
+
+    /**@brief Return the negative of this quaternion
+   * This simply negates each element */
+    inline ADCQuaternion operator-() const
+    {
+        const ADCQuaternion& q2 = *this;
+        return ADCQuaternion( - q2.x, - q2.y,  - q2.z,  - q2.w);
+    }
+    /**@todo document this and it's use */
+    inline ADCQuaternion farthest( const ADCQuaternion& qd) const
+    {
+        ADCQuaternion diff,sum;
+        diff = *this - qd;
+        sum = *this + qd;
+        if( diff.dot(diff) > sum.dot(sum) )
+            return qd;
+        return (-qd);
+    }
+
+    /**@todo document this and it's use */
+    inline ADCQuaternion nearest( const ADCQuaternion& qd) const
+    {
+        ADCQuaternion diff,sum;
+        diff = *this - qd;
+        sum = *this + qd;
+        if( diff.dot(diff) < sum.dot(sum) )
+            return qd;
+        return (-qd);
+    }
+
+
+    /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
+   * @param q The other quaternion to interpolate with
+   * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
+   * Slerp interpolates assuming constant velocity.  */
+    ADCQuaternion slerp(const ADCQuaternion& q, const double& t) const
+    {
+        double theta = angleShortestPath(q) /2.0;
+        if (theta != 0.0)
+        {
+            double d = 1.0 / sin(theta);
+            double s0 = sin((1.0 - t) * theta);
+            double s1 = sin(t * theta);
+            if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+                return ADCQuaternion((x * s0 + -q.x * s1) * d,
+                                  (y * s0 + -q.y * s1) * d,
+                                  (z * s0 + -q.z * s1) * d,
+                                  (w * s0 + -q.w * s1) * d);
+            else
+                return ADCQuaternion((x * s0 + q.x * s1) * d,
+                                  (y * s0 + q.y * s1) * d,
+                                  (z * s0 + q.z * s1) * d,
+                                  (w * s0 + q.w * s1) * d);
+
+        }
+        else
+        {
+            return *this;
+        }
+    }
+
+    static const ADCQuaternion&	getIdentity()
+    {
+        static const ADCQuaternion identityQuat(0,0,0,1);
+        return identityQuat;
+    }
+
+    inline const double& getW() const { return w; }
+
+
+
+};
+
+class ADCPosition
+{
+public:
+    double x;
+    double y;
+    double z;
+    ADCPosition(){}
+    ADCPosition(double xv,double yv,double zv)
+    {
+        x = xv;
+        y = yv;
+        z = zv;
+    }
+};
+
+class ADCPoint
+{
+public:
+    double x;
+    double y;
+    double z;
+    ADCPoint(){}
+    ADCPoint(double xv,double yv,double zv)
+    {
+        x = xv;
+        y = yv;
+        z = zv;
+    }
+};
+
+struct ADCPose
+{
+    ADCPoint position;
+    ADCQuaternion orientation;
+};
+}
+
+#endif

src/common/interpolation/.gitignore

@@ -0,0 +1,74 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+CMakeLists.txt.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/common/interpolation/interpolation.cpp

@@ -0,0 +1,5 @@
+#include "interpolation.h"
+
+Interpolation::Interpolation()
+{
+}

src/common/interpolation/interpolation.h

@@ -0,0 +1,12 @@
+#ifndef INTERPOLATION_H
+#define INTERPOLATION_H
+
+#include "interpolation_global.h"
+
+class INTERPOLATION_EXPORT Interpolation
+{
+public:
+    Interpolation();
+};
+
+#endif // INTERPOLATION_H

src/common/interpolation/interpolation.pro

@@ -0,0 +1,35 @@
+CONFIG -= qt
+
+TEMPLATE = lib
+DEFINES += INTERPOLATION_LIBRARY
+
+CONFIG += c++17
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    interpolation.cpp \
+    linear_interpolation.cpp \
+    spherical_linear_interpolation.cpp \
+    spline_interpolation.cpp \
+    spline_interpolation_points_2d.cpp
+
+HEADERS += \
+    ../include/interpolation/interpolation_utils.hpp \
+    ../include/interpolation/linear_interpolation.hpp \
+    ../include/interpolation/spherical_linear_interpolation.hpp \
+    ../include/interpolation/spline_interpolation.hpp \
+    ../include/interpolation/spline_interpolation_points_2d.hpp \
+    ../include/interpolation/zero_order_hold.hpp \
+    ../include/ivtype/ivpose.h \
+    interpolation.h
+
+# Default rules for deployment.
+unix {
+    target.path = /usr/lib
+}
+!isEmpty(target.path): INSTALLS += target
+
+INCLUDEPATH += $$PWD/../include

src/common/interpolation/interpolation_global.h

@@ -0,0 +1,18 @@
+#ifndef INTERPOLATION_GLOBAL_H
+#define INTERPOLATION_GLOBAL_H
+
+#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+#  define Q_DECL_EXPORT __declspec(dllexport)
+#  define Q_DECL_IMPORT __declspec(dllimport)
+#else
+#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
+#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
+#endif
+
+#if defined(INTERPOLATION_LIBRARY)
+#  define INTERPOLATION_EXPORT Q_DECL_EXPORT
+#else
+#  define INTERPOLATION_EXPORT Q_DECL_IMPORT
+#endif
+
+#endif // INTERPOLATION_GLOBAL_H

src/common/interpolation/linear_interpolation.cpp

@@ -0,0 +1,59 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "interpolation/linear_interpolation.hpp"
+
+#include <vector>
+
+namespace interpolation
+{
+double lerp(const double src_val, const double dst_val, const double ratio)
+{
+  return src_val + (dst_val - src_val) * ratio;
+}
+
+std::vector<double> lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys)
+{
+  // throw exception for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys);
+  interpolation_utils::validateKeysAndValues(base_keys, base_values);
+
+  // calculate linear interpolation
+  std::vector<double> query_values;
+  size_t key_index = 0;
+  for (const auto query_key : validated_query_keys) {
+    while (base_keys.at(key_index + 1) < query_key) {
+      ++key_index;
+    }
+
+    const double src_val = base_values.at(key_index);
+    const double dst_val = base_values.at(key_index + 1);
+    const double ratio = (query_key - base_keys.at(key_index)) /
+                         (base_keys.at(key_index + 1) - base_keys.at(key_index));
+
+    const double interpolated_val = lerp(src_val, dst_val, ratio);
+    query_values.push_back(interpolated_val);
+  }
+
+  return query_values;
+}
+
+double lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values, double query_key)
+{
+  return lerp(base_keys, base_values, std::vector<double>{query_key}).front();
+}
+}  // namespace interpolation

src/common/interpolation/spherical_linear_interpolation.cpp

@@ -0,0 +1,55 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "interpolation/spherical_linear_interpolation.hpp"
+
+namespace interpolation
+{
+iv::ADCQuaternion slerp(
+  const iv::ADCQuaternion & src_quat, const iv::ADCQuaternion & dst_quat,
+  const double ratio)
+{
+    return src_quat.slerp(dst_quat,ratio);
+}
+
+std::vector<iv::ADCQuaternion> slerp(
+  const std::vector<double> & base_keys,
+  const std::vector<iv::ADCQuaternion> & base_values,
+  const std::vector<double> & query_keys)
+{
+  // throw exception for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys);
+  interpolation_utils::validateKeysAndValues(base_keys, base_values);
+
+  // calculate linear interpolation
+  std::vector<iv::ADCQuaternion> query_values;
+  size_t key_index = 0;
+  for (const auto query_key : validated_query_keys) {
+    while (base_keys.at(key_index + 1) < query_key) {
+      ++key_index;
+    }
+
+    const auto src_quat = base_values.at(key_index);
+    const auto dst_quat = base_values.at(key_index + 1);
+    const double ratio = (query_key - base_keys.at(key_index)) /
+                         (base_keys.at(key_index + 1) - base_keys.at(key_index));
+
+    const auto interpolated_quat = slerp(src_quat, dst_quat, ratio);
+    query_values.push_back(interpolated_quat);
+  }
+
+  return query_values;
+}
+
+}  // namespace interpolation

src/common/interpolation/spline_interpolation.cpp

@@ -0,0 +1,289 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "interpolation/spline_interpolation.hpp"
+
+#include <vector>
+
+namespace
+{
+// solve Ax = d
+// where A is tridiagonal matrix
+//     [b_0 c_0 ...                       ]
+//     [a_0 b_1 c_1 ...               O   ]
+// A = [            ...                   ]
+//     [   O         ... a_N-3 b_N-2 c_N-2]
+//     [                   ... a_N-2 b_N-1]
+struct TDMACoef
+{
+  explicit TDMACoef(const size_t num_row)
+  {
+    a.resize(num_row - 1);
+    b.resize(num_row);
+    c.resize(num_row - 1);
+    d.resize(num_row);
+  }
+
+  std::vector<double> a;
+  std::vector<double> b;
+  std::vector<double> c;
+  std::vector<double> d;
+};
+
+inline std::vector<double> solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma_coef)
+{
+  const auto & a = tdma_coef.a;
+  const auto & b = tdma_coef.b;
+  const auto & c = tdma_coef.c;
+  const auto & d = tdma_coef.d;
+
+  const size_t num_row = b.size();
+
+  std::vector<double> x(num_row);
+  if (num_row != 1) {
+    // calculate p and q
+    std::vector<double> p;
+    std::vector<double> q;
+    p.push_back(-c[0] / b[0]);
+    q.push_back(d[0] / b[0]);
+
+    for (size_t i = 1; i < num_row; ++i) {
+      const double den = b[i] + a[i - 1] * p[i - 1];
+      p.push_back(-c[i - 1] / den);
+      q.push_back((d[i] - a[i - 1] * q[i - 1]) / den);
+    }
+
+    // calculate solution
+    x[num_row - 1] = q[num_row - 1];
+
+    for (size_t i = 1; i < num_row; ++i) {
+      const size_t j = num_row - 1 - i;
+      x[j] = p[j] * x[j + 1] + q[j];
+    }
+  } else {
+    x[0] = (d[0] / b[0]);
+  }
+
+  return x;
+}
+}  // namespace
+
+namespace interpolation
+{
+std::vector<double> spline(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys)
+{
+  // calculate spline coefficients
+  SplineInterpolation interpolator(base_keys, base_values);
+
+  // interpolate base_keys at query_keys
+  return interpolator.getSplineInterpolatedValues(query_keys);
+}
+
+std::vector<double> splineByAkima(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys)
+{
+  constexpr double epsilon = 1e-5;
+
+  // calculate m
+  std::vector<double> m_values;
+  for (size_t i = 0; i < base_keys.size() - 1; ++i) {
+    const double m_val =
+      (base_values.at(i + 1) - base_values.at(i)) / (base_keys.at(i + 1) - base_keys.at(i));
+    m_values.push_back(m_val);
+  }
+
+  // calculate s
+  std::vector<double> s_values;
+  for (size_t i = 0; i < base_keys.size(); ++i) {
+    if (i == 0) {
+      s_values.push_back(m_values.front());
+      continue;
+    } else if (i == base_keys.size() - 1) {
+      s_values.push_back(m_values.back());
+      continue;
+    } else if (i == 1 || i == base_keys.size() - 2) {
+      const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0;
+      s_values.push_back(s_val);
+      continue;
+    }
+
+    const double denom = std::abs(m_values.at(i + 1) - m_values.at(i)) +
+                         std::abs(m_values.at(i - 1) - m_values.at(i - 2));
+    if (std::abs(denom) < epsilon) {
+      const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0;
+      s_values.push_back(s_val);
+      continue;
+    }
+
+    const double s_val = (std::abs(m_values.at(i + 1) - m_values.at(i)) * m_values.at(i - 1) +
+                          std::abs(m_values.at(i - 1) - m_values.at(i - 2)) * m_values.at(i)) /
+                         denom;
+    s_values.push_back(s_val);
+  }
+
+  // calculate cubic coefficients
+  std::vector<double> a;
+  std::vector<double> b;
+  std::vector<double> c;
+  std::vector<double> d;
+  for (size_t i = 0; i < base_keys.size() - 1; ++i) {
+    a.push_back(
+      (s_values.at(i) + s_values.at(i + 1) - 2.0 * m_values.at(i)) /
+      std::pow(base_keys.at(i + 1) - base_keys.at(i), 2));
+    b.push_back(
+      (3.0 * m_values.at(i) - 2.0 * s_values.at(i) - s_values.at(i + 1)) /
+      (base_keys.at(i + 1) - base_keys.at(i)));
+    c.push_back(s_values.at(i));
+    d.push_back(base_values.at(i));
+  }
+
+  // interpolate
+  std::vector<double> res;
+  size_t j = 0;
+  for (const auto & query_key : query_keys) {
+    while (base_keys.at(j + 1) < query_key) {
+      ++j;
+    }
+
+    const double ds = query_key - base_keys.at(j);
+    res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds);
+  }
+  return res;
+}
+}  // namespace interpolation
+
+void SplineInterpolation::calcSplineCoefficients(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values)
+{
+  // throw exceptions for invalid arguments
+  interpolation_utils::validateKeysAndValues(base_keys, base_values);
+
+  const size_t num_base = base_keys.size();  // N+1
+
+  std::vector<double> diff_keys;    // N
+  std::vector<double> diff_values;  // N
+  for (size_t i = 0; i < num_base - 1; ++i) {
+    diff_keys.push_back(base_keys.at(i + 1) - base_keys.at(i));
+    diff_values.push_back(base_values.at(i + 1) - base_values.at(i));
+  }
+
+  std::vector<double> v = {0.0};
+  if (num_base > 2) {
+    // solve tridiagonal matrix algorithm
+    TDMACoef tdma_coef(num_base - 2);  // N-1
+
+    for (size_t i = 0; i < num_base - 2; ++i) {
+      tdma_coef.b[i] = 2 * (diff_keys[i] + diff_keys[i + 1]);
+      if (i != num_base - 3) {
+        tdma_coef.a[i] = diff_keys[i + 1];
+        tdma_coef.c[i] = diff_keys[i + 1];
+      }
+      tdma_coef.d[i] =
+        6.0 * (diff_values[i + 1] / diff_keys[i + 1] - diff_values[i] / diff_keys[i]);
+    }
+
+    const std::vector<double> tdma_res = solveTridiagonalMatrixAlgorithm(tdma_coef);
+
+    // calculate v
+    v.insert(v.end(), tdma_res.begin(), tdma_res.end());
+  }
+  v.push_back(0.0);
+
+  // calculate a, b, c, d of spline coefficients
+  multi_spline_coef_ = interpolation::MultiSplineCoef{num_base - 1};  // N
+  for (size_t i = 0; i < num_base - 1; ++i) {
+    multi_spline_coef_.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i];
+    multi_spline_coef_.b[i] = v[i] / 2.0;
+    multi_spline_coef_.c[i] =
+      diff_values[i] / diff_keys[i] - diff_keys[i] * (2 * v[i] + v[i + 1]) / 6.0;
+    multi_spline_coef_.d[i] = base_values[i];
+  }
+
+  base_keys_ = base_keys;
+}
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys);
+
+  const auto & a = multi_spline_coef_.a;
+  const auto & b = multi_spline_coef_.b;
+  const auto & c = multi_spline_coef_.c;
+  const auto & d = multi_spline_coef_.d;
+
+  std::vector<double> res;
+  size_t j = 0;
+  for (const auto & query_key : validated_query_keys) {
+    while (base_keys_.at(j + 1) < query_key) {
+      ++j;
+    }
+
+    const double ds = query_key - base_keys_.at(j);
+    res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds);
+  }
+
+  return res;
+}
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedDiffValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys);
+
+  const auto & a = multi_spline_coef_.a;
+  const auto & b = multi_spline_coef_.b;
+  const auto & c = multi_spline_coef_.c;
+
+  std::vector<double> res;
+  size_t j = 0;
+  for (const auto & query_key : validated_query_keys) {
+    while (base_keys_.at(j + 1) < query_key) {
+      ++j;
+    }
+
+    const double ds = query_key - base_keys_.at(j);
+    res.push_back(c.at(j) + (2.0 * b.at(j) + 3.0 * a.at(j) * ds) * ds);
+  }
+
+  return res;
+}
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedQuadDiffValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys);
+
+  const auto & a = multi_spline_coef_.a;
+  const auto & b = multi_spline_coef_.b;
+
+  std::vector<double> res;
+  size_t j = 0;
+  for (const auto & query_key : validated_query_keys) {
+    while (base_keys_.at(j + 1) < query_key) {
+      ++j;
+    }
+
+    const double ds = query_key - base_keys_.at(j);
+    res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds);
+  }
+
+  return res;
+}

src/common/interpolation/spline_interpolation_points_2d.cpp

@@ -0,0 +1,261 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "interpolation/spline_interpolation_points_2d.hpp"
+
+#include <vector>
+
+namespace
+{
+std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vector<double> & y)
+{
+  if (x.size() != y.size()) {
+    return std::vector<double>{};
+  }
+
+  std::vector<double> dist_v;
+  dist_v.push_back(0.0);
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    const double dx = x.at(i + 1) - x.at(i);
+    const double dy = y.at(i + 1) - y.at(i);
+    dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy));
+  }
+
+  return dist_v;
+}
+
+std::array<std::vector<double>, 4> getBaseValues(
+    const std::vector<iv::ADCPoint> & points)
+{
+  // calculate x, y
+  std::vector<double> base_x;
+  std::vector<double> base_y;
+  std::vector<double> base_z;
+  for (size_t i = 0; i < points.size(); i++) {
+    const auto & current_pos = points.at(i);
+    if (i > 0) {
+      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) {
+        continue;
+      }
+    }
+    base_x.push_back(current_pos.x);
+    base_y.push_back(current_pos.y);
+    base_z.push_back(current_pos.z);
+  }
+
+  // calculate base_keys, base_values
+  if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) {
+    throw std::logic_error("The number of unique points is not enough.");
+  }
+
+  const std::vector<double> base_s = calcEuclidDist(base_x, base_y);
+
+  return {base_s, base_x, base_y, base_z};
+}
+}  // namespace
+
+namespace interpolation
+{
+
+std::array<std::vector<double>, 3> slerp2dFromXY(
+  const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
+  const std::vector<double> & base_y_values, const std::vector<double> & query_keys)
+{
+  // calculate spline coefficients
+  SplineInterpolation interpolator_x(base_keys, base_x_values);
+  SplineInterpolation interpolator_y(base_keys, base_y_values);
+  const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys);
+  const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys);
+
+  // calculate yaw
+  std::vector<double> yaw_vec;
+  for (size_t i = 0; i < diff_x.size(); i++) {
+    double yaw = std::atan2(diff_y[i], diff_x[i]);
+    yaw_vec.push_back(yaw);
+  }
+  // interpolate base_keys at query_keys
+  return {
+    interpolator_x.getSplineInterpolatedValues(query_keys),
+    interpolator_y.getSplineInterpolatedValues(query_keys), yaw_vec};
+}
+
+template <typename T>
+std::vector<double> splineYawFromPoints(const std::vector<T> & points)
+{
+  // calculate spline coefficients
+  SplineInterpolationPoints2d interpolator(points);
+
+  // interpolate base_keys at query_keys
+  std::vector<double> yaw_vec;
+  for (size_t i = 0; i < points.size(); ++i) {
+    const double yaw = interpolator.getSplineInterpolatedYaw(i, 0.0);
+    yaw_vec.push_back(yaw);
+  }
+  return yaw_vec;
+}
+/*template std::vector<double> splineYawFromPoints(
+  const std::vector<ADCPoint> & points);
+*/
+} // namespace interpolation
+
+
+iv::ADCQuaternion  SplineInterpolationPoints2d::createQuaternionFromYaw(double yaw) const
+{
+  double pitch = 0;
+  double roll = 0;
+  double cy = cos(yaw * 0.5);
+  double sy = sin(yaw * 0.5);
+  double cp = cos(pitch * 0.5);
+  double sp = sin(pitch * 0.5);
+  double cr = cos(roll * 0.5);
+  double sr = sin(roll * 0.5);
+
+  iv::ADCQuaternion q;
+  q.w = cy * cp * cr + sy * sp * sr;
+  q.x = cy * cp * sr - sy * sp * cr;
+  q.y = sy * cp * sr + cy * sp * cr;
+  q.z = sy * cp * cr - cy * sp * sr;
+
+  return q;
+}
+iv::ADCPose SplineInterpolationPoints2d::getSplineInterpolatedPose(
+  const size_t idx, const double s) const
+{
+  iv::ADCPose pose;
+  pose.position = getSplineInterpolatedPoint(idx, s);
+  pose.orientation =
+    createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s));
+  return pose;
+}
+
+iv::ADCPoint SplineInterpolationPoints2d::getSplineInterpolatedPoint(
+  const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+
+  double whole_s = base_s_vec_.at(idx) + s;
+  if (whole_s < base_s_vec_.front()) {
+    whole_s = base_s_vec_.front();
+  }
+  if (whole_s > base_s_vec_.back()) {
+    whole_s = base_s_vec_.back();
+  }
+
+  const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0);
+  const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0);
+  const double z = spline_z_.getSplineInterpolatedValues({whole_s}).at(0);
+
+  iv::ADCPoint geom_point;
+  geom_point.x = x;
+  geom_point.y = y;
+  geom_point.z = z;
+  return geom_point;
+}
+
+double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
+  const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+  const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+
+  return std::atan2(diff_y, diff_x);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedYaws() const
+{
+  std::vector<double> yaw_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double yaw = getSplineInterpolatedYaw(i, 0.0);
+    yaw_vec.push_back(yaw);
+  }
+  return yaw_vec;
+}
+
+double SplineInterpolationPoints2d::getSplineInterpolatedCurvature(
+  const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
+  const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+  const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+
+  const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+  const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+
+  return (diff_x * quad_diff_y - quad_diff_x * diff_y) /
+         std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const
+{
+  std::vector<double> curvature_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double curvature = getSplineInterpolatedCurvature(i, 0.0);
+    curvature_vec.push_back(curvature);
+  }
+  return curvature_vec;
+}
+
+size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const
+{
+  const double whole_s = base_s_vec_.at(idx) + offset;
+  for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) {
+    if (whole_s < base_s_vec_.at(s_idx)) {
+      return s_idx;
+    }
+  }
+  return base_s_vec_.size() - 1;
+}
+
+double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+  return base_s_vec_.at(idx);
+}
+
+void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
+    const std::vector<iv::ADCPoint> & 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);
+  const auto & base_z_vec = base.at(3);
+
+  // calculate spline coefficients
+  spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec);
+  spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec);
+  spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec);
+}
+
+

src/decition/mpccontroller/mpc_lateral_controller/mpc_utils.hpp

@@ -15,37 +15,64 @@
 #ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
 #define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
 
-#include "rclcpp/rclcpp.hpp"
-#include "tf2/utils.h"
+//#include "rclcpp/rclcpp.hpp"
+//#include "tf2/utils.h"
 
 #include <Eigen/Core>
 
-#ifdef ROS_DISTRO_GALACTIC
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#else
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#endif
+//#ifdef ROS_DISTRO_GALACTIC
+//#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+//#else
+//#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+//#endif
 
 #include "mpc_lateral_controller/mpc_trajectory.hpp"
 
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
+//#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+//#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+//#include "geometry_msgs/msg/pose_stamped.hpp"
+//#include "geometry_msgs/msg/twist_stamped.hpp"
 
 #include <cmath>
 #include <string>
 #include <utility>
 #include <vector>
 
+
+namespace  iv {
+struct Quaternion_
+{
+    double w;
+    double x;
+    double y;
+    double z;
+};
+
+struct Position
+{
+    double x;
+    double y;
+    double z;
+};
+
+struct ADCPose
+{
+    Position position;
+    Quaternion_ orientation;
+};
+}
+
+
 namespace autoware::motion::control::mpc_lateral_controller
 {
 namespace MPCUtils
 {
 
-using autoware_auto_planning_msgs::msg::Trajectory;
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-using geometry_msgs::msg::Pose;
+//using autoware_auto_planning_msgs::msg::Trajectory;
+//using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+//using geometry_msgs::msg::Pose;
+
+double ADCgetYaw(iv::Quaternion_ q);
 
 /**
  * @brief calculate 2d distance from trajectory[idx1] to trajectory[idx2]
@@ -69,7 +96,10 @@ void convertEulerAngleToMonotonic(std::vector<double> & angle_vector);
  * @param [in] ref_pose reference pose
  * @return lateral distance between the two poses
  */
-double calcLateralError(const Pose & ego_pose, const Pose & ref_pose);
+//double calcLateralError(const Pose & ego_pose, const Pose & ref_pose);
+
+double calcLateralError(const iv::ADCPose & ego_pose, const iv::ADCPose & ref_pose);
+
 
 /**
  * @brief convert the given Trajectory msg to a MPCTrajectory object

src/decition/mpccontroller/mpc_utils.cpp

@@ -46,6 +46,17 @@ namespace MPCUtils
 //using tier4_autoware_utils::createQuaternionFromYaw;
 //using tier4_autoware_utils::normalizeRadian;
 
+
+double ADCgetYaw(iv::Quaternion_ q)
+{
+  // yaw (z-axis rotation)
+  double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+  double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+  double yaw = std::atan2(siny_cosp, cosy_cosp);
+
+  return yaw;
+}
+
 double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)
 {
   const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2);
@@ -69,11 +80,20 @@ void convertEulerAngleToMonotonic(std::vector<double> & angle_vector)
   }
 }
 
-double calcLateralError(const Pose & ego_pose, const Pose & ref_pose)
+//double calcLateralError(const Pose & ego_pose, const Pose & ref_pose)
+//{
+//  const double err_x = ego_pose.position.x - ref_pose.position.x;
+//  const double err_y = ego_pose.position.y - ref_pose.position.y;
+//  const double ref_yaw = tf2::getYaw(ref_pose.orientation);
+//  const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y;
+//  return lat_err;
+//}
+
+double calcLateralError(const iv::ADCPose & ego_pose, const iv::ADCPose & ref_pose)
 {
   const double err_x = ego_pose.position.x - ref_pose.position.x;
   const double err_y = ego_pose.position.y - ref_pose.position.y;
-  const double ref_yaw = tf2::getYaw(ref_pose.orientation);
+  const double ref_yaw = ADCgetYaw(ref_pose.orientation);
   const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y;
   return lat_err;
 }

src/tool/map_lanetoxodr/view/myview.cpp

@@ -221,3 +221,33 @@ void MyView::pinchTriggered(QPinchGesture *gesture)
 
 }
 
+void MyView::keyPressEvent(QKeyEvent *event)
+{
+    //按键按下，key值放入容器，如果是长按触发的repeat就不判断
+    if(!event->isAutoRepeat())
+        mPressKeys.insert(event->key());
+
+    if(event->key() == Qt::Key_J)
+    {
+        emit CtrlManual(true);
+    }
+
+    if(event->key() == Qt::Key_K)
+    {
+        emit CtrlManual(false);
+    }
+    //   qDebug("key count is %d",mPressKeys.count());
+    //    QDateTime dt;
+    //    qDebug("key press %ld",QDateTime::currentMSecsSinceEpoch());
+    //    qDebug("  key is %d",event->key());
+
+}
+
+void MyView::keyReleaseEvent(QKeyEvent *event)
+{
+    if(!event->isAutoRepeat())mPressKeys.remove(event->key());
+    qDebug("key count is %d",mPressKeys.count());
+    //    QDateTime dt;
+    //    qDebug("key release %ld",QDateTime::currentMSecsSinceEpoch());
+}
+

src/tool/map_lanetoxodr/view/myview.h

@@ -29,6 +29,8 @@ protected:
     void mousePressEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
     void mouseReleaseEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
     void mouseDoubleClickEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
+    void keyPressEvent(QKeyEvent *event) Q_DECL_OVERRIDE;
+    void keyReleaseEvent(QKeyEvent *event) Q_DECL_OVERRIDE;
 
     bool event(QEvent *event) Q_DECL_OVERRIDE;
 public Q_SLOTS:
@@ -39,6 +41,12 @@ signals:
     void dbclickxy(double x,double y);
     void beishuchange(double beishu);
 
+signals:
+    void CtrlManual(bool bManual);
+
+public:
+    QSet<int> mPressKeys;
+
 private:
     bool bottonstatus = false;
     QPoint myview_lastMousePos;