From 7d97dd9c8db3cb098403852d29f15549faaa4c16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Fri, 20 Sep 2024 13:07:12 +0300 Subject: [PATCH 1/7] Revert "fix(interpolation): fix bug of interpolation (#8903)" This reverts commit 7819089327e2469f3371bcb106fb6d99432230f7. --- .../interpolation/spline_interpolation.hpp | 29 ++- common/interpolation/package.xml | 1 - .../src/spline_interpolation.cpp | 225 ++++++++++-------- .../test/src/test_spline_interpolation.cpp | 10 +- .../test_spline_interpolation_points_2d.cpp | 19 +- 5 files changed, 167 insertions(+), 117 deletions(-) diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index 54603a68d37a5..38e7b3fed5b8b 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -18,8 +18,6 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/interpolation_utils.hpp" -#include - #include #include #include @@ -28,6 +26,25 @@ namespace interpolation { +// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1) +struct MultiSplineCoef +{ + MultiSplineCoef() = default; + + explicit MultiSplineCoef(const size_t num_spline) + { + a.resize(num_spline); + b.resize(num_spline); + c.resize(num_spline); + d.resize(num_spline); + } + + std::vector a; + std::vector b; + std::vector c; + std::vector d; +}; + // static spline interpolation functions std::vector spline( const std::vector & base_keys, const std::vector & base_values, @@ -81,17 +98,11 @@ class SplineInterpolation size_t getSize() const { return base_keys_.size(); } private: - Eigen::VectorXd a_; - Eigen::VectorXd b_; - Eigen::VectorXd c_; - Eigen::VectorXd d_; - std::vector base_keys_; + interpolation::MultiSplineCoef multi_spline_coef_; void calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values); - - Eigen::Index get_index(const double & key) const; }; #endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index 330c87df18423..bb4af924dd252 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -12,7 +12,6 @@ autoware_cmake autoware_universe_utils - eigen ament_cmake_ros ament_lint_auto diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index 5cf8f68ca519d..1275b47346d78 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -14,40 +14,65 @@ #include "interpolation/spline_interpolation.hpp" -#include #include namespace { -Eigen::VectorXd solve_tridiagonal_matrix_algorithm( - const Eigen::Ref & a, const Eigen::Ref & b, - const Eigen::Ref & c, const Eigen::Ref & d) +// solve Ax = d +// where A is tridiagonal matrix +// [b_0 c_0 ... ] +// [a_0 b_1 c_1 ... O ] +// A = [ ... ] +// [ O ... a_N-3 b_N-2 c_N-2] +// [ ... a_N-2 b_N-1] +struct TDMACoef { - const auto n = d.size(); - - if (n == 1) { - return d.array() / b.array(); + explicit TDMACoef(const size_t num_row) + { + a.resize(num_row - 1); + b.resize(num_row); + c.resize(num_row - 1); + d.resize(num_row); } - Eigen::VectorXd c_prime = Eigen::VectorXd::Zero(n); - Eigen::VectorXd d_prime = Eigen::VectorXd::Zero(n); - Eigen::VectorXd x = Eigen::VectorXd::Zero(n); - - // Forward sweep - c_prime(0) = c(0) / b(0); - d_prime(0) = d(0) / b(0); + std::vector a; + std::vector b; + std::vector c; + std::vector d; +}; - for (auto i = 1; i < n; i++) { - const double m = 1.0 / (b(i) - a(i - 1) * c_prime(i - 1)); - c_prime(i) = i < n - 1 ? c(i) * m : 0; - d_prime(i) = (d(i) - a(i - 1) * d_prime(i - 1)) * m; - } +inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma_coef) +{ + const auto & a = tdma_coef.a; + const auto & b = tdma_coef.b; + const auto & c = tdma_coef.c; + const auto & d = tdma_coef.d; + + const size_t num_row = b.size(); + + std::vector x(num_row); + if (num_row != 1) { + // calculate p and q + std::vector p; + std::vector q; + p.push_back(-c[0] / b[0]); + q.push_back(d[0] / b[0]); + + for (size_t i = 1; i < num_row; ++i) { + const double den = b[i] + a[i - 1] * p[i - 1]; + p.push_back(-c[i - 1] / den); + q.push_back((d[i] - a[i - 1] * q[i - 1]) / den); + } - // Back substitution - x(n - 1) = d_prime(n - 1); + // calculate solution + x[num_row - 1] = q[num_row - 1]; - for (int64_t i = n - 2; i >= 0; i--) { - x(i) = d_prime(i) - c_prime(i) * x(i + 1); + for (size_t i = 1; i < num_row; ++i) { + const size_t j = num_row - 1 - i; + x[j] = p[j] * x[j + 1] + q[j]; + } + } else { + x[0] = (d[0] / b[0]); } return x; @@ -141,59 +166,53 @@ std::vector splineByAkima( } } // namespace interpolation -Eigen::Index SplineInterpolation::get_index(const double & key) const -{ - const auto it = std::lower_bound(base_keys_.begin(), base_keys_.end(), key); - return std::clamp( - static_cast(std::distance(base_keys_.begin(), it)) - 1, 0, - static_cast(base_keys_.size()) - 2); -} - void SplineInterpolation::calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values) { // throw exceptions for invalid arguments interpolation_utils::validateKeysAndValues(base_keys, base_values); - const Eigen::VectorXd x = Eigen::Map( - base_keys.data(), static_cast(base_keys.size())); - const Eigen::VectorXd y = Eigen::Map( - base_values.data(), static_cast(base_values.size())); - - const auto n = x.size(); - - if (n == 2) { - a_ = Eigen::VectorXd::Zero(1); - b_ = Eigen::VectorXd::Zero(1); - c_ = Eigen::VectorXd::Zero(1); - d_ = Eigen::VectorXd::Zero(1); - c_[0] = (y[1] - y[0]) / (x[1] - x[0]); - d_[0] = y[0]; - base_keys_ = base_keys; - return; + + const size_t num_base = base_keys.size(); // N+1 + + std::vector diff_keys; // N + std::vector diff_values; // N + for (size_t i = 0; i < num_base - 1; ++i) { + diff_keys.push_back(base_keys.at(i + 1) - base_keys.at(i)); + diff_values.push_back(base_values.at(i + 1) - base_values.at(i)); + } + + std::vector v = {0.0}; + if (num_base > 2) { + // solve tridiagonal matrix algorithm + TDMACoef tdma_coef(num_base - 2); // N-1 + + for (size_t i = 0; i < num_base - 2; ++i) { + tdma_coef.b[i] = 2 * (diff_keys[i] + diff_keys[i + 1]); + if (i != num_base - 3) { + tdma_coef.a[i] = diff_keys[i + 1]; + tdma_coef.c[i] = diff_keys[i + 1]; + } + tdma_coef.d[i] = + 6.0 * (diff_values[i + 1] / diff_keys[i + 1] - diff_values[i] / diff_keys[i]); + } + + const std::vector tdma_res = solveTridiagonalMatrixAlgorithm(tdma_coef); + + // calculate v + v.insert(v.end(), tdma_res.begin(), tdma_res.end()); + } + v.push_back(0.0); + + // calculate a, b, c, d of spline coefficients + multi_spline_coef_ = interpolation::MultiSplineCoef{num_base - 1}; // N + for (size_t i = 0; i < num_base - 1; ++i) { + multi_spline_coef_.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i]; + multi_spline_coef_.b[i] = v[i] / 2.0; + multi_spline_coef_.c[i] = + diff_values[i] / diff_keys[i] - diff_keys[i] * (2 * v[i] + v[i + 1]) / 6.0; + multi_spline_coef_.d[i] = base_values[i]; } - // Create Tridiagonal matrix - Eigen::VectorXd v(n); - const Eigen::VectorXd h = x.segment(1, n - 1) - x.segment(0, n - 1); - const Eigen::VectorXd a = h.segment(1, n - 3); - const Eigen::VectorXd b = 2 * (h.segment(0, n - 2) + h.segment(1, n - 2)); - const Eigen::VectorXd c = h.segment(1, n - 3); - const Eigen::VectorXd y_diff = y.segment(1, n - 1) - y.segment(0, n - 1); - const Eigen::VectorXd d = 6 * (y_diff.segment(1, n - 2).array() / h.tail(n - 2).array() - - y_diff.segment(0, n - 2).array() / h.head(n - 2).array()); - - // Solve tridiagonal matrix - v.segment(1, n - 2) = solve_tridiagonal_matrix_algorithm(a, b, c, d); - v[0] = 0; - v[n - 1] = 0; - - // Calculate spline coefficients - a_ = (v.tail(n - 1) - v.head(n - 1)).array() / 6.0 / (x.tail(n - 1) - x.head(n - 1)).array(); - b_ = v.segment(0, n - 1) / 2.0; - c_ = (y.tail(n - 1) - y.head(n - 1)).array() / (x.tail(n - 1) - x.head(n - 1)).array() - - (x.tail(n - 1) - x.head(n - 1)).array() * - (2 * v.segment(0, n - 1).array() + v.segment(1, n - 1).array()) / 6.0; - d_ = y.head(n - 1); base_keys_ = base_keys; } @@ -202,17 +221,24 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( { // throw exceptions for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); - std::vector interpolated_values; - interpolated_values.reserve(query_keys.size()); - - for (const auto & key : query_keys) { - const auto idx = get_index(key); - const auto dx = key - base_keys_[idx]; - interpolated_values.emplace_back( - a_[idx] * dx * dx * dx + b_[idx] * dx * dx + c_[idx] * dx + d_[idx]); + + const auto & a = multi_spline_coef_.a; + const auto & b = multi_spline_coef_.b; + const auto & c = multi_spline_coef_.c; + const auto & d = multi_spline_coef_.d; + + 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(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); } - return interpolated_values; + return res; } std::vector SplineInterpolation::getSplineInterpolatedDiffValues( @@ -220,16 +246,23 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( { // throw exceptions for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); - std::vector interpolated_diff_values; - interpolated_diff_values.reserve(query_keys.size()); - for (const auto & key : query_keys) { - const auto idx = get_index(key); - const auto dx = key - base_keys_[idx]; - interpolated_diff_values.emplace_back(3 * a_[idx] * dx * dx + 2 * b_[idx] * dx + c_[idx]); + const auto & a = multi_spline_coef_.a; + const auto & b = multi_spline_coef_.b; + const auto & c = multi_spline_coef_.c; + + 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(c.at(j) + (2.0 * b.at(j) + 3.0 * a.at(j) * ds) * ds); } - return interpolated_diff_values; + return res; } std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( @@ -237,14 +270,20 @@ std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( { // throw exceptions for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); - std::vector interpolated_quad_diff_values; - interpolated_quad_diff_values.reserve(query_keys.size()); - for (const auto & key : query_keys) { - const auto idx = get_index(key); - const auto dx = key - base_keys_[idx]; - interpolated_quad_diff_values.emplace_back(6 * a_[idx] * dx + 2 * b_[idx]); + 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 interpolated_quad_diff_values; + return res; } diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index 6e6a9192cf71a..d3cb2d6d3060b 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -64,7 +64,7 @@ TEST(spline_interpolation, spline) 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.076114, 1.001217, 1.573640}; + const std::vector ans{-0.075611, 0.997242, 1.573258}; const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -112,7 +112,7 @@ TEST(spline_interpolation, spline) const std::vector base_keys = {0.0, 1.0, 1.0001, 2.0, 3.0, 4.0}; const std::vector base_values = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; - const std::vector ans = {0.0, 0.0, 158.738293, 0.1, 0.1, 0.1}; + const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -227,7 +227,7 @@ TEST(spline_interpolation, SplineInterpolation) 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.076114, 1.001217, 1.573640}; + const std::vector ans{-0.075611, 0.997242, 1.573258}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); @@ -242,7 +242,7 @@ TEST(spline_interpolation, SplineInterpolation) 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, 12.0, 18.0}; - const std::vector ans{0.671343, 0.049289, 0.209471, -0.253746}; + const std::vector ans{0.671301, 0.0509853, 0.209426, -0.253628}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); @@ -257,7 +257,7 @@ TEST(spline_interpolation, SplineInterpolation) 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, 12.0, 18.0}; - const std::vector ans{-0.155829, 0.043097, -0.011143, -0.049611}; + const std::vector ans{-0.156582, 0.0440771, -0.0116873, -0.0495025}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(query_keys); diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index 7e41980c78bdc..4013832220cd8 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -51,7 +51,8 @@ TEST(spline_interpolation, splineYawFromPoints) 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.368174, 0.961318, 1.086098, 0.938357, 0.278594}; + 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); @@ -120,8 +121,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) // random const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); - EXPECT_NEAR(random_point.x, 5.28974, epsilon); - EXPECT_NEAR(random_point.y, 10.3450319, epsilon); + 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); @@ -139,17 +140,17 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) { // yaw // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.368174, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.278594, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.808580, epsilon); + 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.368174, epsilon); - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.278594, epsilon); + 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); @@ -164,7 +165,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.271073, epsilon); + 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); From 19ae0ddc5502dec7cca63dd631617cdd27584d53 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 17 Jul 2024 16:00:23 +0200 Subject: [PATCH 2/7] refactor(autoware_interpolation): prefix package and namespace with autoware Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 6 +-- .../README.md | 0 .../interpolation/interpolation_utils.hpp | 10 ++--- .../interpolation/linear_interpolation.hpp | 13 +++--- .../spherical_linear_interpolation.hpp | 12 ++--- .../interpolation/spline_interpolation.hpp | 14 +++--- .../spline_interpolation_points_2d.hpp | 12 ++--- .../interpolation/zero_order_hold.hpp | 16 +++---- .../package.xml | 2 +- .../src/linear_interpolation.cpp | 10 ++--- .../src/spherical_linear_interpolation.cpp | 10 ++--- .../src/spline_interpolation.cpp | 19 ++++---- .../src/spline_interpolation_points_2d.cpp | 11 ++--- .../test/src/test_interpolation.cpp | 0 .../test/src/test_interpolation_utils.cpp | 22 +++++----- .../test/src/test_linear_interpolation.cpp | 17 +++---- .../test_spherical_linear_interpolation.cpp | 6 +-- .../test/src/test_spline_interpolation.cpp | 44 ++++++++++++------- .../test_spline_interpolation_points_2d.cpp | 16 ++++--- .../test/src/test_zero_order_hold.cpp | 32 +++++++++----- common/autoware_motion_utils/package.xml | 2 +- .../src/resample/resample.cpp | 30 ++++++------- .../src/trajectory/interpolation.cpp | 22 +++++----- .../src/trajectory/path_with_lane_id.cpp | 4 +- common/object_recognition_utils/package.xml | 2 +- .../src/predicted_path_utils.cpp | 12 ++--- .../package.xml | 2 +- .../src/mpc.cpp | 6 +-- .../src/mpc_utils.cpp | 10 ++--- .../longitudinal_controller_utils.hpp | 20 ++++----- .../package.xml | 2 +- .../src/longitudinal_controller_utils.cpp | 8 ++-- .../test_longitudinal_controller_utils.cpp | 12 ++--- .../package.xml | 2 +- .../include/predicted_path_checker/utils.hpp | 2 +- .../src/predicted_path_checker_node/utils.cpp | 14 +++--- .../package.xml | 2 +- .../src/pose_covariance_modifier.cpp | 4 +- .../autoware_map_based_prediction/package.xml | 2 +- .../src/map_based_prediction_node.cpp | 2 +- .../src/path_generator.cpp | 3 +- .../package.xml | 2 +- .../optimization_based_planner.cpp | 8 ++-- .../pid_based_planner/pid_based_planner.cpp | 4 +- .../autoware/path_optimizer/mpt_optimizer.hpp | 14 +++--- .../path_optimizer/utils/geometry_utils.hpp | 6 +-- .../path_optimizer/utils/trajectory_utils.hpp | 6 +-- planning/autoware_path_optimizer/package.xml | 2 +- .../src/mpt_optimizer.cpp | 20 ++++----- planning/autoware_path_optimizer/src/node.cpp | 2 +- .../src/utils/trajectory_utils.cpp | 4 +- .../path_smoother/utils/trajectory_utils.hpp | 6 +-- planning/autoware_path_smoother/package.xml | 2 +- .../src/elastic_band_smoother.cpp | 2 +- .../src/utils/trajectory_utils.cpp | 2 +- .../package.xml | 2 +- .../src/static_centerline_generator_node.cpp | 4 +- .../autoware_velocity_smoother/package.xml | 2 +- .../velocity_planning_utils.cpp | 12 ++--- .../src/trajectory_utils.cpp | 6 +-- .../utils/data_structs.hpp | 6 +-- .../package.xml | 2 +- .../path_projection.hpp | 4 +- .../package.xml | 2 +- .../drivable_area_expansion.cpp | 2 +- .../geometric_parallel_parking.cpp | 18 ++++---- .../path_safety_checker/safety_check.cpp | 4 +- .../src/utils/path_shifter/path_shifter.cpp | 4 +- .../src/utils/path_utils.cpp | 2 +- .../package.xml | 2 +- .../src/sampling_planner_module.cpp | 2 +- .../package.xml | 2 +- .../package.xml | 2 +- .../src/scene_intersection_prepare_data.cpp | 4 +- .../package.xml | 2 +- .../src/scene_no_stopping_area.cpp | 2 +- .../package.xml | 2 +- .../src/occlusion_spot_utils.cpp | 2 +- .../package.xml | 2 +- .../utils/geometry_utils.hpp | 6 +-- .../utils/trajectory_utils.hpp | 6 +-- .../autoware_path_sampler/package.xml | 2 +- .../autoware_path_sampler/src/node.cpp | 2 +- .../src/prepare_inputs.cpp | 2 +- .../src/utils/trajectory_utils.cpp | 2 +- .../autoware_sampler_common/structures.hpp | 42 +++++++++--------- .../autoware_sampler_common/package.xml | 2 +- .../vehicle_model/sim_model_actuation_cmd.hpp | 3 +- .../sim_model_delay_steer_map_acc_geared.hpp | 6 +-- .../vehicle_model/sim_model_actuation_cmd.cpp | 4 +- .../package.xml | 2 +- .../src/accel_map.cpp | 10 ++--- .../src/brake_map.cpp | 10 ++--- .../src/steer_map.cpp | 6 +-- 94 files changed, 363 insertions(+), 349 deletions(-) rename common/{interpolation => autoware_interpolation}/CMakeLists.txt (80%) rename common/{interpolation => autoware_interpolation}/README.md (100%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/interpolation_utils.hpp (93%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/linear_interpolation.hpp (75%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/spherical_linear_interpolation.hpp (79%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/spline_interpolation.hpp (91%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/spline_interpolation_points_2d.hpp (89%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/zero_order_hold.hpp (85%) rename common/{interpolation => autoware_interpolation}/package.xml (95%) rename common/{interpolation => autoware_interpolation}/src/linear_interpolation.cpp (86%) rename common/{interpolation => autoware_interpolation}/src/spherical_linear_interpolation.cpp (88%) rename common/{interpolation => autoware_interpolation}/src/spline_interpolation.cpp (93%) rename common/{interpolation => autoware_interpolation}/src/spline_interpolation_points_2d.cpp (97%) rename common/{interpolation => autoware_interpolation}/test/src/test_interpolation.cpp (100%) rename common/{interpolation => autoware_interpolation}/test/src/test_interpolation_utils.cpp (85%) rename common/{interpolation => autoware_interpolation}/test/src/test_linear_interpolation.cpp (80%) rename common/{interpolation => autoware_interpolation}/test/src/test_spherical_linear_interpolation.cpp (96%) rename common/{interpolation => autoware_interpolation}/test/src/test_spline_interpolation.cpp (84%) rename common/{interpolation => autoware_interpolation}/test/src/test_spline_interpolation_points_2d.cpp (92%) rename common/{interpolation => autoware_interpolation}/test/src/test_zero_order_hold.cpp (80%) diff --git a/common/interpolation/CMakeLists.txt b/common/autoware_interpolation/CMakeLists.txt similarity index 80% rename from common/interpolation/CMakeLists.txt rename to common/autoware_interpolation/CMakeLists.txt index cc91bed012432..09797272a2ac8 100644 --- a/common/interpolation/CMakeLists.txt +++ b/common/autoware_interpolation/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(interpolation) +project(autoware_interpolation) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(interpolation SHARED +ament_auto_add_library(autoware_interpolation SHARED src/linear_interpolation.cpp src/spline_interpolation.cpp src/spline_interpolation_points_2d.cpp @@ -17,7 +17,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_interpolation ${test_files}) target_link_libraries(test_interpolation - interpolation + autoware_interpolation ) endif() diff --git a/common/interpolation/README.md b/common/autoware_interpolation/README.md similarity index 100% rename from common/interpolation/README.md rename to common/autoware_interpolation/README.md diff --git a/common/interpolation/include/interpolation/interpolation_utils.hpp b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp similarity index 93% rename from common/interpolation/include/interpolation/interpolation_utils.hpp rename to common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp index 9c0372f788ecb..1c0913c8847f4 100644 --- a/common/interpolation/include/interpolation/interpolation_utils.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_ -#define INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#ifndef AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ #include #include #include #include -namespace interpolation_utils +namespace autoware::interpolation { inline bool isIncreasing(const std::vector & x) { @@ -109,6 +109,6 @@ void validateKeysAndValues( throw std::invalid_argument("The size of base_keys and base_values are not the same."); } } -} // namespace interpolation_utils +} // namespace autoware::interpolation -#endif // INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#endif // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ diff --git a/common/interpolation/include/interpolation/linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp similarity index 75% rename from common/interpolation/include/interpolation/linear_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp index 1f8155613111e..762806b3a5c35 100644 --- a/common/interpolation/include/interpolation/linear_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_ -#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include -namespace interpolation +namespace autoware::interpolation { double lerp(const double src_val, const double dst_val, const double ratio); @@ -30,7 +30,6 @@ std::vector lerp( double lerp( const std::vector & base_keys, const std::vector & base_values, const double query_key); +} // namespace autoware::interpolation -} // namespace interpolation - -#endif // INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp similarity index 79% rename from common/interpolation/include/interpolation/spherical_linear_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp index 8c1d49fb5f607..2e4a8fbd42907 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ -#define INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include @@ -29,7 +29,7 @@ #include -namespace interpolation +namespace autoware::interpolation { geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, @@ -43,6 +43,6 @@ std::vector slerp( geometry_msgs::msg::Quaternion lerpOrientation( const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, const double ratio); -} // namespace interpolation +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp similarity index 91% rename from common/interpolation/include/interpolation/spline_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index 38e7b3fed5b8b..cfc58d7fc7c6c 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_ -#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#include "autoware/interpolation/interpolation_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/interpolation_utils.hpp" #include #include @@ -24,7 +24,7 @@ #include #include -namespace interpolation +namespace autoware::interpolation { // NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1) struct MultiSplineCoef @@ -52,7 +52,6 @@ std::vector spline( std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); -} // namespace interpolation // non-static 1-dimensional spline interpolation // @@ -99,10 +98,11 @@ class SplineInterpolation private: std::vector base_keys_; - interpolation::MultiSplineCoef multi_spline_coef_; + MultiSplineCoef multi_spline_coef_; void calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values); }; +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp similarity index 89% rename from common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp index b2170cf83bde2..c9668a4cad7eb 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp @@ -12,19 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ -#define INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include -namespace interpolation +namespace autoware::interpolation { template std::vector splineYawFromPoints(const std::vector & points); -} // namespace interpolation // non-static points spline interpolation // NOTE: We can calculate yaw from the x and y by interpolation derivatives. @@ -85,5 +84,6 @@ class SplineInterpolationPoints2d std::vector base_s_vec_; }; +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp similarity index 85% rename from common/interpolation/include/interpolation/zero_order_hold.hpp rename to common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp index 966128321b470..c28f870837da7 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__ZERO_ORDER_HOLD_HPP_ -#define INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#ifndef AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#define AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include -namespace interpolation +namespace autoware::interpolation { inline std::vector calc_closest_segment_indices( const std::vector & base_keys, const std::vector & query_keys, const double overlap_threshold = 1e-3) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); + const auto validated_query_keys = validateKeys(base_keys, query_keys); std::vector closest_segment_indices(validated_query_keys.size()); size_t closest_segment_idx = 0; @@ -58,7 +58,7 @@ std::vector zero_order_hold( const std::vector & closest_segment_indices) { // throw exception for invalid arguments - interpolation_utils::validateKeysAndValues(base_keys, base_values); + validateKeysAndValues(base_keys, base_values); std::vector query_values(closest_segment_indices.size()); for (size_t i = 0; i < closest_segment_indices.size(); ++i) { @@ -76,6 +76,6 @@ std::vector zero_order_hold( return zero_order_hold( base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold)); } -} // namespace interpolation +} // namespace autoware::interpolation -#endif // INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#endif // AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ diff --git a/common/interpolation/package.xml b/common/autoware_interpolation/package.xml similarity index 95% rename from common/interpolation/package.xml rename to common/autoware_interpolation/package.xml index bb4af924dd252..c0df4b45acf65 100644 --- a/common/interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -1,7 +1,7 @@ - interpolation + autoware_interpolation 0.1.0 The spline interpolation package Fumiya Watanabe diff --git a/common/interpolation/src/linear_interpolation.cpp b/common/autoware_interpolation/src/linear_interpolation.cpp similarity index 86% rename from common/interpolation/src/linear_interpolation.cpp rename to common/autoware_interpolation/src/linear_interpolation.cpp index f74d085dfee9e..4ba339f0e538e 100644 --- a/common/interpolation/src/linear_interpolation.cpp +++ b/common/autoware_interpolation/src/linear_interpolation.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include -namespace interpolation +namespace autoware::interpolation { double lerp(const double src_val, const double dst_val, const double ratio) { @@ -28,8 +28,8 @@ std::vector lerp( const std::vector & query_keys) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; @@ -56,4 +56,4 @@ double lerp( { return lerp(base_keys, base_values, std::vector{query_key}).front(); } -} // namespace interpolation +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp similarity index 88% rename from common/interpolation/src/spherical_linear_interpolation.cpp rename to common/autoware_interpolation/src/spherical_linear_interpolation.cpp index e44626498a80b..697dda3b06770 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" -namespace interpolation +namespace autoware::interpolation { geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, @@ -34,8 +34,8 @@ std::vector slerp( const std::vector & query_keys) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; @@ -68,4 +68,4 @@ geometry_msgs::msg::Quaternion lerpOrientation( const auto q_interpolated = q_from.slerp(q_to, ratio); return tf2::toMsg(q_interpolated); } -} // namespace interpolation +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/autoware_interpolation/src/spline_interpolation.cpp similarity index 93% rename from common/interpolation/src/spline_interpolation.cpp rename to common/autoware_interpolation/src/spline_interpolation.cpp index 1275b47346d78..b3f92170f9d65 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/autoware_interpolation/src/spline_interpolation.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include -namespace +namespace autoware::interpolation { // solve Ax = d // where A is tridiagonal matrix @@ -77,10 +77,7 @@ inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma return x; } -} // namespace -namespace interpolation -{ std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) @@ -164,13 +161,12 @@ std::vector splineByAkima( } return res; } -} // namespace interpolation void SplineInterpolation::calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values) { // throw exceptions for invalid arguments - interpolation_utils::validateKeysAndValues(base_keys, base_values); + validateKeysAndValues(base_keys, base_values); const size_t num_base = base_keys.size(); // N+1 @@ -204,7 +200,7 @@ void SplineInterpolation::calcSplineCoefficients( v.push_back(0.0); // calculate a, b, c, d of spline coefficients - multi_spline_coef_ = interpolation::MultiSplineCoef{num_base - 1}; // N + multi_spline_coef_ = MultiSplineCoef{num_base - 1}; // N for (size_t i = 0; i < num_base - 1; ++i) { multi_spline_coef_.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i]; multi_spline_coef_.b[i] = v[i] / 2.0; @@ -220,7 +216,7 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( 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 validated_query_keys = validateKeys(base_keys_, query_keys); const auto & a = multi_spline_coef_.a; const auto & b = multi_spline_coef_.b; @@ -245,7 +241,7 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( 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 validated_query_keys = validateKeys(base_keys_, query_keys); const auto & a = multi_spline_coef_.a; const auto & b = multi_spline_coef_.b; @@ -269,7 +265,7 @@ 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 validated_query_keys = validateKeys(base_keys_, query_keys); const auto & a = multi_spline_coef_.a; const auto & b = multi_spline_coef_.b; @@ -287,3 +283,4 @@ std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( return res; } +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp similarity index 97% rename from common/interpolation/src/spline_interpolation_points_2d.cpp rename to common/autoware_interpolation/src/spline_interpolation_points_2d.cpp index 95fde68b171bd..e0f55cfb24ba8 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include -namespace +namespace autoware::interpolation { std::vector calcEuclidDist(const std::vector & x, const std::vector & y) { @@ -66,10 +66,6 @@ std::array, 4> getBaseValues( return {base_s, base_x, base_y, base_z}; } -} // namespace - -namespace interpolation -{ template std::vector splineYawFromPoints(const std::vector & points) @@ -88,8 +84,6 @@ std::vector splineYawFromPoints(const std::vector & points) template std::vector splineYawFromPoints( const std::vector & points); -} // namespace interpolation - geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( const size_t idx, const double s) const { @@ -215,3 +209,4 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec); } +} // namespace autoware::interpolation diff --git a/common/interpolation/test/src/test_interpolation.cpp b/common/autoware_interpolation/test/src/test_interpolation.cpp similarity index 100% rename from common/interpolation/test/src/test_interpolation.cpp rename to common/autoware_interpolation/test/src/test_interpolation.cpp diff --git a/common/interpolation/test/src/test_interpolation_utils.cpp b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp similarity index 85% rename from common/interpolation/test/src/test_interpolation_utils.cpp rename to common/autoware_interpolation/test/src/test_interpolation_utils.cpp index 8b3a3b9faa0c6..2aa41b6fdef00 100644 --- a/common/interpolation/test/src/test_interpolation_utils.cpp +++ b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include @@ -24,43 +24,43 @@ TEST(interpolation_utils, isIncreasing) { // empty const std::vector empty_vec; - EXPECT_THROW(interpolation_utils::isIncreasing(empty_vec), std::invalid_argument); + EXPECT_THROW(autoware::interpolation::isIncreasing(empty_vec), std::invalid_argument); // increase const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isIncreasing(increasing_vec), true); // not decrease const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(not_increasing_vec), false); + EXPECT_EQ(autoware::interpolation::isIncreasing(not_increasing_vec), false); // decrease const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(decreasing_vec), false); + EXPECT_EQ(autoware::interpolation::isIncreasing(decreasing_vec), false); } TEST(interpolation_utils, isNotDecreasing) { // empty const std::vector empty_vec; - EXPECT_THROW(interpolation_utils::isNotDecreasing(empty_vec), std::invalid_argument); + EXPECT_THROW(autoware::interpolation::isNotDecreasing(empty_vec), std::invalid_argument); // increase const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(increasing_vec), true); // not decrease const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(not_increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(not_increasing_vec), true); // decrease const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(decreasing_vec), false); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(decreasing_vec), false); } TEST(interpolation_utils, validateKeys) { - using interpolation_utils::validateKeys; + using autoware::interpolation::validateKeys; const std::vector base_keys{0.0, 1.0, 2.0, 3.0}; const std::vector query_keys{0.0, 1.0, 2.0, 3.0}; @@ -116,7 +116,7 @@ TEST(interpolation_utils, validateKeys) TEST(interpolation_utils, validateKeysAndValues) { - using interpolation_utils::validateKeysAndValues; + using autoware::interpolation::validateKeysAndValues; const std::vector base_keys{0.0, 1.0, 2.0, 3.0}; const std::vector base_values{0.0, 1.0, 2.0, 3.0}; diff --git a/common/interpolation/test/src/test_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp similarity index 80% rename from common/interpolation/test/src/test_linear_interpolation.cpp rename to common/autoware_interpolation/test/src/test_linear_interpolation.cpp index 9c392943bd3c5..0fea1e514e916 100644 --- a/common/interpolation/test/src/test_linear_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include @@ -23,8 +23,8 @@ constexpr double epsilon = 1e-6; TEST(linear_interpolation, lerp_scalar) { - EXPECT_EQ(interpolation::lerp(0.0, 1.0, 0.3), 0.3); - EXPECT_EQ(interpolation::lerp(-0.5, 12.3, 0.3), 3.34); + EXPECT_EQ(autoware::interpolation::lerp(0.0, 1.0, 0.3), 0.3); + EXPECT_EQ(autoware::interpolation::lerp(-0.5, 12.3, 0.3), 3.34); } TEST(linear_interpolation, lerp_vector) @@ -35,7 +35,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -47,7 +47,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -59,7 +59,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -71,7 +71,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.18, 1.12, 1.4}; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -87,7 +87,8 @@ TEST(linear_interpolation, lerp_scalar_query) const std::vector ans{-0.18, 1.12, 1.4}; for (size_t i = 0; i < query_keys.size(); ++i) { - const auto query_value = interpolation::lerp(base_keys, base_values, query_keys.at(i)); + const auto query_value = + autoware::interpolation::lerp(base_keys, base_values, query_keys.at(i)); EXPECT_NEAR(query_value, ans.at(i), epsilon); } } diff --git a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp similarity index 96% rename from common/interpolation/test/src/test_spherical_linear_interpolation.cpp rename to common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp index 0bb9ba8ef2ce8..b7ce134c680bd 100644 --- a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include @@ -34,7 +34,7 @@ inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( TEST(slerp, spline_scalar) { - using interpolation::slerp; + using autoware::interpolation::slerp; // Same value { @@ -79,7 +79,7 @@ TEST(slerp, spline_scalar) TEST(slerp, spline_vector) { - using interpolation::slerp; + using autoware::interpolation::slerp; // query keys are same as base keys { diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp similarity index 84% rename from common/interpolation/test/src/test_spline_interpolation.cpp rename to common/autoware_interpolation/test/src/test_spline_interpolation.cpp index d3cb2d6d3060b..79edf9315ea44 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/spline_interpolation.hpp" #include @@ -22,6 +22,8 @@ constexpr double epsilon = 1e-6; +using autoware::interpolation::SplineInterpolation; + TEST(spline_interpolation, spline) { { // straight: query_keys is same as base_keys @@ -30,7 +32,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -42,7 +44,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -54,7 +56,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -66,7 +68,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.075611, 0.997242, 1.573258}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -78,7 +80,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -90,7 +92,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -102,7 +104,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.808769, -0.077539, 1.035096}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -114,7 +116,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -129,7 +131,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -141,7 +144,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -153,7 +157,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -165,7 +170,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.0801, 1.110749, 1.4864}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -177,7 +183,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -189,7 +196,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -201,7 +209,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.8378, -0.0801, 0.927031}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -213,7 +222,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp similarity index 92% rename from common/interpolation/test/src/test_spline_interpolation_points_2d.cpp rename to common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp index 4013832220cd8..097fc4e43cb7f 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include @@ -23,6 +23,8 @@ constexpr double epsilon = 1e-6; +using autoware::interpolation::SplineInterpolationPoints2d; + TEST(spline_interpolation, splineYawFromPoints) { using autoware::universe_utils::createPoint; @@ -37,7 +39,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -53,7 +55,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -63,7 +65,7 @@ TEST(spline_interpolation, splineYawFromPoints) std::vector points; points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); + EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) @@ -73,7 +75,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -87,7 +89,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } diff --git a/common/interpolation/test/src/test_zero_order_hold.cpp b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp similarity index 80% rename from common/interpolation/test/src/test_zero_order_hold.cpp rename to common/autoware_interpolation/test/src/test_zero_order_hold.cpp index 541af1cf76064..c99348d66d034 100644 --- a/common/interpolation/test/src/test_zero_order_hold.cpp +++ b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/zero_order_hold.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include @@ -29,7 +29,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -41,7 +42,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 0.0, 1.5, 6.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -53,7 +55,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -65,7 +68,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-1.2, 1.0, 2.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -77,7 +81,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001}; const std::vector ans = {0.0, 1.5, 2.5, 3.5, 3.5}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -89,7 +94,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001}; const std::vector ans = {0.0, 1.5, 2.5, 3.5, 0.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -104,7 +110,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys = base_keys; const auto ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_EQ(query_values.at(i), ans.at(i)); } @@ -116,7 +123,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans = {true, true, false, false}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_EQ(query_values.at(i), ans.at(i)); } @@ -128,7 +136,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001}; const std::vector ans = {true, true, false, true, true}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -140,7 +149,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001}; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 81fec78b04812..1fe2c4d8e1ea8 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -22,12 +22,12 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_interpolation autoware_planning_msgs autoware_universe_utils autoware_vehicle_msgs builtin_interfaces geometry_msgs - interpolation libboost-dev rclcpp tf2 diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 1f29a4577e428..2b70bea8a02d5 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -14,12 +14,12 @@ #include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include "autoware/motion_utils/resample/resample_utils.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" #include @@ -61,13 +61,13 @@ std::vector resamplePointVector( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; const auto spline = [&](const auto & input) { - return interpolation::spline(input_arclength, input, resampled_arclength); + return autoware::interpolation::spline(input_arclength, input, resampled_arclength); }; const auto spline_by_akima = [&](const auto & input) { - return interpolation::splineByAkima(input_arclength, input, resampled_arclength); + return autoware::interpolation::splineByAkima(input_arclength, input, resampled_arclength); }; const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); @@ -280,14 +280,14 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampling_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampling_arclength); }; auto closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -462,16 +462,16 @@ autoware_planning_msgs::msg::Path resamplePath( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; std::vector closest_segment_indices; if (use_zero_order_hold_for_v) { closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -636,16 +636,16 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; std::vector closest_segment_indices; if (use_zero_order_hold_for_twist) { closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index 4a9eaeca58d30..00e2f2e54d87d 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -14,8 +14,8 @@ #include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/linear_interpolation.hpp" using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -66,26 +66,26 @@ TrajectoryPoint calcInterpolatedPoint( interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; } else { - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); interpolated_point.acceleration_mps2 = - interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + autoware::interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation interpolated_point.heading_rate_rps = - interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + autoware::interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation - interpolated_point.front_wheel_angle_rad = interpolation::lerp( + interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); interpolated_point.rear_wheel_angle_rad = - interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + autoware::interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation - const double interpolated_time = interpolation::lerp( + const double interpolated_time = autoware::interpolation::lerp( rclcpp::Duration(curr_pt.time_from_start).seconds(), rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); @@ -134,15 +134,15 @@ PathPointWithLaneId calcInterpolatedPoint( interpolated_point.point.longitudinal_velocity_mps = curr_pt.point.longitudinal_velocity_mps; interpolated_point.point.lateral_velocity_mps = curr_pt.point.lateral_velocity_mps; } else { - interpolated_point.point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.point.longitudinal_velocity_mps, next_pt.point.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.point.lateral_velocity_mps, next_pt.point.lateral_velocity_mps, clamped_ratio); } // heading rate interpolation - interpolated_point.point.heading_rate_rps = interpolation::lerp( + interpolated_point.point.heading_rate_rps = autoware::interpolation::lerp( curr_pt.point.heading_rate_rps, next_pt.point.heading_rate_rps, clamped_ratio); return interpolated_point; diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index cd8de63f56c1d..55c7360a25c41 100644 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -14,8 +14,8 @@ #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include #include @@ -106,7 +106,7 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( auto cog_path = path; // calculate curvature and yaw from spline interpolation - const auto spline = SplineInterpolationPoints2d(path.points); + const auto spline = autoware::interpolation::SplineInterpolationPoints2d(path.points); const auto curvature_vec = spline.getSplineInterpolatedCurvatures(); const auto yaw_vec = spline.getSplineInterpolatedYaws(); diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index d0802bbc9b457..a3f4a99300716 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -12,10 +12,10 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_perception_msgs autoware_universe_utils geometry_msgs - interpolation libboost-dev pcl_conversions pcl_ros diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index e26c9e6a7e1ea..96c62d9f50323 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -14,9 +14,9 @@ #include "object_recognition_utils/predicted_path_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spherical_linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include @@ -69,13 +69,13 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( } const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_time, input, resampled_time); + return autoware::interpolation::lerp(input_time, input, resampled_time); }; const auto spline = [&](const auto & input) { - return interpolation::spline(input_time, input, resampled_time); + return autoware::interpolation::spline(input_time, input, resampled_time); }; const auto slerp = [&](const auto & input) { - return interpolation::slerp(input_time, input, resampled_time); + return autoware::interpolation::slerp(input_time, input, resampled_time); }; const auto interpolated_x = use_spline_for_xy ? spline(x) : lerp(x); diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index e25352797a0d0..5c5f8886d6ed3 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -28,7 +29,6 @@ diagnostic_updater eigen geometry_msgs - interpolation osqp_interface rclcpp rclcpp_components diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index b5cd0e7ba3e2a..0f350dc40ad0e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -14,10 +14,10 @@ #include "autoware/mpc_lateral_controller/mpc.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/mpc_lateral_controller/mpc_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "interpolation/linear_interpolation.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -383,8 +383,8 @@ std::pair MPC::updateStateForDelayCompensation( double k, v = 0.0; try { // NOTE: When driving backward, the curvature's sign should be reversed. - k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; - v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); + k = autoware::interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; + v = autoware::interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); } catch (const std::exception & e) { RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what()); return {false, {}}; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 37eb47ab0396e..c257bada0b0b2 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -14,11 +14,11 @@ #include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" #include #include @@ -134,10 +134,10 @@ std::pair resampleMPCTrajectoryByDistance( convertEulerAngleToMonotonic(input_yaw); const auto lerp_arc_length = [&](const auto & input_value) { - return interpolation::lerp(input_arclength, input_value, output_arclength); + return autoware::interpolation::lerp(input_arclength, input_value, output_arclength); }; const auto spline_arc_length = [&](const auto & input_value) { - return interpolation::spline(input_arclength, input_value, output_arclength); + return autoware::interpolation::spline(input_arclength, input_value, output_arclength); }; output.x = spline_arc_length(input.x); @@ -165,7 +165,7 @@ bool linearInterpMPCTrajectory( convertEulerAngleToMonotonic(in_traj_yaw); const auto lerp_arc_length = [&](const auto & input_value) { - return interpolation::lerp(in_index, input_value, out_index); + return autoware::interpolation::lerp(in_index, input_value, out_index); }; try { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp index a36a0b4eefccd..52cab33c048ac 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spherical_linear_interpolation.hpp" #include "tf2/utils.h" #include @@ -99,22 +99,22 @@ std::pair lerpTrajectoryPoint( { const size_t i = seg_idx; - interpolated_point.pose.position.x = interpolation::lerp( + interpolated_point.pose.position.x = autoware::interpolation::lerp( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); - interpolated_point.pose.position.y = interpolation::lerp( + interpolated_point.pose.position.y = autoware::interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.position.z = interpolation::lerp( + interpolated_point.pose.position.z = autoware::interpolation::lerp( points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio); - interpolated_point.pose.orientation = interpolation::lerpOrientation( + interpolated_point.pose.orientation = autoware::interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, interpolate_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio); - interpolated_point.acceleration_mps2 = interpolation::lerp( + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio); - interpolated_point.heading_rate_rps = interpolation::lerp( + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); } diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index 91d77e454b7ff..86dab79a74559 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -30,7 +31,6 @@ diagnostic_updater eigen geometry_msgs - interpolation rclcpp rclcpp_components std_msgs diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 5eb6c87e063eb..fa2a39aa23945 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -176,10 +176,10 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const auto p0 = trajectory.points.at(i).pose; const auto p1 = trajectory.points.at(i + 1).pose; p = trajectory.points.at(i).pose; - p.position.x = interpolation::lerp(p0.position.x, p1.position.x, ratio); - p.position.y = interpolation::lerp(p0.position.y, p1.position.y, ratio); - p.position.z = interpolation::lerp(p0.position.z, p1.position.z, ratio); - p.orientation = interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); + p.position.x = autoware::interpolation::lerp(p0.position.x, p1.position.x, ratio); + p.position.y = autoware::interpolation::lerp(p0.position.y, p1.position.y, ratio); + p.position.z = autoware::interpolation::lerp(p0.position.z, p1.position.z, ratio); + p.orientation = autoware::interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); break; } remain_dist -= dist; diff --git a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 3c547e762bce6..a898727ded3d1 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "gtest/gtest.h" -#include "interpolation/spherical_linear_interpolation.hpp" #include "tf2/LinearMath/Quaternion.h" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -303,7 +303,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +311,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +320,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +328,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +336,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index fa8f5847d6c31..548129eb06f8c 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_universe_utils @@ -30,7 +31,6 @@ diagnostic_updater eigen geometry_msgs - interpolation osqp_interface rclcpp rclcpp_components diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 77d6df2021c8f..984584d16aa8f 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include #include #include #include -#include #include #include diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 48640b21b6064..d06d2b7250689 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -119,26 +119,26 @@ TrajectoryPoint calcInterpolatedPoint( interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; } else { - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); interpolated_point.acceleration_mps2 = - interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + autoware::interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation interpolated_point.heading_rate_rps = - interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + autoware::interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation - interpolated_point.front_wheel_angle_rad = interpolation::lerp( + interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); interpolated_point.rear_wheel_angle_rad = - interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + autoware::interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation - const double interpolated_time = interpolation::lerp( + const double interpolated_time = autoware::interpolation::lerp( rclcpp::Duration(curr_pt.time_from_start).seconds(), rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); diff --git a/localization/autoware_pose_covariance_modifier/package.xml b/localization/autoware_pose_covariance_modifier/package.xml index c6c5a49f991dd..dc4b741edff79 100644 --- a/localization/autoware_pose_covariance_modifier/package.xml +++ b/localization/autoware_pose_covariance_modifier/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake + autoware_interpolation geometry_msgs - interpolation rclcpp rclcpp_components std_msgs diff --git a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp index bb811c55d585d..53d6ecb244f3e 100644 --- a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp +++ b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp @@ -14,7 +14,7 @@ #include "include/pose_covariance_modifier.hpp" -#include +#include #include #include @@ -179,7 +179,7 @@ std::array PoseCovarianceModifierNode::update_ndt_covariances_from_g const double input_normalized = (x - x_min) / (x_max - x_min); // Interpolate to the output range - return interpolation::lerp(y_min, y_max, input_normalized); + return autoware::interpolation::lerp(y_min, y_max, input_normalized); }; auto ndt_variance_from_gnss_variance = [&](double ndt_variance, double gnss_variance) { diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 9e60073de4be6..cd0b42f030eaf 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -14,12 +14,12 @@ ament_cmake autoware_cmake + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_universe_utils glog - interpolation rclcpp rclcpp_components tf2 diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 6427dd9c8914c..5a43a8fb25633 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,6 +14,7 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include #include #include #include @@ -24,7 +25,6 @@ #include #include #include -#include #include diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index d8f6f85b239a7..210795d7e6b25 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -14,9 +14,10 @@ #include "map_based_prediction/path_generator.hpp" +#include +#include #include #include -#include #include diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index a781a20388106..9999be9937111 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs object_recognition_utils osqp_interface diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 899cc66cb4331..3a24cfa7dcfe0 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -14,6 +14,9 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" @@ -21,9 +24,6 @@ #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -237,7 +237,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( } // resample optimum velocity for original each position auto resampled_opt_velocity = - interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); + autoware::interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); for (size_t i = break_id; i < stop_traj_points.size(); ++i) { resampled_opt_velocity.push_back(stop_traj_points.at(i).longitudinal_velocity_mps); } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 74f2002a93420..fefe7fd06007e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -14,12 +14,12 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "interpolation/spline_interpolation.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" @@ -609,7 +609,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( if (unique_s_vec.back() < sum_dist) { return unique_v_vec.back(); } - return interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); }(); acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp( diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 3443ab663b642..3d9192c93d5b1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ #define AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" @@ -22,8 +24,6 @@ #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "osqp_interface/osqp_interface.hpp" #include @@ -45,9 +45,9 @@ struct Bounds static Bounds lerp(Bounds prev_bounds, Bounds next_bounds, double ratio) { const double lower_bound = - interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); + autoware::interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); const double upper_bound = - interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); + autoware::interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); return Bounds{lower_bound, upper_bound}; } @@ -249,10 +249,10 @@ class MPTOptimizer const PlannerData & planner_data, const std::vector & smoothed_points) const; void updateCurvature( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateOrientation( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateBounds( std::vector & ref_points, const std::vector & left_bound, @@ -266,7 +266,7 @@ class MPTOptimizer const double ego_vel) const; void updateVehicleBounds( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateFixedPoint(std::vector & ref_points) const; void updateDeltaArcLength(std::vector & ref_points) const; void updateExtraPoints(std::vector & ref_points) const; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp index edc91bd40d4dc..48e7d68e70e18 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index c9900d2ce8225..ff53cfbc3c230 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 0830d534c7cb0..1a7869b6a87fd 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,13 +14,13 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 367fc915badba..fdffc8a926c24 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -14,13 +14,13 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tf2/utils.h" #include @@ -568,7 +568,7 @@ std::vector MPTOptimizer::calcReferencePoints( // remove repeated points ref_points = trajectory_utils::sanitizePoints(ref_points); - SplineInterpolationPoints2d ref_points_spline(ref_points); + autoware::interpolation::SplineInterpolationPoints2d ref_points_spline(ref_points); ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); // 3. calculate orientation and curvature @@ -580,7 +580,7 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points = autoware::motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length); - ref_points_spline = SplineInterpolationPoints2d(ref_points); + ref_points_spline = autoware::interpolation::SplineInterpolationPoints2d(ref_points); ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); // 5. update fixed points, and resample @@ -588,7 +588,7 @@ std::vector MPTOptimizer::calcReferencePoints( // New start point may be added and resampled. Spline calculation is required. updateFixedPoint(ref_points); ref_points = trajectory_utils::sanitizePoints(ref_points); - ref_points_spline = SplineInterpolationPoints2d(ref_points); + ref_points_spline = autoware::interpolation::SplineInterpolationPoints2d(ref_points); // 6. update bounds // NOTE: After this, resample must not be called since bounds are not interpolated. @@ -614,7 +614,7 @@ std::vector MPTOptimizer::calcReferencePoints( void MPTOptimizer::updateOrientation( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); for (size_t i = 0; i < ref_points.size(); ++i) { @@ -625,7 +625,7 @@ void MPTOptimizer::updateOrientation( void MPTOptimizer::updateCurvature( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { const auto curvature_vec = ref_points_spline.getSplineInterpolatedCurvatures(); for (size_t i = 0; i < ref_points.size(); ++i) { @@ -1092,7 +1092,7 @@ void MPTOptimizer::avoidSuddenSteering( void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1181,10 +1181,10 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( } // for avoidance if (0 < ref_points.at(i).normalized_avoidance_cost) { - const double lat_error_weight = interpolation::lerp( + const double lat_error_weight = autoware::interpolation::lerp( mpt_param_.lat_error_weight, mpt_param_.avoidance_lat_error_weight, ref_points.at(i).normalized_avoidance_cost); - const double yaw_error_weight = interpolation::lerp( + const double yaw_error_weight = autoware::interpolation::lerp( mpt_param_.yaw_error_weight, mpt_param_.avoidance_yaw_error_weight, ref_points.at(i).normalized_avoidance_cost); return {lat_error_weight, yaw_error_weight}; @@ -1206,7 +1206,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( // update R std::vector> R_triplet_vec; for (size_t i = 0; i < N_ref - 1; ++i) { - const double adaptive_steer_weight = interpolation::lerp( + const double adaptive_steer_weight = autoware::interpolation::lerp( mpt_param_.steer_input_weight, mpt_param_.avoidance_steer_input_weight, ref_points.at(i).normalized_avoidance_cost); R_triplet_vec.push_back(Eigen::Triplet(D_u * i, D_u * i, adaptive_steer_weight)); diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index f0d101a046206..62a4882f8a4c1 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -14,12 +14,12 @@ #include "autoware/path_optimizer/node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_optimizer/debug_marker.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index bcfc7e53ee67d..f5ef2ddcd2451 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -181,7 +181,7 @@ std::vector resampleReferencePoints( query_keys.push_back(base_keys.back() - epsilon); } - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); // create output reference points by updating curvature with resampled one std::vector output_ref_points; @@ -200,7 +200,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::SplineInterpolationPoints2d(traj_points); const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index f989ab76f6952..e7f64a896bdd3 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 6cecef433fc3e..b9b79bb6ceaf6 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -14,12 +14,12 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils geometry_msgs - interpolation nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index b92798f92728c..a3082a2c979c3 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -14,10 +14,10 @@ #include "autoware/path_smoother/elastic_band_smoother.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_smoother/utils/geometry_utils.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index e533b963cf655..37c27eaae6f93 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -79,7 +79,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::SplineInterpolationPoints2d(traj_points); const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 931b815b176c3..2abae4af2ca67 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -17,6 +17,7 @@ autoware_behavior_path_planner_common autoware_geography_utils + autoware_interpolation autoware_lanelet2_extension autoware_map_msgs autoware_map_projection_loader @@ -31,7 +32,6 @@ autoware_vehicle_info_utils geometry_msgs global_parameter_loader - interpolation map_loader osqp_interface rclcpp diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 256cd2386c431..79d070c99a232 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -16,6 +16,7 @@ #include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" #include "autoware/map_projection_loader/map_projection_loader.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" @@ -25,7 +26,6 @@ #include "autoware_lanelet2_extension/utility/utilities.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" #include "type_alias.hpp" #include "utils.hpp" @@ -625,7 +625,7 @@ void StaticCenterlineGeneratorNode::validate() } // calculate curvature - SplineInterpolationPoints2d centerline_spline(centerline); + autoware::interpolation::SplineInterpolationPoints2d centerline_spline(centerline); const auto curvature_vec = centerline_spline.getSplineInterpolatedCurvatures(); const double curvature_threshold = vehicle_info_.calcCurvatureFromSteerAngle( vehicle_info_.max_steer_angle_rad - max_steer_angle_margin); diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index f5cd3f2667c04..65684e414e90d 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -21,13 +21,13 @@ autoware_cmake eigen3_cmake_module + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs osqp_interface qp_interface diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 6e12d4a6f20fb..cc84938e31628 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -14,7 +14,7 @@ #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -234,8 +234,8 @@ bool calcStopVelocityWithConstantJerkAccLimit( } if ( - !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distances) || - !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distances)) { + !autoware::interpolation::isIncreasing(xs) || !autoware::interpolation::isIncreasing(distances) || + !autoware::interpolation::isNotDecreasing(xs) || !autoware::interpolation::isNotDecreasing(distances)) { return false; } @@ -245,9 +245,9 @@ bool calcStopVelocityWithConstantJerkAccLimit( return false; } - const auto vel_at_wp = interpolation::lerp(xs, vs, distances); - const auto acc_at_wp = interpolation::lerp(xs, as, distances); - const auto jerk_at_wp = interpolation::lerp(xs, js, distances); + const auto vel_at_wp = autoware::interpolation::lerp(xs, vs, distances); + const auto acc_at_wp = autoware::interpolation::lerp(xs, as, distances); + const auto jerk_at_wp = autoware::interpolation::lerp(xs, js, distances); for (size_t i = 0; i < vel_at_wp.size(); ++i) { output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index 68f05438d38c8..cf031601cfd57 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/linear_interpolation.hpp" #include #include @@ -88,10 +88,10 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( const auto & seg_pt = trajectory.at(seg_idx); const auto & next_pt = trajectory.at(seg_idx + 1); traj_p.pose = autoware::universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); - traj_p.longitudinal_velocity_mps = interpolation::lerp( + traj_p.longitudinal_velocity_mps = autoware::interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = - interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); + autoware::interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); } return traj_p; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 4ba2e1aef893c..6aba4708ddca4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -18,9 +18,9 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include +#include #include #include -#include #include @@ -74,8 +74,8 @@ struct LateralAccelerationMap return std::make_pair(base_min_acc.back(), base_max_acc.back()); } - const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + const double min_acc = autoware::interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = autoware::interpolation::lerp(base_vel, base_max_acc, velocity); return std::make_pair(min_acc, max_acc); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 1e52977611759..6196a4f02cff9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -40,6 +40,7 @@ autoware_behavior_path_planner_common autoware_freespace_planning_algorithms autoware_frenet_planner + autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils @@ -53,7 +54,6 @@ autoware_vehicle_msgs behaviortree_cpp_v3 geometry_msgs - interpolation libboost-dev libopencv-dev magic_enum diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 9296fd22d374b..92102aa3d75e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include @@ -108,7 +108,7 @@ inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const /// @return point interpolated between a and b as per the given ratio inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { - return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; + return {interpolation::lerp(a.x(), b.x(), ratio), autoware::interpolation::lerp(a.y(), b.y(), ratio)}; } /// @brief calculate the point with distance and arc length relative to a linestring diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 7a189250c088d..b1d3a39379d15 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -44,6 +44,7 @@ autoware_adapi_v1_msgs autoware_freespace_planning_algorithms + autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils @@ -55,7 +56,6 @@ autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation magic_enum object_recognition_utils rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index dcab7c22a35fe..648bb0a17622c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -20,13 +20,13 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include #include #include #include #include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index dc0f13270bca2..e2218e37a771b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,18 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" - -#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" - +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index fb1da48e41875..e2ff8ce7b5195 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -15,10 +15,10 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "interpolation/linear_interpolation.hpp" #include #include @@ -364,7 +364,7 @@ std::optional calcInterpolatedPoseWithVelocity( const auto interpolated_pose = autoware::universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); const double interpolated_velocity = - interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); + autoware::interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); return PoseWithVelocityStamped{relative_time, interpolated_pose, interpolated_velocity}; } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 8a6c899b4bd6d..69c538fa6cb54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include -#include #include #include @@ -252,7 +252,7 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs query_distance.push_back(dist_from_start); } if (!query_distance.empty()) { - query_length = interpolation::spline(base_distance, base_length, query_distance); + query_length = autoware::interpolation::spline(base_distance, base_length, query_distance); } // Apply shifting. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 5b234198d9137..3b06e084690e3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -17,12 +17,12 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index d868025475444..5f9f4bcd12b75 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -16,6 +16,7 @@ autoware_behavior_path_planner_common autoware_bezier_sampler autoware_frenet_planner + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_path_sampler @@ -28,7 +29,6 @@ autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs - interpolation pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 8fdedf38ed756..f05045f284413 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -956,7 +956,7 @@ autoware::frenet_planner::SamplingParameters SamplingPlannerModule::prepareSampl max_d -= params_.constraints.ego_width / 2.0; if (min_d < max_d) { for (auto r = 0.0; r <= 1.0; r += 1.0 / (params_.sampling.nb_target_lateral_positions - 1)) - target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r)); + target_lateral_positions.push_back(autoware::interpolation::lerp(min_d, max_d, r)); } } else { target_lateral_positions = params_.sampling.target_lateral_positions; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index cd9c63fc0367f..be04f1f9180b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -34,7 +35,6 @@ geometry_msgs grid_map_core grid_map_ros - interpolation libboost-dev nav_msgs pcl_conversions diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 93e1976a251ba..1c5d23e31b2e1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -31,7 +32,6 @@ autoware_vehicle_info_utils fmt geometry_msgs - interpolation libopencv-dev magic_enum nav_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index ae1d9768eaecc..b694c8aaa2e3e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -17,11 +17,11 @@ #include // for to_bg2d #include // for planning_utils:: +#include #include #include // for lanelet::autoware::RoadMarking #include #include -#include #include #include @@ -146,7 +146,7 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) points.push_back(*point); } - SplineInterpolationPoints2d interpolation(points); + autoware::interpolation::SplineInterpolationPoints2d interpolation(points); const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); std::vector curvatures_positive; for (const auto & curvature : curvatures) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index b630f988af662..21664f7596d60 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_vehicle_info_utils eigen geometry_msgs - interpolation libboost-dev pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index f2bcc0b5abf44..ac58c62036e26 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -17,10 +17,10 @@ #include #include #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index 22da467ba9fac..b414769e84ec2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -18,6 +18,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_vehicle_info_utils geometry_msgs grid_map_ros - interpolation libboost-dev libopencv-dev nav_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 6d40c2167961a..56a5acd8c7dc4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -18,10 +18,10 @@ #include #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 0122c220e7e51..9e5e8b0edda7b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_interpolation autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -34,7 +35,6 @@ diagnostic_msgs eigen geometry_msgs - interpolation nav_msgs pcl_conversions rclcpp diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index a27dd1576c90d..65ee862592bef 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index 992ad95c72bb1..73a2b0c95493e 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/structures.hpp" #include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 26f85631d569f..ca137a0d06a53 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -15,13 +15,13 @@ autoware_bezier_sampler autoware_frenet_planner + autoware_interpolation autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs rclcpp rclcpp_components diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 15d7e883a31d2..34905f79cd364 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -14,6 +14,7 @@ #include "autoware_path_sampler/node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware_path_sampler/path_generation.hpp" @@ -22,7 +23,6 @@ #include "autoware_path_sampler/utils/trajectory_utils.hpp" #include "autoware_sampler_common/constraints/hard_constraint.hpp" #include "autoware_sampler_common/constraints/soft_constraint.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index 06e8ab9042207..127393abc5ac3 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -82,7 +82,7 @@ autoware::frenet_planner::SamplingParameters prepareSamplingParameters( max_d -= params.constraints.ego_width / 2.0; if (min_d < max_d) { for (auto r = 0.0; r <= 1.0; r += 1.0 / (params.sampling.nb_target_lateral_positions - 1)) - target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r)); + target_lateral_positions.push_back(autoware::interpolation::lerp(min_d, max_d, r)); } } else { target_lateral_positions = params.sampling.target_lateral_positions; diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index e8379091615f8..e6317cd21f17d 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -65,7 +65,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::SplineInterpolationPoints2d(traj_points); const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { diff --git a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index 29b3f5a8fc145..6ecb38fa22e85 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -17,8 +17,8 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include #include -#include #include @@ -269,7 +269,7 @@ struct Trajectory : Path t.lengths.reserve(new_size); for (auto i = 0lu; i < new_size; ++i) t.lengths.push_back(lengths.front() + static_cast(i) * fixed_interval); - t.times = interpolation::lerp(lengths, times, t.lengths); + t.times = autoware::interpolation::lerp(lengths, times, t.lengths); std::vector xs; std::vector ys; xs.reserve(points.size()); @@ -278,16 +278,16 @@ struct Trajectory : Path xs.push_back(p.x()); ys.push_back(p.y()); } - const auto new_xs = interpolation::lerp(times, xs, t.times); - const auto new_ys = interpolation::lerp(times, ys, t.times); + const auto new_xs = autoware::interpolation::lerp(times, xs, t.times); + const auto new_ys = autoware::interpolation::lerp(times, ys, t.times); for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]); - t.curvatures = interpolation::lerp(times, curvatures, t.times); - t.jerks = interpolation::lerp(times, jerks, t.times); - t.yaws = interpolation::lerp(times, yaws, t.times); - t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times); - t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times); - t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times); - t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times); + t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); + t.jerks = autoware::interpolation::lerp(times, jerks, t.times); + t.yaws = autoware::interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; t.cost = cost; return t; @@ -305,7 +305,7 @@ struct Trajectory : Path t.lengths.reserve(new_size); for (auto i = 0lu; i < new_size; ++i) t.times.push_back(static_cast(i) * fixed_interval); - t.lengths = interpolation::lerp(times, lengths, t.times); + t.lengths = autoware::interpolation::lerp(times, lengths, t.times); std::vector xs; std::vector ys; xs.reserve(points.size()); @@ -314,16 +314,16 @@ struct Trajectory : Path xs.push_back(p.x()); ys.push_back(p.y()); } - const auto new_xs = interpolation::lerp(times, xs, t.times); - const auto new_ys = interpolation::lerp(times, ys, t.times); + const auto new_xs = autoware::interpolation::lerp(times, xs, t.times); + const auto new_ys = autoware::interpolation::lerp(times, ys, t.times); for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]); - t.curvatures = interpolation::lerp(times, curvatures, t.times); - t.jerks = interpolation::lerp(times, jerks, t.times); - t.yaws = interpolation::lerp(times, yaws, t.times); - t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times); - t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times); - t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times); - t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times); + t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); + t.jerks = autoware::interpolation::lerp(times, jerks, t.times); + t.yaws = autoware::interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; t.cost = cost; return t; diff --git a/planning/sampling_based_planner/autoware_sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml index a1462131d62ad..d1f1e3946a0dd 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -11,7 +11,7 @@ autoware_cmake - interpolation + autoware_interpolation ament_cmake_ros ament_lint_auto diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp index 5615f5c118165..6f67b47d0c768 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -15,9 +15,10 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" + #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "interpolation/linear_interpolation.hpp" #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index ea0653c879727..88e640edd0ab5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -15,9 +15,9 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "interpolation/linear_interpolation.hpp" #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" @@ -57,13 +57,13 @@ class AccelerationMap // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : acceleration_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc const double clamped_acc = CSVLoader::clampValue(acc_des, acc_index_); - const double acc = interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); + const double acc = autoware::interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); return acc; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 19794c04750fd..4e0ead4106758 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -43,11 +43,11 @@ double ActuationMap::getControlCommand(const double actuation, const double stat for (std::vector control_command_values : actuation_map_) { interpolated_control_vec.push_back( - interpolation::lerp(state_index_, control_command_values, clamped_state)); + autoware::interpolation::lerp(state_index_, control_command_values, clamped_state)); } const double clamped_actuation = CSVLoader::clampValue(actuation, actuation_index_); - return interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation); + return autoware::interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation); } std::optional AccelMap::getThrottle(const double acc, double vel) const diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index 4ed4fbee7ac81..86a76c397c728 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -22,9 +22,9 @@ autoware_cmake autoware_control_msgs + autoware_interpolation autoware_vehicle_msgs geometry_msgs - interpolation nav_msgs rclcpp rclcpp_components diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp index 236aa7dc451c3..81bafa709fbd0 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/accel_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -50,7 +50,7 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return false => brake sequence @@ -61,7 +61,7 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons throttle = throttle_index_.back(); return true; } - throttle = interpolation::lerp(interpolated_acc_vec, throttle_index_, acc); + throttle = autoware::interpolation::lerp(interpolated_acc_vec, throttle_index_, acc); return true; } @@ -72,14 +72,14 @@ bool AccelMap::getAcceleration(const double throttle, const double vel, double & // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : accel_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc const double clamped_throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc"); - acc = interpolation::lerp(throttle_index_, interpolated_acc_vec, clamped_throttle); + acc = autoware::interpolation::lerp(throttle_index_, interpolated_acc_vec, clamped_throttle); return true; } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp index ae0f8e5f41b1c..9036a48b027fc 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -51,7 +51,7 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : brake_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate brake @@ -71,7 +71,7 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) } std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); - brake = interpolation::lerp(interpolated_acc_vec, brake_index_rev_, acc); + brake = autoware::interpolation::lerp(interpolated_acc_vec, brake_index_rev_, acc); return true; } @@ -83,14 +83,14 @@ bool BrakeMap::getAcceleration(const double brake, const double vel, double & ac // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : brake_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate brake // When the desired acceleration is smaller than the brake area, return min acc // When the desired acceleration is greater than the brake area, return min acc const double clamped_brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc"); - acc = interpolation::lerp(brake_index_, interpolated_acc_vec, clamped_brake); + acc = autoware::interpolation::lerp(brake_index_, interpolated_acc_vec, clamped_brake); return true; } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp index 6abe8adfdc9e3..2af6029de1bf7 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -46,11 +46,11 @@ void SteerMap::getSteer(const double steer_rate, const double steer, double & ou const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); std::vector steer_rate_interp = {}; for (const auto & steer_rate_vec : steer_map_) { - steer_rate_interp.push_back(interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); + steer_rate_interp.push_back(autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); } const double clamped_steer_rate = CSVLoader::clampValue(steer_rate, steer_rate_interp, "steer: steer_rate"); - output = interpolation::lerp(steer_rate_interp, output_index_, clamped_steer_rate); + output = autoware::interpolation::lerp(steer_rate_interp, output_index_, clamped_steer_rate); } } // namespace autoware::raw_vehicle_cmd_converter From f9ff64244b429f0d4ee77eeb8e51ae6c721e693a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 14 Aug 2024 14:45:21 +0000 Subject: [PATCH 3/7] style(pre-commit): autofix --- .../autoware_motion_utils/src/resample/resample.cpp | 9 ++++++--- .../src/trajectory/interpolation.cpp | 12 ++++++------ .../src/longitudinal_controller_utils.cpp | 3 ++- .../src/predicted_path_checker_node/utils.cpp | 12 ++++++------ .../velocity_planning_utils.cpp | 6 ++++-- .../drivable_area_expansion/path_projection.hpp | 3 ++- .../include/autoware_sampler_common/structures.hpp | 12 ++++++++---- .../vehicle_model/sim_model_actuation_cmd.hpp | 1 - .../sim_model_delay_steer_map_acc_geared.hpp | 3 ++- .../vehicle_model/sim_model_actuation_cmd.cpp | 3 ++- .../src/accel_map.cpp | 3 ++- .../src/brake_map.cpp | 3 ++- .../src/steer_map.cpp | 3 ++- 13 files changed, 44 insertions(+), 29 deletions(-) diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 2b70bea8a02d5..88ebf3f41c408 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -287,7 +287,8 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); const auto zoh = [&](const auto & input) { - return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -471,7 +472,8 @@ autoware_planning_msgs::msg::Path resamplePath( autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -645,7 +647,8 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index 00e2f2e54d87d..0ae9d44683f62 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -70,19 +70,19 @@ TrajectoryPoint calcInterpolatedPoint( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); - interpolated_point.acceleration_mps2 = - autoware::interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( + curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation - interpolated_point.heading_rate_rps = - autoware::interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); - interpolated_point.rear_wheel_angle_rad = - autoware::interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation const double interpolated_time = autoware::interpolation::lerp( diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index fa2a39aa23945..134de5d1ac8bd 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -179,7 +179,8 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( p.position.x = autoware::interpolation::lerp(p0.position.x, p1.position.x, ratio); p.position.y = autoware::interpolation::lerp(p0.position.y, p1.position.y, ratio); p.position.z = autoware::interpolation::lerp(p0.position.z, p1.position.z, ratio); - p.orientation = autoware::interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); + p.orientation = + autoware::interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); break; } remain_dist -= dist; diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index d06d2b7250689..4b6d0755db25f 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -123,19 +123,19 @@ TrajectoryPoint calcInterpolatedPoint( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); - interpolated_point.acceleration_mps2 = - autoware::interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( + curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation - interpolated_point.heading_rate_rps = - autoware::interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); - interpolated_point.rear_wheel_angle_rad = - autoware::interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation const double interpolated_time = autoware::interpolation::lerp( diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index cc84938e31628..48f3138cfe151 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -234,8 +234,10 @@ bool calcStopVelocityWithConstantJerkAccLimit( } if ( - !autoware::interpolation::isIncreasing(xs) || !autoware::interpolation::isIncreasing(distances) || - !autoware::interpolation::isNotDecreasing(xs) || !autoware::interpolation::isNotDecreasing(distances)) { + !autoware::interpolation::isIncreasing(xs) || + !autoware::interpolation::isIncreasing(distances) || + !autoware::interpolation::isNotDecreasing(xs) || + !autoware::interpolation::isNotDecreasing(distances)) { return false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 92102aa3d75e7..f341fb6ba90b2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -108,7 +108,8 @@ inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const /// @return point interpolated between a and b as per the given ratio inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { - return {interpolation::lerp(a.x(), b.x(), ratio), autoware::interpolation::lerp(a.y(), b.y(), ratio)}; + return { + interpolation::lerp(a.x(), b.x(), ratio), autoware::interpolation::lerp(a.y(), b.y(), ratio)}; } /// @brief calculate the point with distance and arc length relative to a linestring diff --git a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index 6ecb38fa22e85..c2cf432229b1b 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -284,8 +284,10 @@ struct Trajectory : Path t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); t.jerks = autoware::interpolation::lerp(times, jerks, t.times); t.yaws = autoware::interpolation::lerp(times, yaws, t.times); - t.longitudinal_velocities = autoware::interpolation::lerp(times, longitudinal_velocities, t.times); - t.longitudinal_accelerations = autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.longitudinal_velocities = + autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = + autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; @@ -320,8 +322,10 @@ struct Trajectory : Path t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); t.jerks = autoware::interpolation::lerp(times, jerks, t.times); t.yaws = autoware::interpolation::lerp(times, yaws, t.times); - t.longitudinal_velocities = autoware::interpolation::lerp(times, longitudinal_velocities, t.times); - t.longitudinal_accelerations = autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.longitudinal_velocities = + autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = + autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp index 6f67b47d0c768..b78ec8ccd538a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -16,7 +16,6 @@ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ #include "autoware/interpolation/linear_interpolation.hpp" - #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" #include "simple_planning_simulator/utils/csv_loader.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 88e640edd0ab5..454a35d95eb8b 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -57,7 +57,8 @@ class AccelerationMap // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : acceleration_map_) { - interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 4e0ead4106758..58d43b619c231 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -47,7 +47,8 @@ double ActuationMap::getControlCommand(const double actuation, const double stat } const double clamped_actuation = CSVLoader::clampValue(actuation, actuation_index_); - return autoware::interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation); + return autoware::interpolation::lerp( + actuation_index_, interpolated_control_vec, clamped_actuation); } std::optional AccelMap::getThrottle(const double acc, double vel) const diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp index 81bafa709fbd0..7365c6380900f 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp @@ -50,7 +50,8 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map_) { - interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return false => brake sequence diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp index 9036a48b027fc..48660eccf8640 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp @@ -51,7 +51,8 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : brake_map_) { - interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate brake diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp index 2af6029de1bf7..babefe96eb91d 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp @@ -46,7 +46,8 @@ void SteerMap::getSteer(const double steer_rate, const double steer, double & ou const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); std::vector steer_rate_interp = {}; for (const auto & steer_rate_vec : steer_map_) { - steer_rate_interp.push_back(autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); + steer_rate_interp.push_back( + autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); } const double clamped_steer_rate = From 667b4d4b3a952c2e1585b9821a5bb92211064368 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 28 Aug 2024 01:22:44 +0900 Subject: [PATCH 4/7] add prefix to new change Signed-off-by: kosuke55 --- .../vehicle_model/sim_model_actuation_cmd.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 58d43b619c231..069cb9e569580 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -62,7 +62,8 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } // calculate throttle @@ -74,7 +75,7 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const return throttle_indices.back(); } - return interpolation::lerp(interpolated_acc_vec, throttle_indices, acc); + return autoware::interpolation::lerp(interpolated_acc_vec, throttle_indices, acc); } double BrakeMap::getBrake(const double acc, double vel) const @@ -88,7 +89,8 @@ double BrakeMap::getBrake(const double acc, double vel) const // (brake, vel, acc) map => (brake, acc) map by fixing vel for (std::vector accelerations : brake_map) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } // calculate brake @@ -101,7 +103,7 @@ double BrakeMap::getBrake(const double acc, double vel) const } std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); - return interpolation::lerp(interpolated_acc_vec, brake_indices, acc); + return autoware::interpolation::lerp(interpolated_acc_vec, brake_indices, acc); } // steer map sim model From 65dcc7fb5fcae1c2c1b2cebe70694d9d3e984cb1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 18 Sep 2024 07:55:56 +0000 Subject: [PATCH 5/7] style(pre-commit): autofix --- .../src/static_centerline_generator_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 79d070c99a232..a6ba007e71485 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,9 +14,9 @@ #include "static_centerline_generator_node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" #include "autoware/map_projection_loader/map_projection_loader.hpp" -#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" From 4996cf7676443990ea3b7c5fbc67fa2f6c3423c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Fri, 20 Sep 2024 13:09:45 +0300 Subject: [PATCH 6/7] fix(interpolation): fix bug of interpolation (#8903) * fix(interpolation): fix bug of interpolation Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * auto -> int64_t Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- .../interpolation/spline_interpolation.hpp | 29 +-- common/autoware_interpolation/package.xml | 1 + .../src/spline_interpolation.cpp | 237 ++++++++---------- .../test/src/test_spline_interpolation.cpp | 10 +- .../test_spline_interpolation_points_2d.cpp | 19 +- 5 files changed, 123 insertions(+), 173 deletions(-) diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index cfc58d7fc7c6c..f7feada78ff4f 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -18,6 +18,8 @@ #include "autoware/interpolation/interpolation_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include + #include #include #include @@ -26,25 +28,6 @@ namespace autoware::interpolation { -// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1) -struct MultiSplineCoef -{ - MultiSplineCoef() = default; - - explicit MultiSplineCoef(const size_t num_spline) - { - a.resize(num_spline); - b.resize(num_spline); - c.resize(num_spline); - d.resize(num_spline); - } - - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; - // static spline interpolation functions std::vector spline( const std::vector & base_keys, const std::vector & base_values, @@ -97,11 +80,17 @@ class SplineInterpolation size_t getSize() const { return base_keys_.size(); } private: + Eigen::VectorXd a_; + Eigen::VectorXd b_; + Eigen::VectorXd c_; + Eigen::VectorXd d_; + std::vector base_keys_; - MultiSplineCoef multi_spline_coef_; void calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values); + + Eigen::Index get_index(const double & key) const; }; } // namespace autoware::interpolation diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml index c0df4b45acf65..d2538bd602f45 100644 --- a/common/autoware_interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_universe_utils + eigen ament_cmake_ros ament_lint_auto diff --git a/common/autoware_interpolation/src/spline_interpolation.cpp b/common/autoware_interpolation/src/spline_interpolation.cpp index b3f92170f9d65..991421919b818 100644 --- a/common/autoware_interpolation/src/spline_interpolation.cpp +++ b/common/autoware_interpolation/src/spline_interpolation.cpp @@ -14,65 +14,40 @@ #include "autoware/interpolation/spline_interpolation.hpp" +#include #include namespace autoware::interpolation { -// solve Ax = d -// where A is tridiagonal matrix -// [b_0 c_0 ... ] -// [a_0 b_1 c_1 ... O ] -// A = [ ... ] -// [ O ... a_N-3 b_N-2 c_N-2] -// [ ... a_N-2 b_N-1] -struct TDMACoef +Eigen::VectorXd solve_tridiagonal_matrix_algorithm( + const Eigen::Ref & a, const Eigen::Ref & b, + const Eigen::Ref & c, const Eigen::Ref & d) { - explicit TDMACoef(const size_t num_row) - { - a.resize(num_row - 1); - b.resize(num_row); - c.resize(num_row - 1); - d.resize(num_row); + const auto n = d.size(); + + if (n == 1) { + return d.array() / b.array(); } - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; + Eigen::VectorXd c_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd d_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd x = Eigen::VectorXd::Zero(n); -inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma_coef) -{ - const auto & a = tdma_coef.a; - const auto & b = tdma_coef.b; - const auto & c = tdma_coef.c; - const auto & d = tdma_coef.d; - - const size_t num_row = b.size(); - - std::vector x(num_row); - if (num_row != 1) { - // calculate p and q - std::vector p; - std::vector q; - p.push_back(-c[0] / b[0]); - q.push_back(d[0] / b[0]); - - for (size_t i = 1; i < num_row; ++i) { - const double den = b[i] + a[i - 1] * p[i - 1]; - p.push_back(-c[i - 1] / den); - q.push_back((d[i] - a[i - 1] * q[i - 1]) / den); - } + // Forward sweep + c_prime(0) = c(0) / b(0); + d_prime(0) = d(0) / b(0); - // calculate solution - x[num_row - 1] = q[num_row - 1]; + for (auto i = 1; i < n; i++) { + const double m = 1.0 / (b(i) - a(i - 1) * c_prime(i - 1)); + c_prime(i) = i < n - 1 ? c(i) * m : 0; + d_prime(i) = (d(i) - a(i - 1) * d_prime(i - 1)) * m; + } - for (size_t i = 1; i < num_row; ++i) { - const size_t j = num_row - 1 - i; - x[j] = p[j] * x[j + 1] + q[j]; - } - } else { - x[0] = (d[0] / b[0]); + // Back substitution + x(n - 1) = d_prime(n - 1); + + for (int64_t i = n - 2; i >= 0; i--) { + x(i) = d_prime(i) - c_prime(i) * x(i + 1); } return x; @@ -162,53 +137,59 @@ std::vector splineByAkima( return res; } +Eigen::Index SplineInterpolation::get_index(const double & key) const +{ + const auto it = std::lower_bound(base_keys_.begin(), base_keys_.end(), key); + return std::clamp( + static_cast(std::distance(base_keys_.begin(), it)) - 1, 0, + static_cast(base_keys_.size()) - 2); +} + void SplineInterpolation::calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values) { // throw exceptions for invalid arguments - validateKeysAndValues(base_keys, base_values); - - const size_t num_base = base_keys.size(); // N+1 - - std::vector diff_keys; // N - std::vector diff_values; // N - for (size_t i = 0; i < num_base - 1; ++i) { - diff_keys.push_back(base_keys.at(i + 1) - base_keys.at(i)); - diff_values.push_back(base_values.at(i + 1) - base_values.at(i)); - } - - std::vector v = {0.0}; - if (num_base > 2) { - // solve tridiagonal matrix algorithm - TDMACoef tdma_coef(num_base - 2); // N-1 - - for (size_t i = 0; i < num_base - 2; ++i) { - tdma_coef.b[i] = 2 * (diff_keys[i] + diff_keys[i + 1]); - if (i != num_base - 3) { - tdma_coef.a[i] = diff_keys[i + 1]; - tdma_coef.c[i] = diff_keys[i + 1]; - } - tdma_coef.d[i] = - 6.0 * (diff_values[i + 1] / diff_keys[i + 1] - diff_values[i] / diff_keys[i]); - } - - const std::vector tdma_res = solveTridiagonalMatrixAlgorithm(tdma_coef); - - // calculate v - v.insert(v.end(), tdma_res.begin(), tdma_res.end()); - } - v.push_back(0.0); - - // calculate a, b, c, d of spline coefficients - multi_spline_coef_ = MultiSplineCoef{num_base - 1}; // N - for (size_t i = 0; i < num_base - 1; ++i) { - multi_spline_coef_.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i]; - multi_spline_coef_.b[i] = v[i] / 2.0; - multi_spline_coef_.c[i] = - diff_values[i] / diff_keys[i] - diff_keys[i] * (2 * v[i] + v[i + 1]) / 6.0; - multi_spline_coef_.d[i] = base_values[i]; + interpolation_utils::validateKeysAndValues(base_keys, base_values); + const Eigen::VectorXd x = Eigen::Map( + base_keys.data(), static_cast(base_keys.size())); + const Eigen::VectorXd y = Eigen::Map( + base_values.data(), static_cast(base_values.size())); + + const auto n = x.size(); + + if (n == 2) { + a_ = Eigen::VectorXd::Zero(1); + b_ = Eigen::VectorXd::Zero(1); + c_ = Eigen::VectorXd::Zero(1); + d_ = Eigen::VectorXd::Zero(1); + c_[0] = (y[1] - y[0]) / (x[1] - x[0]); + d_[0] = y[0]; + base_keys_ = base_keys; + return; } + // Create Tridiagonal matrix + Eigen::VectorXd v(n); + const Eigen::VectorXd h = x.segment(1, n - 1) - x.segment(0, n - 1); + const Eigen::VectorXd a = h.segment(1, n - 3); + const Eigen::VectorXd b = 2 * (h.segment(0, n - 2) + h.segment(1, n - 2)); + const Eigen::VectorXd c = h.segment(1, n - 3); + const Eigen::VectorXd y_diff = y.segment(1, n - 1) - y.segment(0, n - 1); + const Eigen::VectorXd d = 6 * (y_diff.segment(1, n - 2).array() / h.tail(n - 2).array() - + y_diff.segment(0, n - 2).array() / h.head(n - 2).array()); + + // Solve tridiagonal matrix + v.segment(1, n - 2) = solve_tridiagonal_matrix_algorithm(a, b, c, d); + v[0] = 0; + v[n - 1] = 0; + + // Calculate spline coefficients + a_ = (v.tail(n - 1) - v.head(n - 1)).array() / 6.0 / (x.tail(n - 1) - x.head(n - 1)).array(); + b_ = v.segment(0, n - 1) / 2.0; + c_ = (y.tail(n - 1) - y.head(n - 1)).array() / (x.tail(n - 1) - x.head(n - 1)).array() - + (x.tail(n - 1) - x.head(n - 1)).array() * + (2 * v.segment(0, n - 1).array() + v.segment(1, n - 1).array()) / 6.0; + d_ = y.head(n - 1); base_keys_ = base_keys; } @@ -216,71 +197,51 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - const auto & d = multi_spline_coef_.d; - - 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(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_values; + interpolated_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_values.emplace_back( + a_[idx] * dx * dx * dx + b_[idx] * dx * dx + c_[idx] * dx + d_[idx]); } - return res; + return interpolated_values; } std::vector SplineInterpolation::getSplineInterpolatedDiffValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - - 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(c.at(j) + (2.0 * b.at(j) + 3.0 * a.at(j) * ds) * ds); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_diff_values; + interpolated_diff_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_diff_values.emplace_back(3 * a_[idx] * dx * dx + 2 * b_[idx] * dx + c_[idx]); } - return res; + return interpolated_diff_values; } std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = 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); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_quad_diff_values; + interpolated_quad_diff_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_quad_diff_values.emplace_back(6 * a_[idx] * dx + 2 * b_[idx]); } - return res; + return interpolated_quad_diff_values; } } // namespace autoware::interpolation diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp index 79edf9315ea44..766e94a47b968 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -66,7 +66,7 @@ TEST(spline_interpolation, spline) 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}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -114,7 +114,7 @@ TEST(spline_interpolation, spline) const std::vector base_keys = {0.0, 1.0, 1.0001, 2.0, 3.0, 4.0}; const std::vector base_values = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; - const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; + const std::vector ans = {0.0, 0.0, 158.738293, 0.1, 0.1, 0.1}; const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -237,7 +237,7 @@ TEST(spline_interpolation, SplineInterpolation) 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}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); @@ -252,7 +252,7 @@ TEST(spline_interpolation, SplineInterpolation) 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, 12.0, 18.0}; - const std::vector ans{0.671301, 0.0509853, 0.209426, -0.253628}; + const std::vector ans{0.671343, 0.049289, 0.209471, -0.253746}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); @@ -267,7 +267,7 @@ TEST(spline_interpolation, SplineInterpolation) 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, 12.0, 18.0}; - const std::vector ans{-0.156582, 0.0440771, -0.0116873, -0.0495025}; + const std::vector ans{-0.155829, 0.043097, -0.011143, -0.049611}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(query_keys); diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp index 097fc4e43cb7f..974ad606b978d 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -53,8 +53,7 @@ TEST(spline_interpolation, splineYawFromPoints) 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 std::vector ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594}; const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); @@ -123,8 +122,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) // 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); + EXPECT_NEAR(random_point.x, 5.28974, epsilon); + EXPECT_NEAR(random_point.y, 10.3450319, epsilon); // out of range of total length const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); @@ -142,17 +141,17 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) { // yaw // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.368174, epsilon); // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.278594, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.808580, 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); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.368174, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.278594, epsilon); // out of range of index EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); @@ -167,7 +166,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.2441671, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.271073, epsilon); // out of range of total length EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon); From dca216c190b572d31a0b08c2b9633a80489c9515 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Fri, 20 Sep 2024 13:23:26 +0300 Subject: [PATCH 7/7] rename interpolation_utils to autoware::interpolation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .../autoware_interpolation/src/spline_interpolation.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/autoware_interpolation/src/spline_interpolation.cpp b/common/autoware_interpolation/src/spline_interpolation.cpp index 991421919b818..78f3778ae8e21 100644 --- a/common/autoware_interpolation/src/spline_interpolation.cpp +++ b/common/autoware_interpolation/src/spline_interpolation.cpp @@ -149,7 +149,7 @@ void SplineInterpolation::calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values) { // throw exceptions for invalid arguments - interpolation_utils::validateKeysAndValues(base_keys, base_values); + autoware::interpolation::validateKeysAndValues(base_keys, base_values); const Eigen::VectorXd x = Eigen::Map( base_keys.data(), static_cast(base_keys.size())); const Eigen::VectorXd y = Eigen::Map( @@ -197,7 +197,7 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( 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 validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); std::vector interpolated_values; interpolated_values.reserve(query_keys.size()); @@ -215,7 +215,7 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( 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 validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); std::vector interpolated_diff_values; interpolated_diff_values.reserve(query_keys.size()); @@ -232,7 +232,7 @@ 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 validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); std::vector interpolated_quad_diff_values; interpolated_quad_diff_values.reserve(query_keys.size());