Skip to content

Commit

Permalink
fix(tier4_autoware_utils): fix boundary orientation (tier4#1130)
Browse files Browse the repository at this point in the history 
* change boundary orientation

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

* update test

Signed-off-by: yutaka <purewater0901@gmail.com>
  • Loading branch information
@purewater0901 @boyali
purewater0901 authored and boyali committed Oct 3, 2022
1 parent 33b9498 commit 9283acd
Show file tree
Hide file tree
Showing 2 changed files with 47 additions and 26 deletions.
15 changes: 12 additions & 3 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 @@ -382,6 +382,9 @@ geometry_msgs::msg::Point calcInterpolatedPoint(

/**
* @brief Calculate a pose by linear interpolation.
* Note that if ratio>=1.0 or dist(src_pose, dst_pose)<=0.01
* the orientation of the output pose is same as the orientation
* of the dst_pose
*/
template <class Pose1, class Pose2>
geometry_msgs::msg::Pose calcInterpolatedPose(
Expand All @@ -392,10 +395,16 @@ geometry_msgs::msg::Pose calcInterpolatedPose(
output_pose.position = calcInterpolatedPoint(getPoint(src_pose), getPoint(dst_pose), ratio);

if (set_orientation_from_position_direction) {
const double input_poses_dist = calcDistance2d(getPoint(src_pose), getPoint(dst_pose));

// 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);
if (ratio < 1.0 && input_poses_dist > 1e-3) {
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 {
output_pose.orientation = getPose(dst_pose).orientation;
}
} else {
// Get orientation by spherical linear interpolation
tf2::Transform src_tf, dst_tf;
Expand Down
58 changes: 35 additions & 23 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 @@ -873,74 +873,86 @@ TEST(geometry, calcInterpolatedPose)
}
}

// Quaternion Interpolation1
// 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));
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) {
for (double ratio = 0.0; ratio < 1.0; ratio += 0.1) {
const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 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);
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);
}

// Boundary Condition (ratio = 1.0)
const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 1.0);

const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60));
EXPECT_DOUBLE_EQ(p_out.position.x, 1.0);
EXPECT_DOUBLE_EQ(p_out.position.y, 1.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
// Same poses 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(1.0, 1.0, 0.0);
dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60));
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);

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

// Same points are given
// Same points are given (different orientation)
{
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));
dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45));

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, 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);
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 Down

0 comments on commit 9283acd

Please sign in to comment.