From 24c8f6dcf1ebb2626e695d50464a44dd09d12510 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 12 Aug 2022 17:38:11 +0900 Subject: [PATCH 1/4] feat(motion_utils): add option to use slerp for orientation Signed-off-by: Tomohito Ando --- .../motion_utils/trajectory/trajectory.hpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index e6332cfe6a407..a21fb5256ec97 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -703,11 +703,13 @@ inline boost::optional calcLongitudinalOffsetPoint( * @param points points of trajectory, path, ... * @param src_idx index of source point * @param offset length of offset from source point + * @param use_slerp_for_orientation set orientation by spherical interpolation if true * @return offset pose */ template inline boost::optional calcLongitudinalOffsetPose( - const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) + const T & points, const size_t src_idx, const double offset, + const bool use_slerp_for_orientation = false, const bool throw_exception = false) { try { validateNonEmpty(points); @@ -749,7 +751,7 @@ inline boost::optional calcLongitudinalOffsetPose( const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { return tier4_autoware_utils::calcInterpolatedPose( - p_back, p_front, std::abs(dist_res / dist_segment)); + p_back, p_front, std::abs(dist_res / dist_segment), !use_slerp_for_orientation); } } } else { @@ -765,7 +767,7 @@ inline boost::optional calcLongitudinalOffsetPose( const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { return tier4_autoware_utils::calcInterpolatedPose( - p_front, p_back, 1.0 - std::abs(dist_res / dist_segment)); + p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), !use_slerp_for_orientation); } } } @@ -779,11 +781,13 @@ inline boost::optional calcLongitudinalOffsetPose( * @param points points of trajectory, path, ... * @param src_point source point * @param offset length of offset from source point + * @param use_slerp_for_orientation set orientation by spherical interpolation if true * @return offset pase */ template inline boost::optional calcLongitudinalOffsetPose( - const T & points, const geometry_msgs::msg::Point & src_point, const double offset) + const T & points, const geometry_msgs::msg::Point & src_point, const double offset, + const bool use_slerp_for_orientation = false) { try { validateNonEmpty(points); @@ -796,7 +800,8 @@ inline boost::optional calcLongitudinalOffsetPose( const double signed_length_src_offset = calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - return calcLongitudinalOffsetPose(points, src_seg_idx, offset + signed_length_src_offset); + return calcLongitudinalOffsetPose( + points, src_seg_idx, offset + signed_length_src_offset, use_slerp_for_orientation); } /** From cb2fc8272e221fdebd4d61c358c4871ab7fb8b5f Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 18 Aug 2022 18:51:12 +0900 Subject: [PATCH 2/4] test: add tests for calcLongitudinalOffsetPose with spherical interpolation Signed-off-by: Tomohito Ando --- .../test/src/trajectory/test_trajectory.cpp | 167 ++++++++++++++++++ 1 file changed, 167 insertions(+) diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 068fbe3e42de1..38b410942df92 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1312,6 +1312,96 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) } } +TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; + using tier4_autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose(forward) + for (double len = 0.0; len < total_length; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len, true); + const auto ratio = 1 - (len / total_length); + const auto ans_quat = + createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + + // Found pose(backward) + for (double len = total_length; 0.0 < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len, true); + const auto ratio = len / total_length; + const auto ans_quat = + createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, true); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, true); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } +} + TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) { using motion_utils::calcArcLength; @@ -1472,6 +1562,83 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) } } +TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; + using motion_utils::calcLongitudinalOffsetToSegment; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose + for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { + constexpr double deviation = 0.1; + + const auto p_src = createPoint( + len_start * std::cos(deg2rad(45.0)) + deviation, + len_start * std::sin(deg2rad(45.0)) - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + for (double len = -src_offset; len < total_length - src_offset; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len, true); + const auto ratio = 1 - ((src_offset + len) / total_length); + const auto ans_quat = + createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR( + p_out.get().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + EXPECT_NEAR( + p_out.get().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary condition + { + constexpr double deviation = 0.1; + + const auto p_src = createPoint(1.0 + deviation, 1.0 - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + const auto p_out = + calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, true); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } +} + TEST(trajectory, insertTargetPoint) { using motion_utils::calcArcLength; From f4935a215ea87da1a692a3fd01a5a685721ce02b Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 18 Aug 2022 19:15:35 +0900 Subject: [PATCH 3/4] test: modify ratio to be more consitent Signed-off-by: Tomohito Ando --- .../test/src/trajectory/test_trajectory.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 38b410942df92..db899423df6d2 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1340,9 +1340,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Found pose(forward) for (double len = 0.0; len < total_length; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len, true); - const auto ratio = 1 - (len / total_length); + // ratio between two points + const auto ratio = len / total_length; const auto ans_quat = - createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); + createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); EXPECT_NE(p_out, boost::none); EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon); @@ -1357,6 +1358,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Found pose(backward) for (double len = total_length; 0.0 < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len, true); + // ratio between two points const auto ratio = len / total_length; const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); @@ -1600,9 +1602,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) for (double len = -src_offset; len < total_length - src_offset; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len, true); - const auto ratio = 1 - ((src_offset + len) / total_length); + // ratio between two points + const auto ratio = (src_offset + len) / total_length; const auto ans_quat = - createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); + createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); EXPECT_NE(p_out, boost::none); EXPECT_NEAR( From f7bf15ab1a04a34709a0282a4be2f855e2cf1770 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 19 Aug 2022 18:47:43 +0900 Subject: [PATCH 4/4] chore: use common flag name Signed-off-by: Tomohito Ando --- .../motion_utils/trajectory/trajectory.hpp | 19 ++++++++++++------- .../test/src/trajectory/test_trajectory.cpp | 12 ++++++------ 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index a21fb5256ec97..07d508986eb6f 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -703,13 +703,14 @@ inline boost::optional calcLongitudinalOffsetPoint( * @param points points of trajectory, path, ... * @param src_idx index of source point * @param offset length of offset from source point - * @param use_slerp_for_orientation set orientation by spherical interpolation if true + * @param set_orientation_from_position_direction set orientation by spherical interpolation if + * false * @return offset pose */ template inline boost::optional calcLongitudinalOffsetPose( const T & points, const size_t src_idx, const double offset, - const bool use_slerp_for_orientation = false, const bool throw_exception = false) + const bool set_orientation_from_position_direction = true, const bool throw_exception = false) { try { validateNonEmpty(points); @@ -751,7 +752,8 @@ inline boost::optional calcLongitudinalOffsetPose( const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { return tier4_autoware_utils::calcInterpolatedPose( - p_back, p_front, std::abs(dist_res / dist_segment), !use_slerp_for_orientation); + p_back, p_front, std::abs(dist_res / dist_segment), + set_orientation_from_position_direction); } } } else { @@ -767,7 +769,8 @@ inline boost::optional calcLongitudinalOffsetPose( const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { return tier4_autoware_utils::calcInterpolatedPose( - p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), !use_slerp_for_orientation); + p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), + set_orientation_from_position_direction); } } } @@ -781,13 +784,14 @@ inline boost::optional calcLongitudinalOffsetPose( * @param points points of trajectory, path, ... * @param src_point source point * @param offset length of offset from source point - * @param use_slerp_for_orientation set orientation by spherical interpolation if true + * @param set_orientation_from_position_direction set orientation by spherical interpolation if + * false * @return offset pase */ template inline boost::optional calcLongitudinalOffsetPose( const T & points, const geometry_msgs::msg::Point & src_point, const double offset, - const bool use_slerp_for_orientation = false) + const bool set_orientation_from_position_direction = true) { try { validateNonEmpty(points); @@ -801,7 +805,8 @@ inline boost::optional calcLongitudinalOffsetPose( calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); return calcLongitudinalOffsetPose( - points, src_seg_idx, offset + signed_length_src_offset, use_slerp_for_orientation); + points, src_seg_idx, offset + signed_length_src_offset, + set_orientation_from_position_direction); } /** diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index db899423df6d2..fd11702bab034 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1339,7 +1339,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Found pose(forward) for (double len = 0.0; len < total_length; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len, true); + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len, false); // ratio between two points const auto ratio = len / total_length; const auto ans_quat = @@ -1357,7 +1357,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Found pose(backward) for (double len = total_length; 0.0 < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len, true); + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len, false); // ratio between two points const auto ratio = len / total_length; const auto ans_quat = @@ -1375,7 +1375,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Boundary condition { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, true); + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, boost::none); @@ -1390,7 +1390,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Boundary condition { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, true); + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, boost::none); @@ -1601,7 +1601,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); for (double len = -src_offset; len < total_length - src_offset; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len, true); + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len, false); // ratio between two points const auto ratio = (src_offset + len) / total_length; const auto ans_quat = @@ -1628,7 +1628,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); const auto p_out = - calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, true); + calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, boost::none);