feat(tier4_autoware_utils): add point interpolation function (autowar…

…efoundation#1100)

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

@@ -349,6 +349,37 @@ inline geometry_msgs::msg::Pose calcOffsetPose(
   return pose;
 }
 
+/**
+ * @brief Calculate a point by linear interpolation.
+ */
+template <class Point1, class Point2>
+geometry_msgs::msg::Point calcInterpolatedPoint(
+  const Point1 & src, const Point2 & dst, const double ratio)
+{
+  const auto src_point = getPoint(src);
+  const auto dst_point = getPoint(dst);
+
+  tf2::Vector3 src_vec;
+  src_vec.setX(src_point.x);
+  src_vec.setY(src_point.y);
+  src_vec.setZ(src_point.z);
+
+  tf2::Vector3 dst_vec;
+  dst_vec.setX(dst_point.x);
+  dst_vec.setY(dst_point.y);
+  dst_vec.setZ(dst_point.z);
+
+  // Get pose by linear interpolation
+  const auto & vec = tf2::lerp(src_vec, dst_vec, ratio);
+
+  geometry_msgs::msg::Point point;
+  point.x = vec.x();
+  point.y = vec.y();
+  point.z = vec.z();
+
+  return point;
+}
+
 /**
  * @brief Calculate a pose by linear interpolation.
  */

common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp

@@ -806,6 +806,41 @@ TEST(geometry, calcOffsetPose)
   }
 }
 
+TEST(geometry, calcInterpolatedPoint)
+{
+  using tier4_autoware_utils::calcInterpolatedPoint;
+  using tier4_autoware_utils::createPoint;
+
+  {
+    const auto src_point = createPoint(0.0, 0.0, 0.0);
+    const auto dst_point = createPoint(3.0, 0.0, 0.0);
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.x, 3.0 * ratio);
+      EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+    }
+  }
+
+  // Same points are given
+  {
+    const auto src_point = createPoint(0.0, 0.0, 0.0);
+    const auto dst_point = createPoint(0.0, 0.0, 0.0);
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+    }
+  }
+}
+
 TEST(geometry, calcInterpolatedPose)
 {
   using tier4_autoware_utils::calcInterpolatedPose;