Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(tier4_autoware_utils): add clamp function to clamp the ratio #1145

Merged
merged 2 commits into from
Jun 22, 2022
Merged

feat(tier4_autoware_utils): add clamp function to clamp the ratio #1145

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ead18de
feat(tier4_autoware_utils): add clmap function to clamp the ratio
purewater0901 Jun 21, 2022
ee05da0
add explanations
purewater0901 Jun 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 16 additions & 4 deletions common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
View file
Open in desktop
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
Open in desktop
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