From 27e2664de6122d56345983f34ae25102f0db080f Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 13 Jul 2022 10:06:48 +0900 Subject: [PATCH] feat(tier4_autoware_utils): add inset target point function by length (#1303) --- .../trajectory/trajectory.hpp | 42 ++ .../test/src/trajectory/test_trajectory.cpp | 416 ++++++++++++++++++ 2 files changed, 458 insertions(+) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp index 37bf335762ef2..ec0ad9a895a01 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp @@ -866,6 +866,48 @@ inline boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } + +/** + * @brief calculate the point offset from source point along the trajectory (or path) + * @param insert_point_length length to insert point from the beginning of the points + * @param points output points of trajectory, path, ... + * @return index of insert point + */ +template +inline boost::optional insertTargetPoint( + const size_t start_segment_idx, const double insert_point_length, T & points, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (insert_point_length < 0.0 || start_segment_idx >= points.size() - 1) { + return boost::none; + } + + // Get Nearest segment index + boost::optional segment_idx = boost::none; + for (size_t i = start_segment_idx + 1; i < points.size(); ++i) { + const double length = calcSignedArcLength(points, start_segment_idx, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + + if (!segment_idx) { + return boost::none; + } + + // Get Target Point + const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); + const double target_length = std::max( + 0.0, insert_point_length - calcSignedArcLength(points, start_segment_idx, *segment_idx)); + const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); + const auto p_target = calcInterpolatedPoint( + getPoint(points.at(*segment_idx)), getPoint(points.at(*segment_idx + 1)), ratio); + + return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); +} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp index 0415c38fd566c..6d38e6fe42913 100644 --- a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp @@ -2023,3 +2023,419 @@ TEST(trajectory, insertTargetPoint_Length) std::invalid_argument); } } + +TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::findNearestSegmentIndex; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::insertTargetPoint; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Insert + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = createPoint(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Boundary condition) + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start + 1.1e-3, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Right on the terminal point + { + auto traj_out = traj; + + const auto p_target = createPoint(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, 9.0, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Not insert(Overlap base_idx point) + for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start + 1e-4, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Not insert(Overlap base_idx + 1 point) + for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start - 1e-4, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Invalid target point(In front of the beginning point) + { + auto traj_out = traj; + + const auto insert_idx = insertTargetPoint(0, -1.0, traj_out.points); + + EXPECT_EQ(insert_idx, boost::none); + } + + // Invalid target point(Behind the end point) + { + auto traj_out = traj; + + const auto insert_idx = insertTargetPoint(0, 10.0, traj_out.points); + + EXPECT_EQ(insert_idx, boost::none); + } + + // Empty + { + auto empty_traj = generateTestTrajectory(0, 1.0); + EXPECT_THROW(insertTargetPoint(0, 0.0, empty_traj.points), std::invalid_argument); + } +} + +TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Idx) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::findNearestSegmentIndex; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::insertTargetPoint; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Insert + for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { + auto traj_out = traj; + + const size_t start_idx = 2; + const auto p_target = createPoint(x_start + 2.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Before End Point) + { + auto traj_out = traj; + const double x_start = 1.0 - 1e-2; + + const size_t start_idx = 8; + const auto p_target = createPoint(9.0 - 1e-2, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Right on End Point) + { + auto traj_out = traj; + const double x_start = 1.0; + + const size_t start_idx = 8; + const auto p_target = createPoint(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Insert on end point) + { + auto traj_out = traj; + const double x_start = 4.0; + + const size_t start_idx = 5; + const auto p_target = createPoint(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(start point) + { + auto traj_out = traj; + const double x_start = 0.0; + + const size_t start_idx = 0; + const auto p_target = createPoint(0.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, boost::none); + EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = getPose(traj_out.points.at(base_idx)); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Index Out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(9, 0.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(9, 1.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(10, 0.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(10, 1.0, traj_out.points), boost::none); + } + + // No Insert (Length out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(0, 10.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(0, 9.0001, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(5, 5.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(8, 1.0001, traj_out.points), boost::none); + } +}