Skip to content

Commit

Permalink
feat(tier4_autoware_utils): add new orientation calculation (#1126)
Browse files Browse the repository at this point in the history 
* feat(tier4_autoware_utils): add new orientation calculation

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix format
  • Loading branch information
@purewater0901 @yn-mrse
purewater0901 authored and yn-mrse committed Jun 21, 2023
1 parent 75fc0bf commit 54ae7e4
Show file tree
Hide file tree
Showing 2 changed files with 155 additions and 27 deletions.
38 changes: 20 additions & 18 deletions common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -349,25 +349,27 @@ inline geometry_msgs::msg::Pose calcOffsetPose(
*/
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);
const Pose1 & src_pose, const Pose2 & dst_pose, const double ratio,
const bool set_orientation_from_position_direction = true)
{
geometry_msgs::msg::Pose output_pose;
output_pose.position = calcInterpolatedPoint(getPoint(src_pose), getPoint(dst_pose), ratio);

if (set_orientation_from_position_direction) {
// Get orientation from interpolated point and src_pose
const double pitch = calcElevationAngle(getPoint(output_pose), getPoint(dst_pose));
const double yaw = calcAzimuthAngle(output_pose.position, getPoint(dst_pose));
output_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
} else {
// Get orientation by spherical linear interpolation
tf2::Transform src_tf, dst_tf;
tf2::fromMsg(getPose(src_pose), src_tf);
tf2::fromMsg(getPose(dst_pose), dst_tf);
const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), ratio);
output_pose.orientation = tf2::toMsg(quaternion);
}

return pose;
return output_pose;
}
} // namespace tier4_autoware_utils

Expand Down
144 changes: 135 additions & 9 deletions common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -813,6 +813,7 @@ TEST(geometry, calcInterpolatedPose)
using tier4_autoware_utils::createQuaternion;
using tier4_autoware_utils::createQuaternionFromRPY;
using tier4_autoware_utils::deg2rad;
const double epsilon = 1e-3;

// Position Interpolation
{
Expand All @@ -824,7 +825,6 @@ TEST(geometry, calcInterpolatedPose)
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);

Expand All @@ -838,7 +838,7 @@ TEST(geometry, calcInterpolatedPose)
}
}

// Quaternion Interpolation
// Quaternion Interpolation1
{
geometry_msgs::msg::Pose src_pose;
src_pose.position = createPoint(0.0, 0.0, 0.0);
Expand All @@ -848,18 +848,41 @@ TEST(geometry, calcInterpolatedPose)
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));
const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
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);
EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
}
}

// Quaternion Interpolation2
{
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(1.0, 1.0, 0.0);
dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60));

for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);

const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45));
EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio);
EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio);
EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
}
}

Expand All @@ -873,7 +896,6 @@ TEST(geometry, calcInterpolatedPose)
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);

Expand All @@ -887,3 +909,107 @@ TEST(geometry, calcInterpolatedPose)
}
}
}

TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation)
{
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;
const double epsilon = 1e-3;

// 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);

for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false);

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 Interpolation1
{
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));

for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false);

const auto ans_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, ans_quat.x);
EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
}
}

// Quaternion Interpolation2
{
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(1.0, 1.0, 0.0);
dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60));

for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false);

const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60 * ratio));
EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio);
EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio);
EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_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));

for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false);

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);
}
}
}

0 comments on commit 54ae7e4

Please sign in to comment.