Skip to content

Commit

Permalink
feat(tier4_autoware_utils): add clamp function to clamp the ratio (ti…
Browse files Browse the repository at this point in the history 
…er4#1145)
  • Loading branch information
@purewater0901 @boyali
purewater0901 authored and boyali committed Oct 3, 2022
1 parent 87ab67b commit dc0613b
Show file tree
Hide file tree
Showing 2 changed files with 126 additions and 4 deletions.
20 changes: 16 additions & 4 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 @@ -351,6 +351,10 @@ inline geometry_msgs::msg::Pose calcOffsetPose(

/**
* @brief Calculate a point by linear interpolation.
* @param src source point
* @param dst destination point
* @param ratio interpolation ratio, which should be [0.0, 1.0]
* @return interpolated point
*/
template <class Point1, class Point2>
geometry_msgs::msg::Point calcInterpolatedPoint(
Expand All @@ -370,7 +374,8 @@ geometry_msgs::msg::Point calcInterpolatedPoint(
dst_vec.setZ(dst_point.z);

// Get pose by linear interpolation
const auto & vec = tf2::lerp(src_vec, dst_vec, ratio);
const double clamped_ratio = std::clamp(ratio, 0.0, 1.0);
const auto & vec = tf2::lerp(src_vec, dst_vec, clamped_ratio);

geometry_msgs::msg::Point point;
point.x = vec.x();
Expand All @@ -385,20 +390,27 @@ geometry_msgs::msg::Point calcInterpolatedPoint(
* 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
* @param src source point
* @param dst destination point
* @param ratio interpolation ratio, which should be [0.0, 1.0]
* @param set_orientation_from_position_direction set position by spherical interpolation if false
* @return interpolated point
*/
template <class Pose1, class Pose2>
geometry_msgs::msg::Pose calcInterpolatedPose(
const Pose1 & src_pose, const Pose2 & dst_pose, const double ratio,
const bool set_orientation_from_position_direction = true)
{
const double clamped_ratio = std::clamp(ratio, 0.0, 1.0);
geometry_msgs::msg::Pose output_pose;
output_pose.position = calcInterpolatedPoint(getPoint(src_pose), getPoint(dst_pose), ratio);
output_pose.position =
calcInterpolatedPoint(getPoint(src_pose), getPoint(dst_pose), clamped_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
if (ratio < 1.0 && input_poses_dist > 1e-3) {
if (clamped_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);
Expand All @@ -410,7 +422,7 @@ geometry_msgs::msg::Pose calcInterpolatedPose(
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);
const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio);
output_pose.orientation = tf2::toMsg(quaternion);
}

Expand Down
110 changes: 110 additions & 0 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 @@ -839,6 +839,28 @@ TEST(geometry, calcInterpolatedPoint)
EXPECT_DOUBLE_EQ(p_out.z, 0.0);
}
}

// Boundary Condition (Negative Ratio)
{
const auto src_point = createPoint(0.0, 0.0, 0.0);
const auto dst_point = createPoint(3.0, 0.0, 0.0);

const auto p_out = calcInterpolatedPoint(src_point, dst_point, -10.0);
EXPECT_DOUBLE_EQ(p_out.x, 0.0);
EXPECT_DOUBLE_EQ(p_out.y, 0.0);
EXPECT_DOUBLE_EQ(p_out.z, 0.0);
}

// Boundary Condition (Positive Ratio larger than 1.0)
{
const auto src_point = createPoint(0.0, 0.0, 0.0);
const auto dst_point = createPoint(3.0, 0.0, 0.0);

const auto p_out = calcInterpolatedPoint(src_point, dst_point, 10.0);
EXPECT_DOUBLE_EQ(p_out.x, 3.0);
EXPECT_DOUBLE_EQ(p_out.y, 0.0);
EXPECT_DOUBLE_EQ(p_out.z, 0.0);
}
}

TEST(geometry, calcInterpolatedPose)
Expand Down Expand Up @@ -873,6 +895,50 @@ TEST(geometry, calcInterpolatedPose)
}
}

// Boundary Condition (Negative Ratio)
{
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));

const auto p_out = calcInterpolatedPose(src_pose, dst_pose, -10.0);

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, 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 (Positive Ratio larger than 1.0)
{
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));

const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 10.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 Interpolation
{
geometry_msgs::msg::Pose src_pose;
Expand Down Expand Up @@ -989,6 +1055,50 @@ TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation)
}
}

// Boundary Condition (Negative Ratio)
{
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));

const auto p_out = calcInterpolatedPose(src_pose, dst_pose, -10.0, false);

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, 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 (Positive Ratio larger than 1.0)
{
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));

const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 10.0, false);

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 Interpolation1
{
geometry_msgs::msg::Pose src_pose;
Expand Down

0 comments on commit dc0613b

Please sign in to comment.