From 86ae796d135389dc5c0bd6646783011dc25d1e62 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 3 Feb 2023 10:33:45 +0900 Subject: [PATCH] feat(interpolation): add curvature calculation (#2801) * feat(interpolation): add curvature calculation Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * add test Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../interpolation/spline_interpolation.hpp | 20 +- .../spline_interpolation_points_2d.hpp | 22 +- .../src/spline_interpolation.cpp | 27 ++- .../src/spline_interpolation_points_2d.cpp | 86 +++++-- .../test/src/test_spline_interpolation.cpp | 188 +-------------- .../test_spline_interpolation_points_2d.cpp | 222 ++++++++++++++++++ .../src/mpt_optimizer.cpp | 4 +- 7 files changed, 353 insertions(+), 216 deletions(-) create mode 100644 common/interpolation/test/src/test_spline_interpolation_points_2d.cpp diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index 9640bcd6c5120..f5befefeae9da 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -70,9 +70,11 @@ class SplineInterpolation { public: SplineInterpolation() = default; - - void calcSplineCoefficients( - const std::vector & base_keys, const std::vector & base_values); + SplineInterpolation( + const std::vector & base_keys, const std::vector & base_values) + { + calcSplineCoefficients(base_keys, base_values); + } //!< @brief get values of spline interpolation on designated sampling points. //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, @@ -86,9 +88,21 @@ class SplineInterpolation // return value will be dx/dt(t) vector std::vector getSplineInterpolatedDiffValues(const std::vector & query_keys) const; + //!< @brief get 2nd differential values of spline interpolation on designated sampling points. + //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, + // meaning that spline interpolation was applied to x(t), + // return value will be d^2/dt^2(t) vector + std::vector getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const; + + size_t getSize() const { return base_keys_.size(); } + private: std::vector base_keys_; interpolation::MultiSplineCoef multi_spline_coef_; + + void calcSplineCoefficients( + const std::vector & base_keys, const std::vector & base_values); }; #endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index d44d16d88dd04..567a1873aaa5c 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -37,20 +37,19 @@ std::vector splineYawFromPoints(const std::vector & points); // SplineInterpolationPoints2d spline; // // memorize pre-interpolation result internally // spline.calcSplineCoefficients(base_keys, base_values); -// const auto interpolation_result1 = spline.getSplineInterpolatedPoints( +// const auto interpolation_result1 = spline.getSplineInterpolatedPoint( // base_keys, query_keys1); -// const auto interpolation_result2 = spline.getSplineInterpolatedPoints( +// const auto interpolation_result2 = spline.getSplineInterpolatedPoint( // base_keys, query_keys2); -// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaws( +// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw( // base_keys, query_keys1); // ``` class SplineInterpolationPoints2d { public: SplineInterpolationPoints2d() = default; - template - void calcSplineCoefficients(const std::vector & points) + explicit SplineInterpolationPoints2d(const std::vector & points) { std::vector points_inner; for (const auto & p : points) { @@ -63,9 +62,22 @@ class SplineInterpolationPoints2d // std::vector getSplineInterpolatedPoints(const double width); // std::vector getSplineInterpolatedPoses(const double width); + // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw) + geometry_msgs::msg::Pose getSplineInterpolatedPose(const size_t idx, const double s) const; + + // point geometry_msgs::msg::Point getSplineInterpolatedPoint(const size_t idx, const double s) const; + + // yaw double getSplineInterpolatedYaw(const size_t idx, const double s) const; + std::vector getSplineInterpolatedYaws() const; + + // curvature + double getSplineInterpolatedCurvature(const size_t idx, const double s) const; + std::vector getSplineInterpolatedCurvatures() const; + size_t getSize() const { return base_s_vec_.size(); } + size_t getOffsetIndex(const size_t idx, const double offset) const; double getAccumulatedLength(const size_t idx) const; private: diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index bd92af1007b50..1275b47346d78 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -85,10 +85,8 @@ std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { - SplineInterpolation interpolator; - // calculate spline coefficients - interpolator.calcSplineCoefficients(base_keys, base_values); + SplineInterpolation interpolator(base_keys, base_values); // interpolate base_keys at query_keys return interpolator.getSplineInterpolatedValues(query_keys); @@ -266,3 +264,26 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( return res; } + +std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + + const auto & a = multi_spline_coef_.a; + const auto & b = multi_spline_coef_.b; + + std::vector res; + size_t j = 0; + for (const auto & query_key : validated_query_keys) { + while (base_keys_.at(j + 1) < query_key) { + ++j; + } + + const double ds = query_key - base_keys_.at(j); + res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds); + } + + return res; +} diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 71e2629044f11..82c3841c424c5 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -73,14 +73,14 @@ std::array, 3> slerp2dFromXY( const std::vector & base_keys, const std::vector & base_x_values, const std::vector & base_y_values, const std::vector & query_keys) { - SplineInterpolation interpolator_x, interpolator_y; - std::vector yaw_vec; - // calculate spline coefficients - interpolator_x.calcSplineCoefficients(base_keys, base_x_values); - interpolator_y.calcSplineCoefficients(base_keys, base_y_values); + SplineInterpolation interpolator_x(base_keys, base_x_values); + SplineInterpolation interpolator_y(base_keys, base_y_values); const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys); const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys); + + // calculate yaw + std::vector yaw_vec; for (size_t i = 0; i < diff_x.size(); i++) { double yaw = std::atan2(diff_y[i], diff_x[i]); yaw_vec.push_back(yaw); @@ -94,10 +94,8 @@ std::array, 3> slerp2dFromXY( template std::vector splineYawFromPoints(const std::vector & points) { - SplineInterpolationPoints2d interpolator; - // calculate spline coefficients - interpolator.calcSplineCoefficients(points); + SplineInterpolationPoints2d interpolator(points); // interpolate base_keys at query_keys std::vector yaw_vec; @@ -112,6 +110,16 @@ template std::vector splineYawFromPoints( } // namespace interpolation +geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( + const size_t idx, const double s) const +{ + geometry_msgs::msg::Pose pose; + pose.position = getSplineInterpolatedPoint(idx, s); + pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + return pose; +} + geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( const size_t idx, const double s) const { @@ -142,18 +150,64 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c throw std::out_of_range("idx is out of range."); } - double whole_s = base_s_vec_.at(idx) + s; - if (whole_s < base_s_vec_.front()) { - whole_s = base_s_vec_.front(); + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + + return std::atan2(diff_y, diff_x); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedYaws() const +{ + std::vector yaw_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double yaw = getSplineInterpolatedYaw(i, 0.0); + yaw_vec.push_back(yaw); } - if (whole_s > base_s_vec_.back()) { - whole_s = base_s_vec_.back(); + return yaw_vec; +} + +double SplineInterpolationPoints2d::getSplineInterpolatedCurvature( + const size_t idx, const double s) const +{ + if (base_s_vec_.size() <= idx) { + throw std::out_of_range("idx is out of range."); } + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); - return std::atan2(diff_y, diff_x); + const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + + return (diff_x * quad_diff_y - quad_diff_x * diff_y) / + std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const +{ + std::vector curvature_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double curvature = getSplineInterpolatedCurvature(i, 0.0); + curvature_vec.push_back(curvature); + } + return curvature_vec; +} + +size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const +{ + const double whole_s = base_s_vec_.at(idx) + offset; + for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) { + if (whole_s < base_s_vec_.at(s_idx)) { + return s_idx; + } + } + return base_s_vec_.size() - 1; } double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const @@ -174,6 +228,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( const auto & base_y_vec = base.at(2); // calculate spline coefficients - spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); - spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); + spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec); + spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); } diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index c2bb8eb177ba1..c95fc902ade44 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -221,203 +220,18 @@ TEST(spline_interpolation, splineByAkima) } } -TEST(spline_interpolation, splineYawFromPoints) -{ - using tier4_autoware_utils::createPoint; - - { // straight - std::vector points; - points.push_back(createPoint(0.0, 0.0, 0.0)); - points.push_back(createPoint(1.0, 1.5, 0.0)); - points.push_back(createPoint(2.0, 3.0, 0.0)); - points.push_back(createPoint(3.0, 4.5, 0.0)); - points.push_back(createPoint(4.0, 6.0, 0.0)); - - const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // curve - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); - - const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // size of base_keys is 1 (infeasible to interpolate) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - - EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); - } - - { // straight: size of base_keys is 2 (edge case in the implementation) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - - const std::vector ans{0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // straight: size of base_keys is 3 (edge case in the implementation) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - - const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } -} - TEST(spline_interpolation, SplineInterpolation) { - SplineInterpolation s; - // curve: query_keys is random const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.075611, 0.997242, 1.573258}; - s.calcSplineCoefficients(base_keys, base_values); + SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } - -TEST(spline_interpolation, SplineInterpolationPoints2d) -{ - using tier4_autoware_utils::createPoint; - - SplineInterpolationPoints2d s; - - // curve - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); - - s.calcSplineCoefficients(points); - - { // point - // front - const auto front_point = s.getSplineInterpolatedPoint(0, 0.0); - EXPECT_NEAR(front_point.x, -2.0, epsilon); - EXPECT_NEAR(front_point.y, -10.0, epsilon); - - // back - const auto back_point = s.getSplineInterpolatedPoint(4, 0.0); - EXPECT_NEAR(back_point.x, 10.0, epsilon); - EXPECT_NEAR(back_point.y, 12.5, epsilon); - - // random - const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); - EXPECT_NEAR(random_point.x, 5.3036484, epsilon); - EXPECT_NEAR(random_point.y, 10.3343074, epsilon); - - // out of range of total length - const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); - EXPECT_NEAR(front_out_point.x, -2.0, epsilon); - EXPECT_NEAR(front_out_point.y, -10.0, epsilon); - - const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1); - EXPECT_NEAR(back_out_point.x, 10.0, epsilon); - EXPECT_NEAR(back_out_point.y, 12.5, epsilon); - - // out of range of index - EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range); - EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range); - } - - { // yaw - // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); - - // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); - - // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); - - // out of range of total length - EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); - - // out of range of index - EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); - EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range); - } - - { // accumulated distance - // front - EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon); - - // back - EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon); - - // random - EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon); - - // out of range of index - EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range); - EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range); - } - - // size of base_keys is 1 (infeasible to interpolate) - std::vector single_points; - single_points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(s.calcSplineCoefficients(single_points), std::logic_error); -} - -TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) -{ - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::createPoint; - - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - - std::vector trajectory_points; - for (const auto & p : points) { - TrajectoryPoint tp; - tp.pose.position = p; - trajectory_points.push_back(tp); - } - - SplineInterpolationPoints2d s_point; - s_point.calcSplineCoefficients(points); - s_point.getSplineInterpolatedPoint(0, 0.); - - SplineInterpolationPoints2d s_traj_point; - s_traj_point.calcSplineCoefficients(trajectory_points); - s_traj_point.getSplineInterpolatedPoint(0, 0.); -} diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp new file mode 100644 index 0000000000000..a7d7012c19e22 --- /dev/null +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -0,0 +1,222 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +TEST(spline_interpolation, splineYawFromPoints) +{ + using tier4_autoware_utils::createPoint; + + { // straight + std::vector points; + points.push_back(createPoint(0.0, 0.0, 0.0)); + points.push_back(createPoint(1.0, 1.5, 0.0)); + points.push_back(createPoint(2.0, 3.0, 0.0)); + points.push_back(createPoint(3.0, 4.5, 0.0)); + points.push_back(createPoint(4.0, 6.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // size of base_keys is 1 (infeasible to interpolate) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + + EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); + } + + { // straight: size of base_keys is 2 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + + const std::vector ans{0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // straight: size of base_keys is 3 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } +} + +TEST(spline_interpolation, SplineInterpolationPoints2d) +{ + using tier4_autoware_utils::createPoint; + + // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + SplineInterpolationPoints2d s(points); + + { // point + // front + const auto front_point = s.getSplineInterpolatedPoint(0, 0.0); + EXPECT_NEAR(front_point.x, -2.0, epsilon); + EXPECT_NEAR(front_point.y, -10.0, epsilon); + + // back + const auto back_point = s.getSplineInterpolatedPoint(4, 0.0); + EXPECT_NEAR(back_point.x, 10.0, epsilon); + EXPECT_NEAR(back_point.y, 12.5, epsilon); + + // random + const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); + EXPECT_NEAR(random_point.x, 5.3036484, epsilon); + EXPECT_NEAR(random_point.y, 10.3343074, epsilon); + + // out of range of total length + const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); + EXPECT_NEAR(front_out_point.x, -2.0, epsilon); + EXPECT_NEAR(front_out_point.y, -10.0, epsilon); + + const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1); + EXPECT_NEAR(back_out_point.x, 10.0, epsilon); + EXPECT_NEAR(back_out_point.y, 12.5, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range); + } + + { // yaw + // front + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); + + // random + EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); + + // out of range of total length + EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range); + } + + { // curvature + // front + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0, 0.0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); + + // random + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.2441671, epsilon); + + // out of range of total length + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.1), 0.0, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedCurvature(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedCurvature(5, 0.0), std::out_of_range); + } + + { // accumulated distance + // front + EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon); + + // random + EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon); + + // out of range of index + EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range); + EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range); + } + + // size of base_keys is 1 (infeasible to interpolate) + std::vector single_points; + single_points.push_back(createPoint(1.0, 0.0, 0.0)); + EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error); +} + +TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using tier4_autoware_utils::createPoint; + + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + std::vector trajectory_points; + for (const auto & p : points) { + TrajectoryPoint tp; + tp.pose.position = p; + trajectory_points.push_back(tp); + } + + SplineInterpolationPoints2d s_point(points); + s_point.getSplineInterpolatedPoint(0, 0.); + + SplineInterpolationPoints2d s_traj_point(trajectory_points); + s_traj_point.getSplineInterpolatedPoint(0, 0.); +} diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 998f211ba467b..a0e43aa2f16fa 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1450,8 +1450,8 @@ void MPTOptimizer::calcVehicleBounds( return; } - SplineInterpolationPoints2d ref_points_spline_interpolation; - ref_points_spline_interpolation.calcSplineCoefficients(points_utils::convertToPoints(ref_points)); + SplineInterpolationPoints2d ref_points_spline_interpolation( + points_utils::convertToPoints(ref_points)); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx);