From 603486975fcd259c1a1bd3a52634d09870f36038 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 15 Jun 2022 10:46:32 +0900 Subject: [PATCH] feat(tier4_autoware_utils): add pose interpolation function (#1097) Signed-off-by: yutaka --- .../geometry/geometry.hpp | 26 ++++++ .../test/src/geometry/test_geometry.cpp | 82 +++++++++++++++++++ 2 files changed, 108 insertions(+) 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 index 9f5453bc70e23..4b0093ac41201 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -348,6 +348,32 @@ inline geometry_msgs::msg::Pose calcOffsetPose( tf2::toMsg(tf_pose * tf_offset, pose); return pose; } + +/** + * @brief Calculate a pose by linear interpolation. + */ +template +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 index eee28d89ee36d..3e87aa70f1260 100644 --- 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); + } + } +}