feat(tier4_autoware_utils): add pose interpolation function (#1097)

Signed-off-by: yutaka <purewater0901@gmail.com>
tier4 · Jun 20, 2023 · 30e0143 · 30e0143
1 parent f8e5249
commit 30e0143
Show file tree

Hide file tree

Showing 2 changed files with 108 additions and 0 deletions.
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
@@ -343,6 +343,32 @@ inline geometry_msgs::msg::Pose calcOffsetPose(
   tf2::toMsg(tf_pose * tf_offset, pose);
   return pose;
 }
+
+/**
+ * @brief Calculate a pose by linear interpolation.
+ */
+template <class Pose1, class Pose2>
+geometry_msgs::msg::Pose calcInterpolatedPose(
+  const Pose1 & src_pose, const Pose2 & dst_pose, const double ratio)
+{
+  tf2::Transform src_tf, dst_tf;
+  tf2::fromMsg(getPose(src_pose), src_tf);
+  tf2::fromMsg(getPose(dst_pose), dst_tf);
+
+  // Get pose by linear interpolation
+  const auto & point = tf2::lerp(src_tf.getOrigin(), dst_tf.getOrigin(), ratio);
+
+  // Get quaternion by spherical linear interpolation
+  const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), ratio);
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = point.x();
+  pose.position.y = point.y();
+  pose.position.z = point.z();
+  pose.orientation = tf2::toMsg(quaternion);
+
+  return pose;
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_
diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp
@@ -805,3 +805,85 @@ TEST(geometry, calcOffsetPose)
     EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831);
   }
 }
+
+TEST(geometry, calcInterpolatedPose)
+{
+  using tier4_autoware_utils::calcInterpolatedPose;
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::createQuaternion;
+  using tier4_autoware_utils::createQuaternionFromRPY;
+  using tier4_autoware_utils::deg2rad;
+
+  // Position Interpolation
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0);
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(3.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0);
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio);
+      EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+    }
+  }
+
+  // Quaternion Interpolation
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(0.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90));
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
+
+      const auto result_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90 * ratio));
+      EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.x, result_quat.x);
+      EXPECT_DOUBLE_EQ(p_out.orientation.y, result_quat.y);
+      EXPECT_DOUBLE_EQ(p_out.orientation.z, result_quat.z);
+      EXPECT_DOUBLE_EQ(p_out.orientation.w, result_quat.w);
+    }
+  }
+
+  // Same points are given
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(0.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+    }
+  }
+}