diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 909d027991976..a6695d36d8f38 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,6 +14,8 @@ #include "motion_utils/resample/resample.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + namespace motion_utils { std::vector resamplePath( @@ -76,17 +78,37 @@ std::vector resamplePath( resampled_points.at(i) = pose; } + const bool is_driving_forward = + tier4_autoware_utils::isDrivingForward(points.at(0), points.at(1)); // Insert Orientation - for (size_t i = 0; i < resampled_points.size() - 1; ++i) { - const auto & src_point = resampled_points.at(i).position; - const auto & dst_point = resampled_points.at(i + 1).position; - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - resampled_points.at(i).orientation = - tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); - if (i == resampled_points.size() - 2) { - // Terminal Orientation is same as the point before it - resampled_points.at(i + 1).orientation = resampled_points.at(i).orientation; + if (is_driving_forward) { + for (size_t i = 0; i < resampled_points.size() - 1; ++i) { + const auto & src_point = resampled_points.at(i).position; + const auto & dst_point = resampled_points.at(i + 1).position; + const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); + resampled_points.at(i).orientation = + tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + if (i == resampled_points.size() - 2) { + // Terminal Orientation is same as the point before it + resampled_points.at(i + 1).orientation = resampled_points.at(i).orientation; + } + } + } else { + for (size_t i = resampled_points.size() - 1; i >= 1; --i) { + const auto & src_point = resampled_points.at(i).position; + const auto & dst_point = resampled_points.at(i - 1).position; + const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); + resampled_points.at(i).orientation = + tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + } + + // Initial Orientation is depend on the initial value of the resampled_arclength + if (resampled_arclength.front() < 1e-3) { + resampled_points.at(0).orientation = points.at(0).orientation; + } else { + resampled_points.at(0).orientation = resampled_points.at(1).orientation; } } diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index fdade010a5064..a1acaf948f0d8 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -380,6 +380,184 @@ TEST(resample_path, resample_path_by_vector) } } +TEST(resample_path, resample_path_by_vector_backward) +{ + using motion_utils::resamplePath; + + { + autoware_auto_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } + + // change initial orientation + { + autoware_auto_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(0).pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + // Initial Orientation + { + const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + + const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + for (size_t i = 1; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } +} + TEST(resample_path, resample_path_by_vector_non_default) { using motion_utils::resamplePath;