feat(autoware_motion_utils): add spherical linear interpolator (autow…

…arefoundation#9175)

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp

@@ -18,5 +18,6 @@
 #include <autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp>
 #include <autoware/motion_utils/trajectory_container/interpolator/linear.hpp>
 #include <autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp>
+#include <autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp>
 #include <autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp>
 #endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp

@@ -49,7 +49,7 @@ struct InterpolatorMixin : public InterpolatorInterface<T>
   {
   private:
     std::vector<double> bases_;
-    std::vector<double> values_;
+    std::vector<T> values_;
 
   public:
     [[nodiscard]] Builder & set_bases(const Eigen::Ref<const Eigen::VectorXd> & bases)
@@ -64,18 +64,12 @@ struct InterpolatorMixin : public InterpolatorInterface<T>
       return *this;
     }
 
-    [[nodiscard]] Builder & set_values(const std::vector<double> & values)
+    [[nodiscard]] Builder & set_values(const std::vector<T> & values)
     {
       values_ = values;
       return *this;
     }
 
-    [[nodiscard]] Builder & set_values(const Eigen::Ref<const Eigen::VectorXd> & values)
-    {
-      values_ = std::vector<double>(values.begin(), values.end());
-      return *this;
-    }
-
     template <typename... Args>
     [[nodiscard]] std::optional<InterpolatorType> build(Args &&... args)
     {

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp

@@ -0,0 +1,73 @@
+// Copyright 2024 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 AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
+
+#include <geometry_msgs/msg/quaternion.hpp>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Class for SphericalLinear interpolation.
+ *
+ * This class provides methods to perform SphericalLinear interpolation on a set of data points.
+ */
+class SphericalLinear
+: public detail::InterpolatorMixin<SphericalLinear, geometry_msgs::msg::Quaternion>
+{
+private:
+  std::vector<geometry_msgs::msg::Quaternion> quaternions_;
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   * @return True if the interpolator was built successfully, false otherwise.
+   */
+  void build_impl(
+    const std::vector<double> & bases,
+    const std::vector<geometry_msgs::msg::Quaternion> & quaternions) override;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] geometry_msgs::msg::Quaternion compute_impl(const double & s) const override;
+
+public:
+  /**
+   * @brief Default constructor.
+   */
+  SphericalLinear() = default;
+
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override;
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_

common/autoware_motion_utils/src/trajectory_container/interpolator/spherical_linear.cpp

@@ -0,0 +1,64 @@
+// Copyright 2024 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 "autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp"
+
+#include <Eigen/Geometry>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+void SphericalLinear::build_impl(
+  const std::vector<double> & bases,
+  const std::vector<geometry_msgs::msg::Quaternion> & quaternions)
+{
+  this->bases_ = bases;
+  this->quaternions_ = quaternions;
+}
+
+geometry_msgs::msg::Quaternion SphericalLinear::compute_impl(const double & s) const
+{
+  const int32_t idx = this->get_index(s);
+  const double x0 = this->bases_.at(idx);
+  const double x1 = this->bases_.at(idx + 1);
+  const geometry_msgs::msg::Quaternion y0 = this->quaternions_.at(idx);
+  const geometry_msgs::msg::Quaternion y1 = this->quaternions_.at(idx + 1);
+
+  // Spherical linear interpolation (Slerp)
+  const double t = (s - x0) / (x1 - x0);
+
+  // Convert quaternions to Eigen vectors for calculation
+  Eigen::Quaterniond q0(y0.w, y0.x, y0.y, y0.z);
+  Eigen::Quaterniond q1(y1.w, y1.x, y1.y, y1.z);
+
+  // Perform Slerp
+  Eigen::Quaterniond q_slerp = q0.slerp(t, q1);
+
+  // Convert the result back to geometry_msgs::msg::Quaternion
+  geometry_msgs::msg::Quaternion result;
+  result.w = q_slerp.w();
+  result.x = q_slerp.x();
+  result.y = q_slerp.y();
+  result.z = q_slerp.z();
+
+  return result;
+}
+
+size_t SphericalLinear::minimum_required_points() const
+{
+  return 2;
+}
+}  // namespace autoware::motion_utils::trajectory_container::interpolator

common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp

@@ -12,8 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp"
+
 #include <autoware/motion_utils/trajectory_container/interpolator.hpp>
 
+#include <geometry_msgs/msg/quaternion.hpp>
+
 #include <gtest/gtest.h>
 
 #include <optional>
@@ -71,3 +75,50 @@ template class TestInterpolator<
   autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor<double>>;
 template class TestInterpolator<
   autoware::motion_utils::trajectory_container::interpolator::Stairstep<double>>;
+
+/*
+ * Test SphericalLinear interpolator
+ */
+
+geometry_msgs::msg::Quaternion create_quaternion(double w, double x, double y, double z)
+{
+  geometry_msgs::msg::Quaternion q;
+  q.w = w;
+  q.x = x;
+  q.y = y;
+  q.z = z;
+  return q;
+}
+
+TEST(TestSphericalLinearInterpolator, compute)
+{
+  using autoware::motion_utils::trajectory_container::interpolator::SphericalLinear;
+
+  std::vector<double> bases = {0.0, 1.0};
+  std::vector<geometry_msgs::msg::Quaternion> quaternions = {
+    create_quaternion(1.0, 0.0, 0.0, 0.0), create_quaternion(0.0, 1.0, 0.0, 0.0)};
+
+  auto interpolator =
+    autoware::motion_utils::trajectory_container::interpolator::SphericalLinear::Builder()
+      .set_bases(bases)
+      .set_values(quaternions)
+      .build();
+
+  if (!interpolator) {
+    FAIL();
+  }
+
+  double s = 0.5;
+  geometry_msgs::msg::Quaternion result = interpolator->compute(s);
+
+  // Expected values (from SLERP calculation)
+  double expected_w = std::sqrt(2.0) / 2.0;
+  double expected_x = std::sqrt(2.0) / 2.0;
+  double expected_y = 0.0;
+  double expected_z = 0.0;
+
+  EXPECT_NEAR(result.w, expected_w, 1e-6);
+  EXPECT_NEAR(result.x, expected_x, 1e-6);
+  EXPECT_NEAR(result.y, expected_y, 1e-6);
+  EXPECT_NEAR(result.z, expected_z, 1e-6);
+}